mc_solver::CompoundJointConstraint
[API]
Handle compound joint limits (joints whole limits depend on the position of another joint). By default this adds all the compound joints defined in the robot's module.
type constant  compoundJoint 
robot string  default MainRobot Name of the robot affected by this constraint
{
  "type": "compoundJoint",
  "robot": "jvrc1"
}
---
type: compoundJoint
robot: jvrc1
mc_solver::BoundedSpeedConstr
[API]
type constant  boundedSpeed 
robot string  default MainRobot Name of the robot affected by this constraint
constraints array  List of speed constraints in the constraint
object

body string
bodyPoint  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
dof  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
speed  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
lowerSpeed  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
upperSpeed  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
{
  "type": "boundedSpeed",
  "constraints": [
  {
    "body": "body0",
    "speed": [0, 0, 0.1]
  },
  {
    "body": "body1",
    "lowerSpeed": [0, 0, 0.05],
    "upperSpeed": [0, 0, 0.1]
  }
  ]
}
type: boundedSpeed
robotIndex: 0
constraints:
  - body: body0
    speed: [0, 0, 0.1]
  - body: body1
    lowerSpeed: [0, 0, 0.05]
    upperSpeed: [0, 0, 0.1]
mc_solver::CoMIncPlaneConstr
[API]
type constant  CoMIncPlane 
robot string  default MainRobot Name of the robot affected by this constraint
{
  "type": "CoMIncPlane"
}
type: CoMIncPlane
robotIndex: 0
mc_solver::ContactConstraint
[API]
type constant  contact 
contactType enum  string  acceleration  velocity  position  Defaults to velocity if absent
dynamics boolean Defaults to true
{
  "type": "contact",
  "contactType": "position",
  "dynamics": true
}
type: contact
contactType: position
dynamics: true
mc_solver::CollisionsConstraint
[API]
type constant  collision 
r1 string  default MainRobot Name of the first robot involved in the collision
r2 string  default MainRobot Name of the second robot involved in the collision
useCommon boolean  default false If true and r1Index == r2Index, add the common self-collisions set
useMinimal boolean  default false If true and r1Index == r2Index, add the minimal self-collisions set
collisions array  List of collisions in the constraint
mc_rbdyn::Collision
[API]
body1 string First convex body used, wildcards can be used
body2 string Second convex body used, wildcards can be used
iDist number  default 0.05 Interaction distance
sDist number  default 0.01 Safety distance
damping number  default 0.0 Damping, 0 enables automatic computation
{
  "type": "collision",
  "r1": "jvrc1",
  "r2": "otherRobot",
  "collisions": [
    {
      "body1": "b1",
      "body2": "b2",
      "iDist": 0.05,
      "sDist": 0.01,
      "damping": 0
    },
    {
      "body1": "b2",
      "body2": "b3",
      "iDist": 0.05,
      "sDist": 0.01,
      "damping": 0
    }
  ]
}
type: collision
r1Index: 0
r2Index: 0
collisions:
  - body1: b1
    body2: b2
    iDist: 0.05
    sDist: 0.01
    damping: 0
  - body1: b2
    body2: b3
    iDist: 0.05
    sDist: 0.01
    damping: 0
mc_solver::KinematicsConstraint
[API]
type constant  kinematics 
robot string  default MainRobot Name of the robot affected by this constraint
damper array  number
velocityPercent number  default 0.5 Ignored if damper is absent.
{
  "type": "kinematics",
  "damper": [0.1, 0.01, 0.5],
  "velocityPercent": 0.5
}
type: kinematics
robotIndex: 0
damper: [0.1, 0.01, 0.5]
velocityPercent: 0.5
mc_solver::DynamicsConstraint
[API]
type constant  dynamics 
robot string  default MainRobot Name of the robot affected by this constraint
damper array  number
velocityPercent number  default 0.5 Ignored if damper is absent.
infTorque boolean  default false
{
  "type": "dynamics",
  "damper": [0.1, 0.01, 0.5],
  "velocityPercent": 0.5,
  "infTorque": false
}
type: dynamics
robotIndex: 0
damper: [0.1, 0.01, 0.5]
velocityPercent: 0.5
infTorque: false
mc_tasks::force::ImpedanceTask
[API]
This task is similar to AdmittanceTask and DampingTask. Unlike those, the robot can adapt to an external force while following a target pose.
type constant  impedance 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
wrench  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
dimWeight True when the measured wrench reaches the desired wrench ([torque, force] in controlled surface frame). If some values are NaN, this direction is ignored. Only valid if the controlled surface is attached to a force sensor (throws otherwise).
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
move  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
moveWorld  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
target Target a world frame pose (position and orientation)
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Target a world rotation
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation Target a world position
Eigen::Vector3d

array[3]  number
targetSurface Target as a robot surface
targetSurface

Target as a robot surface
robot string  default MainRobot Name of robot to which this surface is attached
surface string
offset_translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
offset_rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
relative Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
object

Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetPosition  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetRotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
stiffness number ≥ 0  default 5 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 1000 Task's weight
wrench Admittance coefficients (converts wrench error to velocity)
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
gains Impedance gains
mc_tasks::force::ImpedanceGains

Impedance gains configuration for mc_tasks::force::ImpedanceTask
mass Coefficient-wise gain on the wrench error
sva::ImpedanceVecd

angular  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
linear  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
damper Coefficient-wise gain on the wrench error
sva::ImpedanceVecd

angular  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
linear  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
spring Coefficient-wise gain on the wrench error
sva::ImpedanceVecd

angular  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
linear  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
wrench Coefficient-wise gain on the wrench error
sva::ImpedanceVecd

angular  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
linear  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
{
  "type": "impedance",
  "surface": "RightGripper",
  "target":
  {
    // Rotation expressed as RPY (in radian)
    // May also be expressed as quaternion or rotation matrix
    "rotation":    [0.0, 0.0, 0.0],
    "translation": [0.5, 0.5, 1.0]
  },
  "wrench":
  {
    "couple": [0.0, 0.0, 0.0],
    "force": [0.0, 0.0, 20.0]
  },
  "wrenchGain":
  {
    "couple": [0.0, 0.0, 0.0],
    "force": [0.0, 0.0, 0.0005]
  }
}
type: impedance
surface: RightGripper
targetSurface:
  robot: ground
  surface: AllGround
  offset_translation: [0.5, 0.5, 1.2]
  offset_rotation: [3.14, 0.0, 0.0]
wrench:
  couple: [0.0, 0.0, 0.0]
  force: [0.0, 0.0, 20.0]
wrenchGain:
  couple: [0.0, 0.0, 0.0]
  force: [0.0, 0.0, 0.001]
mc_tasks::PostureTask
[API]
type constant  posture 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Ignored by PostureTask
Eigen::VectorXd

array  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
stiffness number ≥ 0  default 1 Tasks's stiffness
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 10 Task's weight
posture array  array  number Posture target
jointGains array  Gains for specific joints
tasks::qp::JointGains

jointName string
stiffness number
damping number
target array  array  [ string , array  [ number ] ] Map of joint names -> vector of joint values
{
  "type": "posture",
  "stiffness": 1.0,
  "weight": 1.0,
  "posture": [
              [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7],
              [0],
              [],
              [1.5]
             ]
}
type: posture
stiffness: 1.0
weight: 1.0
posture:
  - [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7]
  - [0]
  - []
  - [1.5]
mc_tasks::EndEffectorTask
[API]
type constant  body6d 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight True when the measured wrench reaches the desired wrench ([torque, force] in controlled surface frame). If some values are NaN, this direction is ignored. Only valid if the controlled surface is attached to a force sensor (throws otherwise).
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
bodyPoint  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
orientation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
position  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
relative Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
Relative target specification

Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
orientation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
position  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
orientationStiffness number ≥ 0  default 2
orientationWeight number ≥ 0  default 1000
positionStiffness number ≥ 0  default 2
positionWeight number ≥ 0  default 1000
stiffness number ≥ 0 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0 Task's weight
{
  "type": "body6d",
  "body": "r_wrist",
  // Rotation expressed as RPY (in radian)
  // May also be expressed as quaternion or rotation matrix
  "orientation": [0.0, 0.0, 0.0],
  "position": [0.0, 0.0, 0.0]
}
type: body6d
body: r_wrist
# Rotation expressed as RPY (in radian)
# May also be expressed as quaternion or rotation matrix
orientation: [0.0, 0.0, 0.0]
position: [0.0, 0.0, 0.0]
mc_tasks::RemoveContactTask
[API]
type constant  removeContact 
name string Name of the task
contact Contact to establish or remove. The tasks controls the first robot "r1".
mc_rbdyn::Contact
[API]
Requires an mc_rbdyn::Robots instance to be loaded
r1Surface string
r2Surface string
r1 string  default MainRobot Name of first robot involved in the contact
r2 string  default First extra robot Name of second robot involved in the contact
isFixed boolean
X_r2s_r1s  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
X_b_s  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
friction number  default 0.7
ambiguityId integer  default -1
speed number ≥ 0  default 0.01 Speed at which the contact surface "r1Surface" moves along the normal "T_0_s".
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
stiffness number ≥ 0  default 2 Task's stiffness
weight number ≥ 0  default 1000 Task's weight
T_0_s  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
{
  "type": "removeContact",
  "contact":
  {
    "r1Surface": "LeftFoot",
    "r2Surface": "AllGround",
    "r1": "jvrc1",
    "r2": "ground",
    "isFixed": false
  },
  "speed": 0.05,
  "stiffness": 1.0,
  "weight": 100.0
}
type: removeContact
contact:
  r1Surface: LeftFoot
  r2Surface: AllGround
  r1: jvrc1
  r2: ground
  isFixed: false
