322 lines
6.6 KiB
Plaintext
322 lines
6.6 KiB
Plaintext
; robot model meta-information
|
|
model_information {
|
|
manipulatorModelType 0 // 0: Default-arm, 1: Wheel-based manipulator, 2: Floating-arm manipulator, 3: Fully actuated floating-arm manipulator
|
|
|
|
; motion joints in the URDF to consider fixed
|
|
removeJoints {
|
|
[0] "gripper"
|
|
[1] "gripper_bar"
|
|
[2] "ee_bar"
|
|
[3] "left_finger"
|
|
[4] "right_finger"
|
|
[5] "ee_gripper"
|
|
[6] "root_joint"
|
|
|
|
}
|
|
|
|
; base frame of the robot (from URDF)
|
|
baseFrame "root"
|
|
; end-effector frame of the robot (from URDF)
|
|
eeFrame "wx250/gripper_prop_link"
|
|
}
|
|
|
|
model_settings
|
|
{
|
|
usePreComputation true
|
|
recompileLibraries true
|
|
}
|
|
|
|
; DDP settings
|
|
ddp
|
|
{
|
|
algorithm SLQ
|
|
|
|
nThreads 3
|
|
threadPriority 50
|
|
|
|
maxNumIterations 1
|
|
minRelCost 0.1
|
|
constraintTolerance 1e-3
|
|
|
|
displayInfo false
|
|
displayShortSummary false
|
|
checkNumericalStability false
|
|
debugPrintRollout false
|
|
debugCaching false
|
|
|
|
AbsTolODE 1e-5
|
|
RelTolODE 1e-3
|
|
maxNumStepsPerSecond 100000
|
|
timeStep 1e-3
|
|
backwardPassIntegratorType ODE45
|
|
|
|
constraintPenaltyInitialValue 20.0
|
|
constraintPenaltyIncreaseRate 2.0
|
|
|
|
preComputeRiccatiTerms true
|
|
|
|
useFeedbackPolicy false
|
|
|
|
strategy LINE_SEARCH
|
|
lineSearch
|
|
{
|
|
minStepLength 1e-2
|
|
maxStepLength 1.0
|
|
hessianCorrectionStrategy DIAGONAL_SHIFT
|
|
hessianCorrectionMultiple 1e-3
|
|
}
|
|
}
|
|
|
|
; Rollout settings
|
|
rollout
|
|
{
|
|
AbsTolODE 1e-5
|
|
RelTolODE 1e-3
|
|
timeStep 1e-2
|
|
integratorType ODE45
|
|
maxNumStepsPerSecond 100000
|
|
checkNumericalStability false
|
|
}
|
|
|
|
; MPC settings
|
|
mpc
|
|
{
|
|
timeHorizon 1.0 ; [s]
|
|
solutionTimeWindow 0.2 ; [s]
|
|
coldStart false
|
|
|
|
debugPrint false
|
|
|
|
mpcDesiredFrequency 100 ; [Hz]
|
|
mrtDesiredFrequency 400 ; [Hz]
|
|
}
|
|
|
|
; initial state
|
|
initialState
|
|
{
|
|
; initial state for the different types of arm base DOFs
|
|
base
|
|
{
|
|
defaultManipulator
|
|
{
|
|
}
|
|
|
|
floatingArmManipulator
|
|
{
|
|
(0,0) 0.0 ; position x
|
|
(1,0) 0.0 ; position y
|
|
(2,0) 0.0 ; position z
|
|
(3,0) 0.0 ; euler angle z
|
|
(4,0) 0.0 ; euler angle y
|
|
(5,0) 0.0 ; euler angle x
|
|
}
|
|
|
|
fullyActuatedFloatingArmManipulator
|
|
{
|
|
(0,0) 0.0 ; position x
|
|
(1,0) 0.0 ; position y
|
|
(2,0) 0.0 ; position z
|
|
(3,0) 0.0 ; euler angle z
|
|
(4,0) 0.0 ; euler angle y
|
|
(5,0) 0.0 ; euler angle x
|
|
}
|
|
|
|
wheelBasedMobileManipulator
|
|
{
|
|
(0,0) 0.0 ; position x
|
|
(1,0) 0.0 ; position y
|
|
(2,0) 0.0 ; heading
|
|
}
|
|
}
|
|
|
|
; initial state for the arm DOFs
|
|
arm
|
|
{
|
|
(0,0) -0.56 ; arm_1
|
|
(1,0) 1.65 ; arm_2
|
|
(2,0) -1.71 ; arm_3
|
|
(3,0) -1.07 ; arm_4
|
|
(4,0) 0.09 ; arm_5
|
|
(5,0) 0.19 ; arm_6
|
|
}
|
|
}
|
|
|
|
inputCost
|
|
{
|
|
; control weight matrixarmarm
|
|
|
|
R
|
|
{
|
|
; input costs for the different types of arm base DOFs
|
|
base
|
|
{
|
|
defaultManipulator
|
|
{
|
|
}
|
|
|
|
floatingArmManipulator
|
|
{
|
|
}
|
|
|
|
fullyActuatedFloatingArmManipulator
|
|
{
|
|
scaling 1e-2
|
|
|
|
(0,0) 5.0 ; position x
|
|
(1,1) 5.0 ; position y
|
|
(2,2) 5.0 ; position z
|
|
(3,3) 5.0 ; euler angle z
|
|
(4,4) 5.0 ; euler angle y
|
|
(5,5) 5.0 ; euler angle x
|
|
}
|
|
|
|
wheelBasedMobileManipulator
|
|
{
|
|
scaling 1e-2
|
|
|
|
(0,0) 5.0 ; forward velocity
|
|
(1,1) 5.0 ; turning velocity
|
|
}
|
|
}
|
|
|
|
; input costs for the arm DOFs
|
|
arm
|
|
{
|
|
scaling 1e-2
|
|
|
|
(0,0) 1.0 ; arm_1 velocity
|
|
(1,1) 1.0 ; arm_2 velocity
|
|
(2,2) 1.0 ; arm_3 velocity
|
|
(3,3) 1.0 ; arm_4 velocity
|
|
(4,4) 1.0 ; arm_5 velocity
|
|
(5,5) 1.0 ; arm_6 velocity
|
|
}
|
|
}
|
|
}
|
|
endEffector
|
|
{
|
|
; end effector quadratic penalty scaling
|
|
muPosition 10.0
|
|
muOrientation 5.0
|
|
}
|
|
|
|
finalEndEffector
|
|
{
|
|
muPosition 10.0
|
|
muOrientation 5.0
|
|
}
|
|
|
|
selfCollision
|
|
{
|
|
; activate self-collision constraint
|
|
activate false
|
|
|
|
; TODO: Replace the collision meshes of the arm with primitive shapes.
|
|
}
|
|
|
|
; Only applied for arm joints: limits parsed from URDF
|
|
jointPositionLimits
|
|
{
|
|
; activate constraint
|
|
activate true
|
|
|
|
; relaxed log barrier mu
|
|
mu 0.01
|
|
|
|
; relaxed log barrier delta
|
|
delta 1e-3
|
|
}
|
|
|
|
jointVelocityLimits
|
|
{
|
|
; relaxed log barrier mu
|
|
mu 0.01
|
|
|
|
; relaxed log barrier delta
|
|
delta 1e-3
|
|
|
|
lowerBound
|
|
{
|
|
; velocity limits for the different types of arm base DOFs
|
|
base
|
|
{
|
|
defaultManipulator
|
|
{
|
|
}
|
|
|
|
floatingArmManipulator
|
|
{
|
|
}
|
|
|
|
fullyActuatedFloatingArmManipulator
|
|
{
|
|
(0,0) -0.1 ; linear velocity x
|
|
(1,0) -0.1 ; linear velocity y
|
|
(2,0) -0.1 ; linear velocity z
|
|
(3,0) -0.3 ; euler angle velocity z
|
|
(4,0) -0.3 ; euler angle velocity y
|
|
(5,0) -0.3 ; euler angle velocity x
|
|
}
|
|
|
|
wheelBasedMobileManipulator
|
|
{
|
|
(0,0) -0.1 ; forward velocity
|
|
(1,0) -0.3 ; turning velocity
|
|
}
|
|
}
|
|
|
|
; velocity limits for the arm DOFs
|
|
arm
|
|
{
|
|
(0,0) -15.0 ; arm_1 [rad/s]
|
|
(1,0) -15.0 ; arm_2 [rad/s]
|
|
(2,0) -15.0 ; arm_3 [rad/s]
|
|
(3,0) -15.0 ; arm_4 [rad/s]
|
|
(4,0) -15.0 ; arm_5 [rad/s]
|
|
(5,0) -15.0 ; arm_6 [rad/s]
|
|
}
|
|
}
|
|
|
|
upperBound
|
|
{
|
|
; velocity limits for the different types of arm base DOFs
|
|
base
|
|
{
|
|
defaultManipulator
|
|
{
|
|
}
|
|
|
|
floatingArmManipulator
|
|
{
|
|
}
|
|
|
|
fullyActuatedFloatingArmManipulator
|
|
{
|
|
(0,0) 0.1 ; linear velocity x
|
|
(1,0) 0.1 ; linear velocity y
|
|
(2,0) 0.1 ; linear velocity z
|
|
(3,0) 0.3 ; euler angle velocity z
|
|
(4,0) 0.3 ; euler angle velocity y
|
|
(5,0) 0.3 ; euler angle velocity x
|
|
}
|
|
|
|
wheelBasedMobileManipulator
|
|
{
|
|
(0,0) 0.1 ; forward velocity
|
|
(1,0) 0.3 ; turning velocity
|
|
}
|
|
}
|
|
|
|
; velocity limits for the arm DOFs
|
|
arm
|
|
{
|
|
(0,0) 15.0 ; arm_1 [rad/s]
|
|
(1,0) 15.0 ; arm_2 [rad/s]
|
|
(2,0) 15.0 ; arm_3 [rad/s]
|
|
(3,0) 15.0 ; arm_4 [rad/s]
|
|
(4,0) 15.0 ; arm_5 [rad/s]
|
|
(5,0) 15.0 ; arm_6 [rad/s]
|
|
}
|
|
}
|
|
}
|