Libraries for sampling and validating a humanoid biped with one or more fixed feet for stability constraints in MoveIt! including:
- Fast stability heuristic checking
- Center of mass, static stability, and support polygon computations.
- Smart sampling of whole body positions using random number generation
Provides a wrapper for hrl_kinematics that allows the center of mass stability testing for robots based on KDL.
Developed by Dave Coleman and Shintaro Noda at JSK, University of Tokyo.
This package requires an experimental branch of moveit_core be installed. It might be a while until it is merged into the main distribution. See experimental_fake_base
Overall, making this work on your machine will be fairly complicated until a long list of pending MoveIt! pull requests are accepted. See Dave Coleman for more details.
Performs two course-grain checks and, if they pass, finally does full COM check:
- Checks for a course-grain stability check by seeing if the torso is within a bounding box. This bound box is set in the yaml file and should be found through many iterations. A 0.1m expansion is added to the bounding box so that it over-accepts poses.
- Checks if the other foot is above ground (z >= 0)
- Checks if the COM is within the foot projection using hrl_kinematics
It does not check for self collision or collision with environment, because that is accomplished by other components in MoveIt! automatically.
You can use the validator within a MoveIt! planning scene as a custom stability checker - see the function getStateFeasibilityFn() for use with a planning scene by calling:
planning_scene_->setStateFeasibilityPredicate(humanoid_stability_->getStateFeasibilityFn());
The sampler first samples the fixed leg, performs some quick checks, then samples the rest of the body and performs exact checks.
You can use a custom constraint sampler plugin for MoveIt! by adding a "constraint_samplers" param to your move_group launch file, for example:
<node name="hrp2jsknt_moveit_demos" pkg="hrp2jsknt_moveit_demos" type="hrp2_demos">
<rosparam command="load" file="$(find moveit_humanoid_stability)/config/hrp2jsknt_stability.yaml"/>
<param name="constraint_samplers" value="moveit_humanoid_stability/HumanoidConstraintSamplerAllocator"/>
</node>
A yaml configuration file is required to load the settings for your robot. See config/hrp2jsknt_stability.yaml
N/A
Rviz markers are available on topic /humanoid_constraint_sample_markers and the robot state is published on /humanoid_constraint_sample_robots
- Only supports standing on left or right foot - needs dual feet support
- Only supports fixed link remaining the same (never changes location or link)
- Improve conversion of joint states from MoveIt! to KDL
- Smartly check which foot/feet to check for stability
- Improve other foot check for above ground to include all of mesh, not just the origin of the tip link of the leg
- Cleanup code (currently research code)
- Sample for dual legs, which is a closed chain, and probably requires an IK solver