speed: 0.05
stiffness: 1.0
weight: 100.0
mc_tasks::LookAtSurfaceTask
[API]
Aligns a unit vector expressed in body coordinates with a target unit vector from the body to a surface.
Example: Align a camera's viewing direction with a surface on a desired object to look at it.
type constant  lookAtSurface 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Unit vector in body frame representing the direction that will be aligned with the target vector (unit vector from body to surface)
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
bodyVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
surfaceRobot string  default MainRobot Name of the robot to which the desired surface is attached
surface string Surface to use as target. The target is a unit vector from the controlled body towards this surface.
offset  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
stiffness number ≥ 0  default 0.5 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 200 Task's weight
{
  "type": "lookAtSurface",
  "body": "dcamera",
  "bodyVector": [1.0, 0.0, 0.0],
  "surfaceRobot": "ground",
  "surface": "AllGround"
}
type: lookAtSurface
body: dcamera
bodyVector: [1.0, 0.0, 0.0]
surfaceRobot: ground
surface: AllGround
mc_tasks::VectorOrientationTask
[API]
type constant  vectorOrientation 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Unit vector in body frame representing the direction that will be aligned with the target vector (unit vector from body to surface)
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
bodyVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight
{
  "type": "vectorOrientation",
  "body": "dcamera",
  "bodyVector": [0.0, 0.0, 0.0],
  "targetVector": [0.0, 0.0, 0.0]
}
type: vectorOrientation
body: dcamera
bodyVector: [0.0, 0.0, 0.0]
targetVector: [0.0, 0.0, 0.0]
mc_tasks::OrientationTask
[API]
type constant  orientation 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Unit vector in body frame representing the direction that will be aligned with the target vector (unit vector from body to surface)
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
orientation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
relative The target is specified relatively to the middle point between two surfaces s1 and s2
Relative target specification

The target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
orientation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight
{
  "type": "orientation",
  "body": "r_wrist",
  // Rotation expressed as RPY (in radian)
  // May also be expressed as quaternion or rotation matrix
  "orientation":  [0.0, 0.0, 0.0]
}
type: orientation
body: r_wrist
# Rotation expressed as RPY (in radian)
# May also be expressed as quaternion or rotation matrix
orientation: [0.0, 0.0, 0.0]
mc_tasks::Momentum

type constant  momentum 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight True when the measured wrench reaches the desired wrench ([torque, force] in controlled surface frame). If some values are NaN, this direction is ignored. Only valid if the controlled surface is attached to a force sensor (throws otherwise).
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
weight number ≥ 0  default 500 Task's weight
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
momentum Admittance coefficients (converts wrench error to velocity)
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
{
  "type": "momentum",
  "robot": "jvrc1",
  "momentum": [0, 0, 0, 0, 0, 10]
}
type: momentum
robot: jvrc1
momentum: [0, 0, 0, 0, 0, 10]
mc_tasks::lipm_stabilizer::StabilizerTask
[API]
The stabilizer task attempts to make the real system track as best as possible the desired state of the CoM (position, velocity, acceleration) and ZMP based on the LIPM model. The reference desired dynamic state must be valid, and continuous (i.e obtained from a walking MPC). A suitable state observation pipeline is required to observe the CoM position and velocity and ZMP of the real system (e.g [Encoder, KinematicInertialObserver]). Default gains are defined in the robot module (mc_rbdyn::RobotModule) and may be overriden from configuration (the default gains reported in this schemas are those of mc_rtc in case none is provided by the RobotModule).
See the LIPM Stabilizer tutorial and the API Documentation mc_tasks::lipm_stabilizer::StabilizerTask for further information.
type constant  lipm_stabilizer 
name string
robot string  default MainRobot Name of the robot to which this task applies
leftFootSurface string
rightFootSurface string
torsoBodyName string
friction number ≥ 0  default 0.7
tasks Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks
Tasks configuration

Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks
com CoM task parameters. The CoM task should have a higher importance than all other active tasks. Too low gains will degrade the stabilizer's tracking performance
CoM task parameters

CoM task parameters. The CoM task should have a higher importance than all other active tasks. Too low gains will degrade the stabilizer's tracking performance
stiffness array  [ number  default 1000 , number  default 1000 , number  default 100 ]
weight number ≥ 0  default 1000 Task's weight.
height number ≥ 0  default 0.84 Desired height of the CoM.
activeJoints array  string
contact Contact tasks parameters (CoP tasks). These tasks have very high weight to simulate a contact constraint in task space.
Contact tasks parameters

Contact tasks parameters (CoP tasks). These tasks have very high weight to simulate a contact constraint in task space.
weight number ≥ 0  default 100000
stiffness Dimensional stiffness for the CoP tasks
sva::MotionVecd

Dimensional stiffness for the CoP tasks
angular array  [ number ≥ 0  default 1 , number ≥ 0  default 1 , number ≥ 0  default 1 ]
linear array  [ number ≥ 0  default 1 , number ≥ 0  default 1 , number ≥ 0  default 1 ]
damping Dimensional damping for the CoP tasks
sva::MotionVecd

Dimensional damping for the CoP tasks
angular array  [ number  default 300 , number  default 300 , number  default 300 ]
linear array  [ number  default 300 , number  default 300 , number  default 300 ]
pelvis Pelvis task parameters (regularization). This task contributes to stabilization by keeping the pelvis aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution.
Pelvis task parameters

Pelvis task parameters (regularization). This task contributes to stabilization by keeping the pelvis aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution.
weight number ≥ 0  default 100
stiffness number ≥ 0  default 10
torso Toso task parameters (regularization). This task contributes to stabilization by keeping the torso aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution. The pitch value allows to lean the torso forward/backwards while respecting the feet's orientation in the other directions
Torso task parameters

Toso task parameters (regularization). This task contributes to stabilization by keeping the torso aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution. The pitch value allows to lean the torso forward/backwards while respecting the feet's orientation in the other directions
weight number ≥ 0  default 100
stiffness number ≥ 0  default 10
pitch number  default 0
safety_thresholds These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See mc_rbdyn::lipm_stabilizer::SafetyThresholds
mc_rbyn::lipm_stabilizer::SafetyThresholds

These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See mc_rbdyn::lipm_stabilizer::SafetyThresholds
MAX_AVERAGE_DCM_ERROR number ≥ 0  default 0.05 Maximum average (integral) DCM error in [m]
MAX_COP_ADMITTANCE number ≥ 0  default 0.1 Maximum CoP admittance for foot damping control
MAX_DCM_D_GAIN number ≥ 0  default 2 Maximum DCM derivative gain (no unit)
MAX_DCM_I_GAIN number ≥ 0  default 100 Maximum DCM average integral gain in [Hz]
MAX_DCM_P_GAIN number ≥ 0  default 20 Maximum DCM proportional gain in [Hz]
MAX_COMD_GAIN number ≥ 0  default 10 Maximum CoMd gain in [Hz]
MAX_ZMPD_GAIN number ≥ 0  default 10 Maximum ZMPd gain in [Hz]
MAX_DFZ_ADMITTANCE number ≥ 0  default 5e-4 Maximum admittance in [s] / [kg] for foot force difference control
MAX_DFZ_DAMPING number ≥ 0  default 10 Maximum normalized damping in [Hz] for foot force difference control
MAX_FDC_RX_VEL number ≥ 0  default 0.2 Maximum x-axis angular velocity in [rad] / [s] for foot damping control
MAX_FDC_RY_VEL number ≥ 0  default 0.2 Maximum y-axis angular velocity in [rad] / [s] for foot damping control
MAX_FDC_RZ_VEL number ≥ 0  default 0.2 Maximum z-axis angular velocity in [rad] / [s] for foot damping control
MIN_DS_PRESSURE number ≥ 0  default 15 Minimum normal contact force in DSP, used to avoid low-pressure targets when close to contact switches
MIN_NET_TOTAL_FORCE_ZMP number ≥ 0  default 1
fdqp_weights Force distribution QP parameters
mc_rbdyn::lipm_stabilizer::FDQPWeights

Force distribution QP parameters
net_wrench number ≥ 0  default 10000
ankle_torque number ≥ 0  default 100
pressure number ≥ 0  default 1
vdc Vertical drift compensation parameters
Vertical drift compensation parameters

Vertical drift compensation parameters
frequency number ≥ 0  default 1 Frequency used in double-support vertical drift compensation
stiffness number ≥ 0  default 1000 Stiffness used in single-support vertical drift compensation
admittance Admittance parameters for the contact tasks. Contacts are expected to be free to move along the pitch and roll axis.
Admittance Parameters

Admittance parameters for the contact tasks. Contacts are expected to be free to move along the pitch and roll axis.
cop array  [ number  default 0 , number  default 0 ]
dfz number ≥ 0  default 1e-4
dfz_damping number ≥ 0  default 0
velFilterGain 0 ≤ number ≤ 1  default 0.8 Gain for the low-pass filter on reference velocity
maxVel maximum velocity of the cop tasks
sva::MotionVecd

