Skip to content

Commit

Permalink
Merge pull request #95 from mmurooka/external-force-stabilizer
Browse files Browse the repository at this point in the history
  • Loading branch information
arntanguy authored Mar 25, 2021
2 parents ccb1b85 + 356d517 commit 41704b5
Show file tree
Hide file tree
Showing 42 changed files with 1,383 additions and 194 deletions.
1 change: 1 addition & 0 deletions doc/Gemfile
Original file line number Diff line number Diff line change
Expand Up @@ -5,3 +5,4 @@ gem 'jekyll-octicons'
gem 'liquefy'
gem 'jekyll_inline_highlight'
gem 'jekyll-toc'
gem 'neatjson'
55 changes: 24 additions & 31 deletions doc/_data/schemas/MetaTask/LIPMStabilizerTask.json
Original file line number Diff line number Diff line change
@@ -1,51 +1,44 @@
{
"title": "mc_tasks::lipm_stabilizer::StabilizerTask",
"description": "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 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).<br />See the <a href=\"{{site.baseurl}}/tutorials/recipes/lipm-stabilizer.html\">LIPM Stabilizer tutorial</a> and the API Documentation {% doxygen mc_tasks::lipm_stabilizer::StabilizerTask %} for further information.",
"type": "object",
"allOf":
[
{
"type": "object",
"title": "mc_tasks::lipm_stabilizer::StabilizerTask",
"description": "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 <a href=\"{{site.baseurl}}/tutorials/recipes/observers.html\">state observation pipeline</a> 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 ({% doxygen 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).<br />See the <a href=\"{{site.baseurl}}/tutorials/recipes/lipm-stabilizer.html\">LIPM Stabilizer tutorial</a> and the API Documentation {% doxygen mc_tasks::lipm_stabilizer::StabilizerTask %} for further information.",
"properties":
{
"type": { "enum": ["lipm_stabilizer"] },
"name": { "type": "string" },

"robot_name" :
"robot": { "$ref": "/../../common/MetaTask_robot.json" }
}
},
{
"$ref": "/../../common/LIPMStabilizerTaskConfiguration.json"
},
{
"allOf":
[
{
"type": "object",
"title": "Robot-specific stabilizer configuration",
"description": "Per-robot configuration, entries specidied here will overwite the global stabilizer configuration entries",
"properties":
{
"friction": { "type": "number", "minimum": 0 },
"tasks":
"*" :
{
"type": "object",
"title": "Tasks configuration",
"description": "Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks",
"properties":
{
"com": {"$ref": "/../../common/com.json"},
"contact": {"$ref": "/../../common/contact.json"},
"pelvis": {"$ref": "/../../common/pelvis.json"},
"torso": {"$ref": "/../../common/torso.json"}
}
},
"fdqp_weights": {"$ref": "/../../common/fdqp_weights.json"},
"vdc": {"$ref": "/../../common/vdc.json"},
"admittance": {"$ref": "/../../common/admittance.json"},
"dcm_tracking": {"$ref": "/../../common/dcm_tracking.json"},
"contacts":
"title": "Robot-specific stabilizer configuration",
"description": "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"
}
}
},
{
"properties":
{
"*" :
{
"type": "array",
"items": { "type": "string" }
"$ref": "/../../common/LIPMStabilizerTaskConfiguration.json"
}
}
}
}
},
{
"$ref": "/../../common/StabilizerTask_common.json"
]
}
],
"required": ["type", "contacts"]
Expand Down
12 changes: 6 additions & 6 deletions doc/_data/schemas/State/Grippers.json
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,13 @@
},
"grippers":
{
"type": "array",
"type": "object",
"title": "Grippers configuration",
"description": "Map of gripper name -> gripper configuration",
"items":
[
{ "type": "string", "description": "Gripper name" },
{ "$ref": "/../../mc_control/generic_gripper.json" }
]
"properties":
{
"*": { "$ref": "/../../mc_control/generic_gripper.json" }
}
},
"keepSafetyConfig": { "type": "boolean", "default": false, "description": "When true, keep this gripper's safety configuration after the state has been destroyed, otherwise restore it to its former value" }
},
Expand Down
7 changes: 5 additions & 2 deletions doc/_data/schemas/State/StabilizerStandingState.json
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,14 @@
[
{
"title": "mc_control::fsm::StabilizerStandingState",
"description": "Simple state managing the LIPM Stabilizer.<br />It provides set-point position target to the CoM, from which a full dynamic trajectory (com, comd, comdd, zmp) is computed and provided as reference to the lipm_stabilizer::StabilizerTask",
"description": "This state creates a {% doxygen mc_tasks::lipm_stabilizer::StabilizerTask %}, and provides set-point position target to the CoM, from which a full dynamic trajectory (com, comd, comdd, zmp) is computed and provided as reference to the lipm_stabilizer::StabilizerTask. For further information, refer to the <a href=\"{{site.baseurl}}/tutorials/recipes/lipm-stabilizer.html#using-the-stabilizer-fsm\">tutorial</a> and the <a href=\"{{site.baseurl}}/tutorials/samples/list-of-samples.html#lipmstabilizer\">LIPMStabilizer sample controller</a>.",
"type": "object",
"properties":
{
"StabilizerConfig": { "$ref": "/../common/StabilizerTask.json" },
"StabilizerConfig":
{
"$ref": "/../MetaTask/LIPMStabilizerTask.json"
},
"stiffness": { "type": "number", "minimum": 0, "default": 5, "description": "Stiffness of CoM target" },
"damping": { "type": "number", "minimum": 0, "description": "Damping of CoM target (default=2*sqrt(stiffness))" },
"above": { "type": "string", "description": "Put CoM over the special frames LeftAnkle, RightAnkle or Center, or any other valid surface on the robot"},
Expand Down
11 changes: 11 additions & 0 deletions doc/_data/schemas/common/LIPMStabilizerTaskConfiguration.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
{
"allOf":
[
{
"$ref": "/../../common/StabilizerConfiguration.json"
},
{
"$ref": "/../../common/StabilizerTask_common.json"
}
]
}
31 changes: 31 additions & 0 deletions doc/_data/schemas/common/StabilizerConfiguration.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
{
"type": "object",
"properties":
{
"leftFootSurface": { "type": "string" },
"rightFootSurface": { "type": "string" },
"torsoBodyName": { "type": "string" },

"friction": { "type": "number", "minimum": 0, "default": 0.7 },
"tasks":
{
"type": "object",
"title": "Tasks configuration",
"description": "Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks",
"properties":
{
"com": {"$ref": "/../../common/com.json"},
"contact": {"$ref": "/../../common/contact.json"},
"pelvis": {"$ref": "/../../common/pelvis.json"},
"torso": {"$ref": "/../../common/torso.json"}
}
},
"safety_thresholds": {"$ref": "/../../common/stabilizer_safety_thresholds.json"},
"fdqp_weights": {"$ref": "/../../common/fdqp_weights.json"},
"vdc": {"$ref": "/../../common/vdc.json"},
"admittance": {"$ref": "/../../common/admittance.json"},
"dcm_tracking": {"$ref": "/../../common/dcm_tracking.json"},
"dcm_bias": {"$ref": "/../../common/dcm_bias.json"},
"external_wrench": {"$ref": "/../../common/external_wrench.json"}
}
}
4 changes: 2 additions & 2 deletions doc/_data/schemas/common/StabilizerTask.json
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,11 @@
[
{
"type": "object",
"title": "Stabilizer Configuration",
"title": "LIPMStabilizerTask Configuration",
"description": "Stabilizer Configuration.<br />Note: the values declared in the robot module take precedence above the default values reported here."
},
{
"$ref": "/../../common/StabilizerTask_common.json"
"$ref": "/../../common/LIPMStabilizerTaskConfiguration.json"
}
]
}
35 changes: 9 additions & 26 deletions doc/_data/schemas/common/StabilizerTask_common.json
Original file line number Diff line number Diff line change
@@ -1,35 +1,18 @@
{
"type": "object",
"properties":
{
"robot": { "$ref": "/../../common/MetaTask_robot.json" },
"leftFootSurface": { "type": "string" },
"rightFootSurface": { "type": "string" },
"torsoBodyName": { "type": "string" },

"friction": { "type": "number", "minimum": 0, "default": 0.7 },
"tasks":
{
"type": "object",
"title": "Tasks configuration",
"description": "Configuration of the tasks managed by the stabilizer: CoM, left and right contact CoP tasks, and torso and pelvis regularization tasks",
"properties":
{
"com": {"$ref": "/../../common/com.json"},
"contact": {"$ref": "/../../common/contact.json"},
"pelvis": {"$ref": "/../../common/pelvis.json"},
"torso": {"$ref": "/../../common/torso.json"}
}
},
"safety_thresholds": {"$ref": "/../../common/stabilizer_safety_thresholds.json"},
"fdqp_weights": {"$ref": "/../../common/fdqp_weights.json"},
"vdc": {"$ref": "/../../common/vdc.json"},
"admittance": {"$ref": "/../../common/admittance.json"},
"dcm_tracking": {"$ref": "/../../common/dcm_tracking.json"},
"dcm_bias": {"$ref": "/../../common/dcm_bias.json"},
"contacts":
{
"type": "array",
"items": { "type": "string" },
"minItems": 0,
"maxItems": 2,
"description": "Support contact surfaces (default: [Left, Right])",
"items":
{
"type": "string",
"enum": ["Left", "Right"]
},
"default": ["Left", "Right"]
},
"Left": { "$ref": "../../common/contactTarget.json" },
Expand Down
2 changes: 1 addition & 1 deletion doc/_data/schemas/common/contactTarget.json
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
{
"type": "object",
"title": "Stabilizer Contact Target",
"description": "Stabilizer Contact Target",
"description": "Stabilizer Contact Target (default: current contact surface pose)",
"properties":
{
"rotation": { "$ref": "/../../Eigen/Vector3d.json" },
Expand Down
2 changes: 1 addition & 1 deletion doc/_data/schemas/common/dcm_bias.json
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
{
"type": "object",
"title": "DCM Bias Estimator",
"title": "mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration",
"description": "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 <a href=\"https://jrl-umi3218.github.io/state-observation/doxygen/HEAD/classes.html#letter_l\">state-observation</a>",
"properties":
{
Expand Down
12 changes: 7 additions & 5 deletions doc/_data/schemas/common/dcm_tracking.json
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,14 @@
"type": "object",
"properties":
{
"prop": { "type": "number", "minimum": 0, "default": 1 },
"integral": { "type": "number", "minimum": 0, "default": 5 },
"deriv": { "type": "number", "minimum": 0, "default": 0 }
"prop": { "type": "number", "minimum": 0, "default": 1, "description": "Proportional gain on DCM error" },
"integral": { "type": "number", "minimum": 0, "default": 5, "description": "Integral gain on DCM error" },
"deriv": { "type": "number", "minimum": 0, "default": 0, "description": "Derivative gain on DCM error" },
"comdError": { "type": "number", "minimum": 0, "default": 1, "description": "Gain on CoM derivative error"},
"zmpd": { "type": "number", "minimum": 0, "default": 0, "description": "Gain on ZMP derivative" }
}
},
"derivator_time_constant": { "type": "number", "minimum": 0, "default": 1 },
"integrator_time_constant": { "type": "number", "minimum": 0, "default": 15 }
"derivator_time_constant": { "type": "number", "minimum": 0, "default": 1, "description": "Time window for exponential moving average filter of the DCM integrator [s]" },
"integrator_time_constant": { "type": "number", "minimum": 0, "default": 15, "description": "Time window for the stationary offset filter of the DCM derivator [s]" }
}
}
70 changes: 70 additions & 0 deletions doc/_data/schemas/common/external_wrench.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
{
"type": "object",
"title": "mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration",
"description": "Parameters for external wrench compensation. See {% doxygen mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration %} for detailed usage.",
"properties":
{
"add_expected_com_offset": {
"type": "boolean",
"default": "false",
"description": "Whether to add the CoM offset expected from the external wrenches."
},
"subtract_measured_value": {
"type": "boolean",
"default": "false",
"description": "Use the measured external wrenches instead of target ones"
},
"modify_com_error": {
"type": "boolean",
"default": "false",
"description": "Modify CoM depending on the error of the external wrenches in target and measurement"
},
"modify_zmp_error": {
"type": "boolean",
"default": "false",
"description": "Modify ZMP depending on the error of the external wrenches in target and measurement"
},
"modify_zmp_error_d": {
"type": "boolean",
"default": "false",
"description": "Modify ZMP velocity depending on the error of the external wrenches in target and measurement"
},

"com_offset_err_com_limit": {
"type": "number",
"minimum": 0,
"default": 0.1,
"description": "Limit of CoM offset error handled by CoM modification [m]"
},
"com_offset_err_zmp_limit": {
"type": "number",
"minimum": 0,
"default": 0.1,
"description": "Limit of ZMP offset error handled by ZMP modification [m]"
},
"ext_wrench_sum_cutoff": {
"type": "number",
"minimum": 0,
"default": 0.05,
"description": "Cutoff period [s] for the low-pass filter of the sum of the measured external wrenches"
},
"com_offset_cutoff": {
"type": "number",
"minimum": 0,
"default": 0.05,
"description": "Cutoff period [s] for the low-pass filter of CoM offset"
},
"com_offset_com_cutoff": {
"type": "number",
"minimum": 0,
"default": 1.0,
"description": "Cutoff period [s] for the low-pass filter of CoM offset to extract CoM modification"
},
"derivator_time_constant": {
"type": "number",
"minimum": 0,
"default": 1.0,
"description": "Time window [s] for the stationary offset filter of the CoM offset derivator"
}
}
}
2 changes: 1 addition & 1 deletion doc/_data/schemas/common/fdqp_weights.json
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
{
"type": "object",
"title": "Force distribution QP parameters",
"title": "mc_rbdyn::lipm_stabilizer::FDQPWeights",
"description": "Force distribution QP parameters",
"properties":
{
Expand Down
26 changes: 14 additions & 12 deletions doc/_data/schemas/common/stabilizer_safety_thresholds.json
Original file line number Diff line number Diff line change
@@ -1,20 +1,22 @@
{
"type": "object",
"title": "Safety thresholds for the stabilizer parameters",
"title": "mc_rbyn::lipm_stabilizer::SafetyThresholds",
"description": "These thresholds are intended to make the stabilizer behaviour safer against invalid parameter choices, and sensor noise. Change these values with caution. See {% doxygen mc_rbdyn::lipm_stabilizer::SafetyThresholds %}",
"properties":
{
"MAX_AVERAGE_DCM_ERROR": { "type": "number", "minimum": 0, "default": 0.05 },
"MAX_COP_ADMITTANCE": { "type": "number", "minimum": 0, "default": 0.1 },
"MAX_DCM_D_GAIN": { "type": "number", "minimum": 0, "default": 2 },
"MAX_DCM_I_GAIN": { "type": "number", "minimum": 0, "default": 100 },
"MAX_DCM_P_GAIN": { "type": "number", "minimum": 0, "default": 20 },
"MAX_DFZ_ADMITTANCE": { "type": "number", "minimum": 0, "default": 5e-4 },
"MAX_DFZ_DAMPING": { "type": "number", "minimum": 0, "default": 10 },
"MAX_FDC_RX_VEL": { "type": "number", "minimum": 0, "default": 0.2 },
"MAX_FDC_RY_VEL": { "type": "number", "minimum": 0, "default": 0.2 },
"MAX_FDC_RZ_VEL": { "type": "number", "minimum": 0, "default": 0.2 },
"MIN_DS_PRESSURE": { "type": "number", "minimum": 0, "default": 15 },
"MAX_AVERAGE_DCM_ERROR": { "type": "number", "minimum": 0, "default": 0.05, "description": "Maximum average (integral) DCM error in [m]" },
"MAX_COP_ADMITTANCE": { "type": "number", "minimum": 0, "default": 0.1, "description": "Maximum CoP admittance for foot damping control" },
"MAX_DCM_D_GAIN": { "type": "number", "minimum": 0, "default": 2, "description": "Maximum DCM derivative gain (no unit)" },
"MAX_DCM_I_GAIN": { "type": "number", "minimum": 0, "default": 100, "description": "Maximum DCM average integral gain in [Hz]" },
"MAX_DCM_P_GAIN": { "type": "number", "minimum": 0, "default": 20, "description": "Maximum DCM proportional gain in [Hz]" },
"MAX_COMD_GAIN": { "type": "number", "minimum": 0, "default": 10, "description": "Maximum CoMd gain in [Hz]" },
"MAX_ZMPD_GAIN": { "type": "number", "minimum": 0, "default": 10, "description": "Maximum ZMPd gain in [Hz]" },
"MAX_DFZ_ADMITTANCE": { "type": "number", "minimum": 0, "default": 5e-4 , "description": "Maximum admittance in [s] / [kg] for foot force difference control" },
"MAX_DFZ_DAMPING": { "type": "number", "minimum": 0, "default": 10 , "description": "Maximum normalized damping in [Hz] for foot force difference control" },
"MAX_FDC_RX_VEL": { "type": "number", "minimum": 0, "default": 0.2, "description": "Maximum x-axis angular velocity in [rad] / [s] for foot damping control" },
"MAX_FDC_RY_VEL": { "type": "number", "minimum": 0, "default": 0.2, "description": "Maximum y-axis angular velocity in [rad] / [s] for foot damping control" },
"MAX_FDC_RZ_VEL": { "type": "number", "minimum": 0, "default": 0.2, "description": "Maximum z-axis angular velocity in [rad] / [s] for foot damping control" },
"MIN_DS_PRESSURE": { "type": "number", "minimum": 0, "default": 15, "description": "Minimum normal contact force in DSP, used to avoid low-pressure targets when close to contact switches" },
"MIN_NET_TOTAL_FORCE_ZMP": { "type": "number", "minimum": 0, "default": 1 }
}
}
Loading

0 comments on commit 41704b5

Please sign in to comment.