maximum velocity of the cop tasks
angular array  [ number ≥ 0  default 0.3 , number ≥ 0  default 0.3 , number ≥ 0  default 0.3 ]
linear array  [ number ≥ 0  default 0.1 , number ≥ 0  default 0.1 , number ≥ 0  default 0.1 ]
dcm_tracking DCM Tracking parameters
DCM Tracking Parameters

DCM Tracking parameters
gains
object

prop number ≥ 0  default 1 Proportional gain on DCM error
integral number ≥ 0  default 5 Integral gain on DCM error
deriv number ≥ 0  default 0 Derivative gain on DCM error
comdError number ≥ 0  default 1 Gain on CoM derivative error
zmpd number ≥ 0  default 0 Gain on ZMP derivative
derivator_time_constant number ≥ 0  default 1 Time window for exponential moving average filter of the DCM integrator [s]
integrator_time_constant number ≥ 0  default 15 Time window for the stationary offset filter of the DCM derivator [s]
dcm_bias A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCM). The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. The dynamics of the DCM depends on the Zero Moment Point. This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. For more details please refer to LipmDcmEstimator class in state-observation
mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration

A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCM). The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. The dynamics of the DCM depends on the Zero Moment Point. This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. For more details please refer to LipmDcmEstimator class in state-observation
dcmMeasureErrorStd number ≥ 0  default 0.01 the standard deviation of the dcm estimation error, NOT including the bias (m)
zmpMeasureErrorStd number ≥ 0  default 0.05 the standard deviaiton of the zmp estimation error (m)
biasDriftPerSecondStd number ≥ 0  default 0.02 the standard deviation of the bias drift (m/s)
biasLimit number ≥ 0  default 0.02 the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit)
withDCMBias boolean  default false whether the bias estimation is activated
withDCMFilter boolean  default false whether the DCM is filtered with this estimator
external_wrench Parameters for external wrench compensation. See mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration for detailed usage.
mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration

Parameters for external wrench compensation. See mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration for detailed usage.
add_expected_com_offset boolean  default false Whether to add the CoM offset expected from the external wrenches.
subtract_measured_value boolean  default false Use the measured external wrenches instead of target ones
modify_com_error boolean  default false Modify CoM depending on the error of the external wrenches in target and measurement
modify_zmp_error boolean  default false Modify ZMP depending on the error of the external wrenches in target and measurement
modify_zmp_error_d boolean  default false Modify ZMP velocity depending on the error of the external wrenches in target and measurement
com_offset_err_com_limit number ≥ 0  default 0.1 Limit of CoM offset error handled by CoM modification [m]
com_offset_err_zmp_limit number ≥ 0  default 0.1 Limit of ZMP offset error handled by ZMP modification [m]
ext_wrench_sum_cutoff number ≥ 0  default 0.05 Cutoff period [s] for the low-pass filter of the sum of the measured external wrenches
com_offset_cutoff number ≥ 0  default 0.05 Cutoff period [s] for the low-pass filter of CoM offset
com_offset_com_cutoff number ≥ 0  default 1.0 Cutoff period [s] for the low-pass filter of CoM offset to extract CoM modification
derivator_time_constant number ≥ 0  default 1.0 Time window [s] for the stationary offset filter of the CoM offset derivator
contacts array  enum  string  Left  Right  Support contact surfaces (default: [Left, Right])
Left Stabilizer Contact Target (default: current contact surface pose)
Stabilizer Contact Target

Stabilizer Contact Target (default: current contact surface pose)
rotation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
height number ≥ 0
Right Stabilizer Contact Target (default: current contact surface pose)
Stabilizer Contact Target

Stabilizer Contact Target (default: current contact surface pose)
rotation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
height number ≥ 0
* You may specify different configuration entries on a per-robot basis. In this case the key is the robot name and the values are the configuration parameters you want to modify for that robot
Robot-specific stabilizer configuration

You may specify different configuration entries on a per-robot basis. In this case the key is the robot name and the values are the configuration parameters you want to modify for that robot
leftFootSurface string
rightFootSurface string
torsoBodyName string
friction number ≥ 0  default 0.7
tasks Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks
Tasks configuration

Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks
com CoM task parameters. The CoM task should have a higher importance than all other active tasks. Too low gains will degrade the stabilizer's tracking performance
CoM task parameters

CoM task parameters. The CoM task should have a higher importance than all other active tasks. Too low gains will degrade the stabilizer's tracking performance
stiffness array  [ number  default 1000 , number  default 1000 , number  default 100 ]
weight number ≥ 0  default 1000 Task's weight.
height number ≥ 0  default 0.84 Desired height of the CoM.
activeJoints array  string
contact Contact tasks parameters (CoP tasks). These tasks have very high weight to simulate a contact constraint in task space.
Contact tasks parameters

Contact tasks parameters (CoP tasks). These tasks have very high weight to simulate a contact constraint in task space.
weight number ≥ 0  default 100000
stiffness Dimensional stiffness for the CoP tasks
sva::MotionVecd

Dimensional stiffness for the CoP tasks
angular array  [ number ≥ 0  default 1 , number ≥ 0  default 1 , number ≥ 0  default 1 ]
linear array  [ number ≥ 0  default 1 , number ≥ 0  default 1 , number ≥ 0  default 1 ]
damping Dimensional damping for the CoP tasks
sva::MotionVecd

Dimensional damping for the CoP tasks
angular array  [ number  default 300 , number  default 300 , number  default 300 ]
linear array  [ number  default 300 , number  default 300 , number  default 300 ]
pelvis Pelvis task parameters (regularization). This task contributes to stabilization by keeping the pelvis aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution.
Pelvis task parameters

Pelvis task parameters (regularization). This task contributes to stabilization by keeping the pelvis aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution.
weight number ≥ 0  default 100
stiffness number ≥ 0  default 10
torso Toso task parameters (regularization). This task contributes to stabilization by keeping the torso aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution. The pitch value allows to lean the torso forward/backwards while respecting the feet's orientation in the other directions
Torso task parameters

Toso task parameters (regularization). This task contributes to stabilization by keeping the torso aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution. The pitch value allows to lean the torso forward/backwards while respecting the feet's orientation in the other directions
weight number ≥ 0  default 100
stiffness number ≥ 0  default 10
pitch number  default 0
safety_thresholds These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See mc_rbdyn::lipm_stabilizer::SafetyThresholds
mc_rbyn::lipm_stabilizer::SafetyThresholds

These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See mc_rbdyn::lipm_stabilizer::SafetyThresholds
MAX_AVERAGE_DCM_ERROR number ≥ 0  default 0.05 Maximum average (integral) DCM error in [m]
MAX_COP_ADMITTANCE number ≥ 0  default 0.1 Maximum CoP admittance for foot damping control
MAX_DCM_D_GAIN number ≥ 0  default 2 Maximum DCM derivative gain (no unit)
MAX_DCM_I_GAIN number ≥ 0  default 100 Maximum DCM average integral gain in [Hz]
MAX_DCM_P_GAIN number ≥ 0  default 20 Maximum DCM proportional gain in [Hz]
MAX_COMD_GAIN number ≥ 0  default 10 Maximum CoMd gain in [Hz]
MAX_ZMPD_GAIN number ≥ 0  default 10 Maximum ZMPd gain in [Hz]
MAX_DFZ_ADMITTANCE number ≥ 0  default 5e-4 Maximum admittance in [s] / [kg] for foot force difference control
MAX_DFZ_DAMPING number ≥ 0  default 10 Maximum normalized damping in [Hz] for foot force difference control
MAX_FDC_RX_VEL number ≥ 0  default 0.2 Maximum x-axis angular velocity in [rad] / [s] for foot damping control
MAX_FDC_RY_VEL number ≥ 0  default 0.2 Maximum y-axis angular velocity in [rad] / [s] for foot damping control
MAX_FDC_RZ_VEL number ≥ 0  default 0.2 Maximum z-axis angular velocity in [rad] / [s] for foot damping control
MIN_DS_PRESSURE number ≥ 0  default 15 Minimum normal contact force in DSP, used to avoid low-pressure targets when close to contact switches
MIN_NET_TOTAL_FORCE_ZMP number ≥ 0  default 1
fdqp_weights Force distribution QP parameters
mc_rbdyn::lipm_stabilizer::FDQPWeights

Force distribution QP parameters
net_wrench number ≥ 0  default 10000
ankle_torque number ≥ 0  default 100
pressure number ≥ 0  default 1
vdc Vertical drift compensation parameters
Vertical drift compensation parameters

Vertical drift compensation parameters
frequency number ≥ 0  default 1 Frequency used in double-support vertical drift compensation
stiffness number ≥ 0  default 1000 Stiffness used in single-support vertical drift compensation
admittance Admittance parameters for the contact tasks. Contacts are expected to be free to move along the pitch and roll axis.
Admittance Parameters

Admittance parameters for the contact tasks. Contacts are expected to be free to move along the pitch and roll axis.
cop array  [ number  default 0 , number  default 0 ]
dfz number ≥ 0  default 1e-4
dfz_damping number ≥ 0  default 0
velFilterGain 0 ≤ number ≤ 1  default 0.8 Gain for the low-pass filter on reference velocity
maxVel maximum velocity of the cop tasks
sva::MotionVecd

maximum velocity of the cop tasks
angular array  [ number ≥ 0  default 0.3 , number ≥ 0  default 0.3 , number ≥ 0  default 0.3 ]
linear array  [ number ≥ 0  default 0.1 , number ≥ 0  default 0.1 , number ≥ 0  default 0.1 ]
dcm_tracking DCM Tracking parameters
DCM Tracking Parameters

DCM Tracking parameters
gains
object

prop number ≥ 0  default 1 Proportional gain on DCM error
integral number ≥ 0  default 5 Integral gain on DCM error
deriv number ≥ 0  default 0 Derivative gain on DCM error
comdError number ≥ 0  default 1 Gain on CoM derivative error
zmpd number ≥ 0  default 0 Gain on ZMP derivative
derivator_time_constant number ≥ 0  default 1 Time window for exponential moving average filter of the DCM integrator [s]
integrator_time_constant number ≥ 0  default 15 Time window for the stationary offset filter of the DCM derivator [s]
dcm_bias A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCM). The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. The dynamics of the DCM depends on the Zero Moment Point. This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. For more details please refer to LipmDcmEstimator class in state-observation
mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration

A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCM). The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. The dynamics of the DCM depends on the Zero Moment Point. This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. For more details please refer to LipmDcmEstimator class in state-observation
dcmMeasureErrorStd number ≥ 0  default 0.01 the standard deviation of the dcm estimation error, NOT including the bias (m)
zmpMeasureErrorStd number ≥ 0  default 0.05 the standard deviaiton of the zmp estimation error (m)
biasDriftPerSecondStd number ≥ 0  default 0.02 the standard deviation of the bias drift (m/s)
biasLimit number ≥ 0  default 0.02 the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit)
withDCMBias boolean  default false whether the bias estimation is activated
withDCMFilter boolean  default false whether the DCM is filtered with this estimator
external_wrench Parameters for external wrench compensation. See mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration for detailed usage.
mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration

Parameters for external wrench compensation. See mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration for detailed usage.
add_expected_com_offset boolean  default false Whether to add the CoM offset expected from the external wrenches.
subtract_measured_value boolean  default false Use the measured external wrenches instead of target ones
modify_com_error boolean  default false Modify CoM depending on the error of the external wrenches in target and measurement
modify_zmp_error boolean  default false Modify ZMP depending on the error of the external wrenches in target and measurement
modify_zmp_error_d boolean  default false Modify ZMP velocity depending on the error of the external wrenches in target and measurement
com_offset_err_com_limit number ≥ 0  default 0.1 Limit of CoM offset error handled by CoM modification [m]
com_offset_err_zmp_limit number ≥ 0  default 0.1 Limit of ZMP offset error handled by ZMP modification [m]
ext_wrench_sum_cutoff number ≥ 0  default 0.05 Cutoff period [s] for the low-pass filter of the sum of the measured external wrenches
com_offset_cutoff number ≥ 0  default 0.05 Cutoff period [s] for the low-pass filter of CoM offset
com_offset_com_cutoff number ≥ 0  default 1.0 Cutoff period [s] for the low-pass filter of CoM offset to extract CoM modification
derivator_time_constant number ≥ 0  default 1.0 Time window [s] for the stationary offset filter of the CoM offset derivator
contacts array  enum  string  Left  Right  Support contact surfaces (default: [Left, Right])
Left Stabilizer Contact Target (default: current contact surface pose)
Stabilizer Contact Target

Stabilizer Contact Target (default: current contact surface pose)
rotation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
height number ≥ 0
Right Stabilizer Contact Target (default: current contact surface pose)
Stabilizer Contact Target

Stabilizer Contact Target (default: current contact surface pose)
rotation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
height number ≥ 0
{
   "type": "lipm_stabilizer",
   "contacts": [ "Left", "Right" ],
   "Left": {
      "rotation": [ 0, 0, 0 ],
      "height": 0
   },
   "Right": {
      "overwriteRotationRPY": {
        "roll": 0,
        "pitch": 0
      },
      "height": 0
   },
   "jvrc1": {
      "dcm_tracking": {
         "gains": {
            "prop": 4,
            "integral": 10,
            "deriv": 0.5
         },
         "derivator_time_constant": 1,
         "integrator_time_constant": 10
      }
   }
}
###
# Stabilizer MetaTask configuration
#
# This task is initialized in the following order:
# - Default configuration is loaded from the
#   robot module (gains, default surface names, joint names, etc).
# - Then general configuration is loaded from this YAML task configuration
# - Then robot-specific configuration is loaded from the `robot_name` group
#   in which you may override any of the previous configuration
#
# In most cases, the user will not need to care about the general stabilizer configuration,
# and is only expected to provide information about how the statbilizer is meant to be used:
# - contacts: list of established contacts (one of [Left, Right])
#   By default the current estimated contact surface pose from the real robot
#   is used as the reference contact frame. You may override this behaviour by providing
#   `Left` or `Right` entries with the desired contact targets.
#
# The JSON example shows such a simple configuration
###
# More complete example with most of the possible configurations.
# See the JSON schema for a full list of available members
#
# Note: than in practice, most users will not need to provide
# all these configuration parameters as the default
# should be good-enough in most use-cases.
#
# Note2: the gains provided here are just meant as an example on how to override
# the default gains with your own, do not expect those to behave well.
# Please refer to the RobotModule for valid default gains.
###
type: lipm_stabilizer
enabled: true

# Global configuration
friction: 0.8

contacts: [Left, Right]
Left:
  rotation: [0,0,0]
  height: 0
Right:
  # Keep the world yaw intact, align roll/pitch with the (flat) ground
  overwriteRotationRPY:
    roll: 0
    pitch: 0
  height: 0

# Weights for the force-distribution QP
fdqp_weights:
  net_wrench: 10000
  ankle_torque: 100
  pressure: 1
tasks:
  com:
    stiffness: [1000, 1000, 100]
    weight: 1000
    height: 0.87

contact:
  damping: 300
  stiffness: 1
  weight: 10000

# Vertical drift frequency
vdc:
  frequency: 1
  stiffness: 1000

# Configuration specific to jvrc1.
jvrc1:
  leftFootSurface: LeftFootCenter
  rightFootSurface: RightFootCenter
  torsoBodyName: WAIST_R_S
  tasks:
    com:
      active_joints:
      [
       Root,
       R_HIP_Y, R_HIP_R, R_HIP_P, R_KNEE, R_ANKLE_P, R_ANKLE_R,
       L_HIP_Y, L_HIP_R, L_HIP_P, L_KNEE, L_ANKLE_P, L_ANKLE_R
      ]
      height: 0.75

  admittance:
    cop: [0.005, 0.005]

  dcm_tracking:
    gains:
      prop: 4
      integral: 10
      deriv: 0.5
    derivator_time_constant: 1
    integrator_time_constant: 10

# Configuration specific to other_robot
other_robot:
  leftFootSurface: YourLeftFootCenter
  rightFootSurface: YourRightFootCenter
  torsoBodyName: torso

  admittance:
    cop: [0.001, 0.001]
mc_tasks::ExactCubicTrajectoryTask
[API]
Follow an exact cubic curve (passing exactly through waypoints with initial/final velocity and acceleration constraints)
All targets and waypoints are defined in world frame, except when using "targetSurface". In that case they are defined in target surface frame.
type constant  exact_cubic_trajectory 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
wrench  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
timeElapsed number ≥ 0 True when the trajectory's "duration" is elapsed
dimWeight True when the measured wrench reaches the given wrench [torque, force] in controlled surface frame.
if some values are NaN, this direction is ignored
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
stiffness number ≥ 0  default 100 Tasks's stiffness. Contrary to most tasks, the stiffness here is expected to be high, as the trajectory task will track small errors. It is not uncommon to use a stiffness of 1000 or more if tracking accuracy is important.
Warning: Be careful to avoid discontinuities in the trajectory (e.g while manipulating the targets from the GUI), as the high tracking stiffness could generate fast motions.
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight. Note that the trajectory task tends to severely constrain the robot motion and may significantly interfere with other tasks when it is given too much importance.
duration number ≥ 0  default 10 Task's duration. Note, you may use the "timeElapsed: true" completion criteria.
paused boolean  default false When true, start the task in a paused state (targets the current surface pose until unpaused)
displaySamples number ≥ 1  default 20 Number of points to sample along the trajectory for visual display
gainsInterpolation Linear interpolation parameters for the tasks's gains expressed as pairs of (time, gain). The initial gains are those specified by dimWeight, stiffness and damping unless otherwise specified.
GainsInterpolation

Linear interpolation parameters for the tasks's gains expressed as pairs of (time, gain). The initial gains are those specified by dimWeight, stiffness and damping unless otherwise specified.
stiffness array  [ number ,  default [1, 1, 1, 1, 1, 1] ] Gains can be either represented as Vector6d (dimensional gains) or double
Eigen::Vector6d

array[6]  number
damping array  [ number ,  default [1, 1, 1, 1, 1, 1] ] Gains can be either represented as Vector6d (dimensional gains) or double
Eigen::Vector6d

array[6]  number
dimWeight array  [ number ,  default [1, 1, 1, 1, 1, 1] ] Gains can be either represented as Vector6d (dimensional gains) or double
Eigen::Vector6d

array[6]  number
targetSurface Targets and waypoints defined in target surface frame
Relative Target

Targets and waypoints defined in target surface frame
robot string  default MainRobot Name of robot on which this surface is defined
surface string Surface on the robot.
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
controlPoints array  [ number ,  default [0,0,0] ] Position waypoints (pairs of time and position)
Eigen::Vector3d

array[3]  number
oriWaypoints array  [ number , ] Orientation waypoints (pairs of time and orientation)
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
controlPoints array  [ number ,  default [0,0,0] ] Position waypoints relative to world (pairs of time and position). Ignored if targetSurface is specified
Eigen::Vector3d

array[3]  number
oriWaypoints array  [ number , ] Orientation waypoints relative to world (pairs of time and orientation) relative to the world. Ignored if targetSurface is specified
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
init_vel  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
init_acc  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
end_vel  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
end_acc  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
{
  "type": "exact_cubic_trajectory",
  "surface": "LeftFoot",
  "stiffness": 10000.0,
  "duration": 15.0,
  "weight": 100,
  "dimWeight": [0.5, 0.5, 0.5, 1.0, 1.0, 1.0],
  "displaySamples": 100,
  "completion": { "timeElapsed": true },
  "targetSurface":
  {
    "robot": "ground",
    "surface": "AllGround",
    "translation": [0.05, -0.05, -0.025],
    "rotation": [0.0, -1.57, 0.0],
    // control points in target surface frame
    "controlPoints":
    [
      [8.0, [-0.25, -0.35, 0.1]],
      [12.0, [-0.08, -0.13, 0.1]]
    ],
    "oriWaypoints":
    [
      [13.0, [0.0, -1.57, 0.0]]
    ],
    "init_vel" : [0.0,0.0,0.0],
    "init_acc" : [0.0,0.0,0.0],
    "end_vel" : [0.0,0.0,0.0],
    "end_acc" : [0.0,0.0,0.0]
  }
}
type: exact_cubic_trajectory
surface: LeftFoot
stiffness: 10000.0
duration: 15.0
weight: 100
dimWeight: [0.5, 0.5, 0.5, 1.0, 1.0, 1.0]
displaySamples: 100
# Task is complete when it has reached its total duration,
# Or if a force greater than 20N has been detected
# along the Z axis (early contact)
completion:
  OR:
    - timeElapsed: true
    - wrench: [.NaN, .NaN, .NaN, .NaN, .NaN, 20]
targetSurface:
  robot: ground
  surface: AllGround
  translation: [0.05, -0.05, -0.025]
  rotation: [0.0, -1.57, 0.0]
  # control points in target surface frame
  controlPoints:
    - [8.0, [-0.25, -0.35, 0.1]]
    - [12.0, [-0.08, -0.13, 0.1]]
  oriWaypoints:
    - [13.0, [0.0, -1.57, 0.0]]
  init_vel : [0.0,0.0,0.0]
  init_acc : [0.0,0.0,0.0]
  end_vel : [0.0,0.0,0.0]
  end_acc : [0.0,0.0,0.0]
mc_tasks::CoMTask
[API]
Task that manages the robot's center of mass (CoM).
Targets are loaded in the following order: current com position -> com -> above -> move_com. For example if you wish to move the CoM 2cm left of the right foot, you can combine "above: RightFoot" and "move_com: [0,0.02,0]". When no target is provided, the current CoM position is targeted.
type constant  com 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight  default [0,0,0] Move the desired CoM position (world frame)
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
stiffness number ≥ 0  default 5 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 100 Task's weight
com  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
move_com  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
{
  "type": "com",
  "com": [0.0, 0.0, 0.71]
}
type: com
com: [0.0, 0.0, 0.71]
mc_tasks::force::CoPTask
[API]
This task control the center of pressure (CoP) of a robot surface attached to a force sensor.
- Force measurements are used in an admittance control law to compute the velocity of the surface motion to bring the measured CoP to its desired target position.
- Only axes where the admittance gains are non null will be affected by force control. Along all other axes, the reference position and velocity will be tracked.
- High stiffness/Low damping will favor position control (targetPose/targetSurface)
- Low stiffness/High damping will favour velocity control.
- Use [low-stiffness/high-damping/non-zero admittance gains] to achieve force control along one of the controlled surface axis.
type constant  cop 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
wrench  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
copError True when the difference between the desired and measured CoP is below this threshold
Eigen::Vector2d

array[2]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
dimWeight True when the measured wrench reaches the desired wrench ([torque, force] in controlled surface frame). If some values are NaN, this direction is ignored
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
admittance Admittance coefficients (converts wrench error to velocity)
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
cop True when the difference between the desired and measured CoP is below this threshold
Eigen::Vector2d

array[2]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetPose  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetSurface Target as a robot surface
targetSurface

Target as a robot surface
robot string  default MainRobot Name of robot to which this surface is attached
surface string
offset_translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
offset_rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
stiffness number ≥ 0  default 5 Task's stiffness (affects position control)
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping (affects velocity/force control).
weight number ≥ 0  default 1000 Task's weight
{
  "type": "cop",
  "surface": "LeftFootCenter",
  "pose":
  {
    // Rotation expressed as RPY (in radian)
    // May also be expressed as quaternion or rotation matrix
    "rotation":    [0.0, 0.0, 0.0],
    "translation": [0.0, 0.0, 0.0]
  },
  "cop": [2.0, 0.0],
  "force": [0.0, 0.0, 0.0],
  "admittance":
  {
    "couple": [0.0, 0.005, 0.0],
    "force": [0.0, 0.0, 0.0005]
  }
}
type: cop
surface: LeftFootCenter
cop: [2.0, 0.0]
targetSurface:
  robot: ground
  surface: AllGround
  offset_translation: [0.0, 0.0, 0.1]
  offset_rotation: [3.14, 0.0, 0.0]
force: [0.0, 0.0, 0.0]
admittance:
  couple: [0.0, 0.005, 0.0]
  force: [0.0, 0.0, 0.0005]
mc_tasks::SurfaceTransformTask
[API]
type constant  surfaceTransform 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
wrench  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
dimWeight Trajectory's reference acceleration
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
move  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
moveWorld  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetSurface Target as a robot surface
targetSurface

Target as a robot surface
robot string  default MainRobot Name of robot to which this surface is attached
surface string
offset_translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
offset_rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
relative Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
object

Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetPosition  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetRotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight
refVel  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
refAccel  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
{
  "type": "surfaceTransform",
  "surface": "LeftFoot",
  "target":
  {
    // Rotation expressed as RPY (in radian)
    // May also be expressed as quaternion or rotation matrix
    "rotation":    [0.0, 0.0, 0.0],
    "translation": [0.0, 0.0, 0.0]
  }
}
type: surfaceTransform
surface: LeftFoot
targetSurface:
  robot: ground
  surface: AllGround
  offset_translation: [0.0, 0.0, 0.1]
  offset_rotation: [3.14, 0.0, 0.0]
mc_tasks::PositionBasedVisServoTask
[API]
type constant  pbvs 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight  default [0,0,0] True when the measured force reaches the desired force (in controlled surface frame). If some values are NaN, this direction is ignored
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
body string  default MainRobot Ignored if surface is specified
X_b_s  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight
{
  "type": "pbvs",
  "surface": "LeftFoot"
}
type: pbvs
surface: LeftFoot
mc_tasks::AddContactTask
[API]
type constant  addContact 
name string Name of the task
contact Contact to establish or remove. The tasks controls the first robot "r1".
mc_rbdyn::Contact
[API]
Requires an mc_rbdyn::Robots instance to be loaded
r1Surface string
r2Surface string
r1 string  default MainRobot Name of first robot involved in the contact
r2 string  default First extra robot Name of second robot involved in the contact
isFixed boolean
X_r2s_r1s  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
X_b_s  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
friction number  default 0.7
ambiguityId integer  default -1
speed number ≥ 0  default 0.01 Speed at which the contact surface "r1Surface" moves along the normal "T_0_s".
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
stiffness number ≥ 0  default 2 Task's stiffness
weight number ≥ 0  default 1000 Task's weight
T_0_s  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
{
  "type": "addContact",
  "contact":
  {
    "r1Surface": "LeftFoot",
    "r2Surface": "AllGround",
    "r1": "jvrc1",
    "r2": "ground",
    "isFixed": false
  },
  "speed": 0.05,
  "stiffness": 1.0,
  "weight": 100.0
}
type: addContact
contact:
  r1Surface: LeftFoot
  r2Surface: AllGround
  r1: jvrc1
  r2: ground
  isFixed: false
speed: 0.05
stiffness: 1.0
weight: 100.0
mc_tasks::LookAtTFTask
[API]
type constant  lookAtTF 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight  default [0,0,0] True when the measured force reaches the desired force (in controlled surface frame). If some values are NaN, this direction is ignored
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
bodyVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
sourceFrame string
targetFrame string
stiffness number ≥ 0  default 0.5 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 200 Task's weight
{
  "type": "lookAtTF",
  "body": "dcamera",
  "bodyVector": [1, 0, 0],
  "sourceFrame": "robot_map",
  "targetFrame": "target_name"
}
type: lookAtTF
body: bodyName
bodyVector: [1, 0, 0]
sourceFrame: robot_map
targetFrame: target_name
mc_tasks::RelativeEndEffectorTask
[API]
type constant  relBody6d 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Trajectory's reference acceleration
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
relBody string Body relative to which this task's target is applied
bodyPoint  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
orientation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
position  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
relative Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
Relative target specification

Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
orientation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
position  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
orientationStiffness number ≥ 0  default 10
orientationWeight number ≥ 0  default 1000
positionStiffness number ≥ 0  default 10
positionWeight number ≥ 0  default 1000
stiffness number ≥ 0 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0 Task's weight
{
  "type": "relBody6d",
  "body": "r_wrist",
  "relBody": "l_wrist",
  // Rotation expressed as RPY (in radian)
  // May also be expressed as quaternion or rotation matrix
  "orientation": [0.0, 0.0, 0.0],
  "position": [0.0, 0.0, 0.0]
}
type: relBody6d
body: r_wrist
relBody: l_wrist
# Rotation expressed as RPY (in radian)
# May also be expressed as quaternion or rotation matrix
orientation: [0.0, 0.0, 0.0]
position: [0.0, 0.0, 0.0]
mc_tasks::force::AdmittanceTask
[API]
This task tracks a desired wrench with an admittance control law (converts wrench-error to a desired surface velocity).
- Force measurements are used in an admittance control law to compute the velocity of the surface to bring the measured wrench towards its desired target.
- Only axes where the admittance gains are non null will be affected by force control. Along all other axes, the reference position and velocity will be tracked.
- High stiffness/Low damping will favor position control (targetPose/targetSurface)
- Low stiffness/High damping will favour velocity control.
- Use [low-stiffness/high-damping/non-zero admittance gains] to achieve force control along the desired surface axes.
See the API documentation mc_tasks::force::AdmittanceTask, and the admittance sample tutorial for futher information.
type constant  admittance 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
wrench  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
dimWeight Trajectory's reference acceleration
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
move  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
moveWorld  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetSurface Target as a robot surface
targetSurface

Target as a robot surface
robot string  default MainRobot Name of robot to which this surface is attached
surface string
offset_translation  default [0,0,0] True when the measured force reaches the desired force (in controlled surface frame). If some values are NaN, this direction is ignored
Eigen::Vector3d

array[3]  number
offset_rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
relative Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
object

Relative target specification, the target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetPosition  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetRotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
overwriteRPY Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
rpy

Overwrites the specified roll/pitch/yaw axes with the provided angle (in world frame) while keeping the other axes unchanged (example: keep world yaw rotation intact, but align roll and ptich with the floor plane)
roll number
pitch number
yaw number
stiffness number ≥ 0  default 5 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 1000 Task's weight
targetPose  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
wrench Admittance coefficients (converts wrench error to velocity)
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
refVelB Clamp computed target velocity
sva::MotionVecd

angular  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
linear  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
maxVel Clamp computed target velocity
sva::MotionVecd

angular  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
linear  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
admittance Admittance coefficients (converts wrench error to velocity)
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
{
  "type": "admittance",
  "surface": "LeftFootCenter",
  "target":
  {
    // Rotation expressed as RPY (in radian)
    // May also be expressed as quaternion or rotation matrix
    "rotation":    [0.0, 0.0, 0.0],
    "translation": [0.0, 0.0, 0.0]
  },
  "wrench":
  {
    "couple": [0.0, 0.0, 0.0],
    "force": [0.0, 0.0, 20.0]
  },
  "admittance":
  {
    "couple": [0.0, 0.0, 0.0],
    "force": [0.0, 0.0, 0.0005]
  }
}
type: admittance
surface: LeftFootCenter
targetSurface:
  robot: ground
  surface: AllGround
  offset_translation: [0.0, 0.0, 0.1]
  offset_rotation: [3.14, 0.0, 0.0]
wrench:
  couple: [0.0, 0.0, 0.0]
  force: [0.0, 0.0, 20.0]
admittance:
  couple: [0.0, 0.0, 0.0]
  force: [0.0, 0.0, 0.0005]
mc_tasks::BSplineTrajectoryTask
[API]
Follow a bezier curve
type constant  bspline_trajectory 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
wrench  default [1, 1, 1, 1, 1, 1] Contact DoF
Eigen::Vector6d

array[6]  number
timeElapsed number ≥ 0 True when the trajectory's "duration" is elapsed
dimWeight Trajectory's reference acceleration
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
surface string Name of the surface controlled by this task
stiffness number ≥ 0  default 100 Tasks's stiffness. Contrary to most tasks, the stiffness here is expected to be high, as the trajectory task will track small errors. It is not uncommon to use a stiffness of 1000 or more if tracking accuracy is important.
Warning: Be careful to avoid discontinuities in the trajectory (e.g while manipulating the targets from the GUI), as the high tracking stiffness could generate fast motions.
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight. Note that the trajectory task tends to severely constrain the robot motion and may significantly interfere with other tasks when it is given too much importance.
duration number ≥ 0  default 10 Task's duration. Note, you may use the "timeElapsed: true" completion criteria.
paused boolean  default false When true, start the task in a paused state (targets the current surface pose until unpaused)
displaySamples number ≥ 1  default 20 Number of points to sample along the trajectory for visual display
gainsInterpolation Linear interpolation parameters for the tasks's gains expressed as pairs of (time, gain). The initial gains are those specified by dimWeight, stiffness and damping unless otherwise specified.
GainsInterpolation

Linear interpolation parameters for the tasks's gains expressed as pairs of (time, gain). The initial gains are those specified by dimWeight, stiffness and damping unless otherwise specified.
stiffness array  [ number ,  default [1, 1, 1, 1, 1, 1] ] Gains can be either represented as Vector6d (dimensional gains) or double
Eigen::Vector6d

array[6]  number
damping array  [ number ,  default [1, 1, 1, 1, 1, 1] ] Gains can be either represented as Vector6d (dimensional gains) or double
Eigen::Vector6d

array[6]  number
dimWeight array  [ number ,  default [1, 1, 1, 1, 1, 1] ] Gains can be either represented as Vector6d (dimensional gains) or double
Eigen::Vector6d

array[6]  number
targetSurface Target and waypoints defined in target surface frame
Relative Target

Target and waypoints defined in target surface frame
robot string  default MainRobot Name of robot on which this surface is defined
surface string Surface on the robot.
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
controlPoints array   default [0,0,0] Position waypoints
Eigen::Vector3d

array[3]  number
oriWaypoints array  [ number , ] Orientation waypoints (pairs of time and orientation)
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
target  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
controlPoints array   default [0,0,0] Control points relative to world. Ignored if targetSurface is specified
Eigen::Vector3d

array[3]  number
oriWaypoints array  [ number , ] Orientation waypoints relative to world (pairs of time and orientation) relative to the world. Ignored if targetSurface is specified
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
{
  "type": "bspline_trajectory",
  "surface": "LeftFoot",
  "stiffness": 10000.0,
  "duration": 15.0,
  "weight": 100,
  "dimWeight": [0.5, 0.5, 0.5, 1.0, 1.0, 1.0],
  "displaySamples": 100,
  "completion": { "timeElapsed": true },
  "targetSurface":
  {
    "robot": "ground",
    "surface": "AllGround",
    "translation": [0.05, -0.05, -0.025],
    "rotation": [0.0, -1.57, 0.0],
    // control points in target surface frame
    "controlPoints":
    [
      [-0.25, -0.35, 0.1],
      [-0.08, -0.13, 0.1]
    ],
    "oriWaypoints":
    [
      [13.0, [0.0, -1.57, 0.0]]
    ]
  }
}
type: bspline_trajectory
surface: LeftFoot
stiffness: 10000.0
duration: 15.0
weight: 100
dimWeight: [0.5, 0.5, 0.5, 1.0, 1.0, 1.0]
displaySamples: 100
# Task is complete when it has reached its total duration,
# Or if a force greater than 20N has been detected
# along the Z axis (early contact)
completion:
  OR:
    - timeElapsed: true
    - wrench: [.NaN, .NaN, .NaN, .NaN, .NaN, 20]
targetSurface:
  robot: ground
  surface: AllGround
  translation: [0.05, -0.05, -0.025]
  rotation: [0.0, -1.57, 0.0]
  # control points in target surface frame
  controlPoints:
  [
    [-0.25, -0.35, 0.1],
    [-0.08, -0.13, 0.1]
  ]
  oriWaypoints:
  [
    [13.0, [0.0, -1.57, 0.0]]
  ]
mc_tasks::force::ComplianceTask
[API]
type constant  compliance 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Trajectory's reference acceleration
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
wrench Admittance coefficients (converts wrench error to velocity)
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
dof Stored in row-major order.
Eigen::Matrix6d

Stored in row-major order.
array[36]  number
forceThresh number
torqueThresh number
forceGain array[2]  number
torqueGain array[2]  number
stiffness number ≥ 0  default 5 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 1000 Task's weight
{
  "type": "compliance",
  "body": "R_WRIST_Y_S",
  "wrench":
  {
    "couple": [0.0, 0.0, 0.0],
    "force": [0.0, 0.0, 0.0]
  }
}
type: compliance
body: L_WRIST_Y_S
wrench:
  couple: [0.0, 0.0, 0.0]
  force: [0.0, 0.0, 0.0]
mc_tasks::LookAtTask
[API]
type constant  lookAt 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight  default [0,0,0] Specify a target vector relative to the current orientation of the robot in the world
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
bodyVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetPos  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
targetVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
relativeVector  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight
{
  "type": "lookAt",
  "body": "dcamera",
  "bodyVector": [1.0, 0.0, 0.0],
  "targetPos": [0.0, 0.0, 0.0]
}
type: lookAt
body: dcamera
bodyVector: [1.0, 0.0, 0.0]
targetPos: [0.0, 0.0, 0.0]
mc_tasks::GazeTask
[API]
type constant  gaze 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight Trajectory's reference acceleration
Eigen::Vector6d

array[6]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
weight number ≥ 0  default 500 Task's weight
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
X_b_gaze  default Identity Transformation between the camera link and the parent body.
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
{
  "type": "gaze",
  "body": "dcamera",
  "X_b_gaze":
  {
    // Rotation expressed as RPY (in radian)
    // May also be expressed as quaternion or rotation matrix
    "rotation":    [0.0, 0.0, 0.0],
    "translation": [0.0, 0.0, 0.0]
  }
}
type: gaze
body: dcamera
X_b_gaze:
  # Rotation expressed as RPY (in radian)
  # May also be expressed as quaternion or rotation matrix
  rotation: [0.0, 0.0, 0.0]
  translation: [0.0, 0.0, 0.0]
mc_tasks::PositionTask
[API]
type constant  position 
name string Name of the task
robot string  default MainRobot Name of the robot to which this task applies
completion Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
Completion Criteria

Builds a conditional function that returns true when all of its conditions are satisfied.
Some states (e.g MetaTasks) may use these criterias to determine when a task is finished.
AND array[2]  [ , ] True when criteria1 and criteria2 are both true
Completion Criteria

Completion criteria on the left-hand side of the AND comparison
Completion Criteria

Completion criteria on the right-hand side of the AND comparison
OR array[2]  [ , ] True when either criteria1 or criteria2 is true
Completion Criteria

Completion criteria on the left-hand side of the OR comparison
Completion Criteria

Completion criteria on the right-hand side of the OR comparison
timeout number ≥ 0 True when the task has been added to the solver for longer than this threshold
eval number ≥ 0 True when the task's error (norm) is below this threshold
speed number ≥ 0 True when the task's velocity (norm) is below this threshold
dimWeight  default [0,0,0] Specify a target vector relative to the current orientation of the robot in the world
Eigen::Vector3d

array[3]  number
activeJoints array  string A list of joints used by this task
If empty the task uses all available joints
unactiveJoints array  string A list of joints not used by this task
Has no effect if activeJoints is specified
body string Name of the body controlled by this task
bodyPoint  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
position  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
relative The target is specified relatively to the middle point between two surfaces s1 and s2
Relative target specification

The target is specified relatively to the middle point between two surfaces s1 and s2
s1 string
s2 string
position  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
stiffness number ≥ 0  default 2 Task's stiffness. May also be an array of the task's dimension (dimensional stiffness).
damping number ≥ 0  default \(2\sqrt{\mathit{stiffness}}\) Task's damping. May also be an array of the task's dimension (dimensional damping).
weight number ≥ 0  default 500 Task's weight
{
  "type": "position",
  "body": "r_wrist",
  "position": [0.0, 0.0, 0.0]
}
type: position
body: r_wrist
position: [0.0, 0.0, 0.0]
mc_control::Gripper
[API]
Gripper target and safety configuration
target array  number Target for all active joints in the gripper
OR
Keys are joint names and values are the corresponding target joint angle
Per-joint target

Keys are joint names and values are the corresponding target joint angle
* number Joint target [rad]
opening 0 ≤ number ≤ 1 Target opening percentage of the gripper
OR
Keys are joint names and values are the corresponding target opening percentage
Per-joint opening

Keys are joint names and values are the corresponding target opening percentage
* 0 ≤ number ≤ 1 Joint target opening percentage
safety Parameters to prevent over-torques on position-controlled grippers
mc_rbdyn::RobotModule::Gripper::Safety
[API]
Parameters to prevent over-torques on position-controlled grippers
overCommandLimitIterN number ≥ 1  default 5 Maximum number of iterations spent above "threshold" before the gripper is release by "release"
actualCommandDiffTrigger number ≥ 0  default 0.14 Encoder-command difference [rad] over which the gripper is considered in overtorque
releaseSafetyOffset number ≥ 0  default 0.034 Angle [rad] by which the gripper should be released when safety is triggered
percentVMax 0 ≤ number ≤ 1  default 0.25 Gripper opening speed as a percentage of the maximum velocity
{
  "opening": 0.3,
  "safety":
  {
    "overCommandLimitIterN": 5,
    "releaseSafetyOffset":  0.034,
    "percentVMax": 0.5
  }
}
opening:
  L_UTHUMB: 0.3
safety:
  overCommandLimitIterN: 5
  releaseSafetyOffset: 0.034
  percentVMax: 0.5
mc_rbdyn::RobotModule::Gripper
[API]
name string
joints array  string
reverse_limits boolean
safety Parameters to prevent over-torques on position-controlled grippers
mc_rbdyn::RobotModule::Gripper::Safety
[API]
Parameters to prevent over-torques on position-controlled grippers
overCommandLimitIterN number ≥ 1  default 5 Maximum number of iterations spent above "threshold" before the gripper is release by "release"
actualCommandDiffTrigger number ≥ 0  default 0.14 Encoder-command difference [rad] over which the gripper is considered in overtorque
releaseSafetyOffset number ≥ 0  default 0.034 Angle [rad] by which the gripper should be released when safety is triggered
percentVMax 0 ≤ number ≤ 1  default 0.25 Gripper opening speed as a percentage of the maximum velocity
mimics array 
mc_rbdyn::Mimic
[API]
name string
joint string
multiplier number  default 1
offset number  default 0
{
  "name": "l_gripper",
  "joints": ["j0", "j1", "j2"],
  "reverse_limits": false
}
name: l_gripper
joints: [j0, j1, j2]
reverse_limits: false
mc_rbdyn::lipm_stabilizer::StabilizerConfiguration
[API]
Configuration of mc_tasks::lipm_stabilizer::StabilizerTask stabilization parameters (DCM tracking, admittance, tasks weights, etc). A default configuration is provided by each robot's mc_rbdyn::RobotModule.
leftFootSurface string
rightFootSurface string
torsoBodyName string
friction number ≥ 0  default 0.7
tasks Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks
Tasks configuration

Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks
com CoM task parameters. The CoM task should have a higher importance than all other active tasks. Too low gains will degrade the stabilizer's tracking performance
CoM task parameters

CoM task parameters. The CoM task should have a higher importance than all other active tasks. Too low gains will degrade the stabilizer's tracking performance
stiffness array  [ number  default 1000 , number  default 1000 , number  default 100 ]
weight number ≥ 0  default 1000 Task's weight.
height number ≥ 0  default 0.84 Desired height of the CoM.
activeJoints array  string
contact Contact tasks parameters (CoP tasks). These tasks have very high weight to simulate a contact constraint in task space.
Contact tasks parameters

Contact tasks parameters (CoP tasks). These tasks have very high weight to simulate a contact constraint in task space.
weight number ≥ 0  default 100000
stiffness Dimensional stiffness for the CoP tasks
sva::MotionVecd

Dimensional stiffness for the CoP tasks
angular array  [ number ≥ 0  default 1 , number ≥ 0  default 1 , number ≥ 0  default 1 ]
linear array  [ number ≥ 0  default 1 , number ≥ 0  default 1 , number ≥ 0  default 1 ]
damping Dimensional damping for the CoP tasks
sva::MotionVecd

Dimensional damping for the CoP tasks
angular array  [ number  default 300 , number  default 300 , number  default 300 ]
linear array  [ number  default 300 , number  default 300 , number  default 300 ]
pelvis Pelvis task parameters (regularization). This task contributes to stabilization by keeping the pelvis aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution.
Pelvis task parameters

Pelvis task parameters (regularization). This task contributes to stabilization by keeping the pelvis aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution.
weight number ≥ 0  default 100
stiffness number ≥ 0  default 10
torso Toso task parameters (regularization). This task contributes to stabilization by keeping the torso aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution. The pitch value allows to lean the torso forward/backwards while respecting the feet's orientation in the other directions
Torso task parameters

Toso task parameters (regularization). This task contributes to stabilization by keeping the torso aligned with the contacts orientation. Without it, the robot has been known to vibrate, lower the gains with caution. The pitch value allows to lean the torso forward/backwards while respecting the feet's orientation in the other directions
weight number ≥ 0  default 100
stiffness number ≥ 0  default 10
pitch number  default 0
safety_thresholds These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See mc_rbdyn::lipm_stabilizer::SafetyThresholds
mc_rbyn::lipm_stabilizer::SafetyThresholds

These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See mc_rbdyn::lipm_stabilizer::SafetyThresholds
MAX_AVERAGE_DCM_ERROR number ≥ 0  default 0.05 Maximum average (integral) DCM error in [m]
MAX_COP_ADMITTANCE number ≥ 0  default 0.1 Maximum CoP admittance for foot damping control
MAX_DCM_D_GAIN number ≥ 0  default 2 Maximum DCM derivative gain (no unit)
MAX_DCM_I_GAIN number ≥ 0  default 100 Maximum DCM average integral gain in [Hz]
MAX_DCM_P_GAIN number ≥ 0  default 20 Maximum DCM proportional gain in [Hz]
MAX_COMD_GAIN number ≥ 0  default 10 Maximum CoMd gain in [Hz]
MAX_ZMPD_GAIN number ≥ 0  default 10 Maximum ZMPd gain in [Hz]
MAX_DFZ_ADMITTANCE number ≥ 0  default 5e-4 Maximum admittance in [s] / [kg] for foot force difference control
MAX_DFZ_DAMPING number ≥ 0  default 10 Maximum normalized damping in [Hz] for foot force difference control
MAX_FDC_RX_VEL number ≥ 0  default 0.2 Maximum x-axis angular velocity in [rad] / [s] for foot damping control
MAX_FDC_RY_VEL number ≥ 0  default 0.2 Maximum y-axis angular velocity in [rad] / [s] for foot damping control
MAX_FDC_RZ_VEL number ≥ 0  default 0.2 Maximum z-axis angular velocity in [rad] / [s] for foot damping control
MIN_DS_PRESSURE number ≥ 0  default 15 Minimum normal contact force in DSP, used to avoid low-pressure targets when close to contact switches
MIN_NET_TOTAL_FORCE_ZMP number ≥ 0  default 1
fdqp_weights Force distribution QP parameters
mc_rbdyn::lipm_stabilizer::FDQPWeights

Force distribution QP parameters
net_wrench number ≥ 0  default 10000
ankle_torque number ≥ 0  default 100
pressure number ≥ 0  default 1
vdc Vertical drift compensation parameters
Vertical drift compensation parameters

Vertical drift compensation parameters
frequency number ≥ 0  default 1 Frequency used in double-support vertical drift compensation
stiffness number ≥ 0  default 1000 Stiffness used in single-support vertical drift compensation
admittance Admittance parameters for the contact tasks. Contacts are expected to be free to move along the pitch and roll axis.
Admittance Parameters

Admittance parameters for the contact tasks. Contacts are expected to be free to move along the pitch and roll axis.
cop array  [ number  default 0 , number  default 0 ]
dfz number ≥ 0  default 1e-4
dfz_damping number ≥ 0  default 0
velFilterGain 0 ≤ number ≤ 1  default 0.8 Gain for the low-pass filter on reference velocity
maxVel maximum velocity of the cop tasks
sva::MotionVecd

maximum velocity of the cop tasks
angular array  [ number ≥ 0  default 0.3 , number ≥ 0  default 0.3 , number ≥ 0  default 0.3 ]
linear array  [ number ≥ 0  default 0.1 , number ≥ 0  default 0.1 , number ≥ 0  default 0.1 ]
dcm_tracking DCM Tracking parameters
DCM Tracking Parameters

DCM Tracking parameters
gains
object

prop number ≥ 0  default 1 Proportional gain on DCM error
integral number ≥ 0  default 5 Integral gain on DCM error
deriv number ≥ 0  default 0 Derivative gain on DCM error
comdError number ≥ 0  default 1 Gain on CoM derivative error
zmpd number ≥ 0  default 0 Gain on ZMP derivative
derivator_time_constant number ≥ 0  default 1 Time window for exponential moving average filter of the DCM integrator [s]
integrator_time_constant number ≥ 0  default 15 Time window for the stationary offset filter of the DCM derivator [s]
dcm_bias A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCM). The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. The dynamics of the DCM depends on the Zero Moment Point. This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. For more details please refer to LipmDcmEstimator class in state-observation
mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration

A humanoid robot can be modeled as an inverted pendulum. The dynamics can be linearized to obtain a dynamics with a convergent and a divergent component of motion (DCM). The DCM can be measured using the CoM and its velocity, but the CoM position can be biased. The dynamics of the DCM depends on the Zero Moment Point. This estimator uses Kalman Filtering to estimate this bias and give a delay-free filtering of the DCM. For more details please refer to LipmDcmEstimator class in state-observation
dcmMeasureErrorStd number ≥ 0  default 0.01 the standard deviation of the dcm estimation error, NOT including the bias (m)
zmpMeasureErrorStd number ≥ 0  default 0.05 the standard deviaiton of the zmp estimation error (m)
biasDriftPerSecondStd number ≥ 0  default 0.02 the standard deviation of the bias drift (m/s)
biasLimit number ≥ 0  default 0.02 the X and Y (expressed in local frame) largest accepted absolute values of the bias (zero means no limit)
withDCMBias boolean  default false whether the bias estimation is activated
withDCMFilter boolean  default false whether the DCM is filtered with this estimator
external_wrench Parameters for external wrench compensation. See mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration for detailed usage.
mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration

Parameters for external wrench compensation. See mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration for detailed usage.
add_expected_com_offset boolean  default false Whether to add the CoM offset expected from the external wrenches.
subtract_measured_value boolean  default false Use the measured external wrenches instead of target ones
modify_com_error boolean  default false Modify CoM depending on the error of the external wrenches in target and measurement
modify_zmp_error boolean  default false Modify ZMP depending on the error of the external wrenches in target and measurement
modify_zmp_error_d boolean  default false Modify ZMP velocity depending on the error of the external wrenches in target and measurement
com_offset_err_com_limit number ≥ 0  default 0.1 Limit of CoM offset error handled by CoM modification [m]
com_offset_err_zmp_limit number ≥ 0  default 0.1 Limit of ZMP offset error handled by ZMP modification [m]
ext_wrench_sum_cutoff number ≥ 0  default 0.05 Cutoff period [s] for the low-pass filter of the sum of the measured external wrenches
com_offset_cutoff number ≥ 0  default 0.05 Cutoff period [s] for the low-pass filter of CoM offset
com_offset_com_cutoff number ≥ 0  default 1.0 Cutoff period [s] for the low-pass filter of CoM offset to extract CoM modification
derivator_time_constant number ≥ 0  default 1.0 Time window [s] for the stationary offset filter of the CoM offset derivator
{
  "leftFootSurface": "LeftFoot",
  "rightFootSurface": "RightFoot",
  "friction": 0.7,
  "torsoBodyName": "CHEST_LINK1",
  "tasks": {
    "com": {
      "stiffness": [
        1000,
        1000,
        100
      ],
      "weight": 1000,
      "active_joints": [
        "Root",
        "RLEG_JOINT0",
        "RLEG_JOINT1",
        "RLEG_JOINT2",
        "RLEG_JOINT3",
        "RLEG_JOINT4",
        "RLEG_JOINT5",
        "LLEG_JOINT0",
        "LLEG_JOINT1",
        "LLEG_JOINT2",
        "LLEG_JOINT3",
        "LLEG_JOINT4",
        "LLEG_JOINT5"
      ],
      "height": 0.87
    },
    "contact": {
      "damping": 300,
      "stiffness": 1,
      "weight": 10000
    }
  },
  "fdqp_weights": {
    "net_wrench": 10000,
    "ankle_torque": 100,
    "pressure": 1
  },
  "admittance": {
    "cop": [
      0.001,
      0.001
    ]
  },
  "dcm_tracking": {
    "gains": {
      "prop": 3,
      "integral": 20,
      "deriv": 0.5
    },
    "derivator_time_constant": 5,
    "integrator_time_constant": 30
  }
}
---
leftFootSurface: LeftFoot
rightFootSurface: RightFoot
friction: 0.7
torsoBodyName: CHEST_LINK1
# Configuration  of the QP tasks used by the stabilizer
tasks:
  com:
    stiffness: [1000, 1000, 100]
    weight: 1000
    active_joints: [Root, RLEG_JOINT0, RLEG_JOINT1, RLEG_JOINT2, RLEG_JOINT3, RLEG_JOINT4, RLEG_JOINT5, LLEG_JOINT0, LLEG_JOINT1, LLEG_JOINT2, LLEG_JOINT3, LLEG_JOINT4, LLEG_JOINT5]
    height: 0.87
  contact:
    damping: 300
    stiffness: 1
    weight: 10000

# Weights for the force-distribution QP
fdqp_weights:
  net_wrench: 10000
  ankle_torque: 100
  pressure: 1

# Admittance coefficients (feet force-control)
admittance:
  cop: [0.001, 0.001]

# Gains on DCM error 
dcm_tracking:
  gains:
    prop: 3
    integral: 20
    deriv: 0.5
  derivator_time_constant: 5
  integrator_time_constant: 30
mc_rbdyn::RobotModule
[API]
path string Path to the location of the robot's data files
name string
urdf_path string Path to the URDF file, relative to "path"
mb If mb is provided, mbc, bounds, visuals and collisionTransforms are required. Otherwise, the URDF is loaded when the object is loaded, using the filteredLinks and fixed properties.
rbd::MultiBody

bodies array 
rbd::Body

name string
inertia
sva::RBInertiad

mass number ≥ 0
momentum  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
inertia Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
joints array 
rbd::Joint

type
rbd::Joint::Type

type enum  string  rev  prism  spherical  planar  cylindrical  free  fixed 
axis  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
forward boolean
name string
isMimic boolean
mimicName string
mimicMultiplier number
mimicOffset number
preds array  integer
succs array  integer
parents array  integer
transforms array   default Identity
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-major order.
Also accepts arrays of size 3 (RPY) and 4 (Quaternion).
array[9]  number
translation  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
mbc Required if mb is provided.
rbd::MultiBodyConfig

q array  array  number
alpha array  array  number
alphaD array  array  number
force array 
sva::ForceVecd

couple  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
force  default [0,0,0] Returns true with output 'OK' when the dcm error is under this threshold.
Eigen::Vector3d

array[3]  number
jointConfig array   default Identity
sva::PTransformd

Either rotation or translation can be ommited if the other is present
rotation Offset in rotation wrt to the target robot's surface
Eigen::Matrix3d

Stored in row-