Humanoid_msgs Definitions

This thread is to discuss a new set of standard message definitions for humanoid robots. The current standard is humanoid_msgs, but it is not widely adopted and is currently limited to bipedal footstep planning/execution.

As a starting point, I will propose modifications to StepTarget.msg to extend functionality for 3D terrain, and include commonly used step parameters:

Target for a single stepping motion of a humanoid’s leg

geometry_msgs/Pose pose # step pose as relative offset to last leg
uint8 swing_leg # which leg to use (left/right, see below)
float32 swing_duration # the duration spent in single support phase
float32 step_duration # the duration spend in double support phase
float32 swing_height # the max height of the swing foot relative to the stance foot

uint8 RIGHT_LEG=0
uint8 LEFT_LEG=1

@k-okada @stumpf Could JSK and Vigir footstep planning be made compatible with something like this?

Another footstep message is from IHMC: https://github.com/ihmcrobotics/ihmc_ros_core/blob/develop/ihmc_msgs/msg/FootstepDataRosMessage.msg

It is sent as a list of footsteps using:

Thank you for starting this thread! I think it is great to start with common data structures and interfaces.

Just to clarify: Is this thread focused on message definitions describing footstep planning and execution? Or should it also discuss other message definitions for coordinated multi-arm manipulation actions?

@rmerriam The IHMC definition looks appealing to me. But is there any reason in general to provide the option for sole vs ankle relative coordinates? If I recall correctly, that was only to handle BDI’s interface to the Atlas robot. And why are position/orientation split out into separate fields?

@airballking We could talk multi-arm manipulation messages on this thread as well. I think we should expand to have humanoid_nav_msgs, humanoid_manipulation_msgs, etc.

Can’t help you with they 'why…?". I participated in the NASA Space Robotics Centennial Challenge which used the IHMC API for controlling NASA’s R5 / Valkyrie. Thought it deserved being mentioned.

yet another footstep msg -> https://github.com/jsk-ros-pkg/jsk_common_msgs/blob/master/jsk_footstep_msgs/msg/Footstep.msg :wink:

I’d add that humanoid_msgs shouldn’t be a fork of someone else but be directly hosted in the org. Get the original author to move ownership of it or recreate the package.

The messages of PAL are using humanoids_msgs but the status message is a nice addition:

1 Like

Thanks for initiating this discussion. We have also been working in this area and have established a generic interface for the control of legged robots (independent of the number of legs). Specifically, a footstep as mention above is implemented as:

# Step defined by foothold position and swing profile.

# Leg name ('LF_LEG', 'RH_LEG' etc.).
string name

# Target position of the foot by the end of the motion.
geometry_msgs/PointStamped target

# Step apex swing heights in control frame.
# If 0, default is used.
float64 profile_height

# Average velocity of the foot motion [m/s].
# If 0, default is used.
float64 average_velocity

# Type of the swing trajectory ('triangle', 'square', etc.).
# If empty, default is used.
string profile_type

# If a contact (touchdown) of the foot at the end of the swing is not expected.
# Default is false.
bool ignore_contact

# Target foothold surface normal.
# Leave empty of no contact is expected or not known.
geometry_msgs/Vector3Stamped surface_normal

# If pose adaptation should ignore this leg motion.
# Default is false.
bool ignore_for_pose_adaptation

Two parts of this message format are interesting: profile_type & ignore_contact. One activity in the Space Robotics Challenge was climbing stairs. The IHMC messages didn’t allow, as far as I could tell, the profile shaping or setting a non-contact, i.e. a step always ended in double contact stance.

From a stance with both feet on a step the first foot moved fine, clearing the riser and settling on the tread. There was no nose. It did this because the foot moved straight up and then horizontally onto the tread. The second food would catch the riser since it move at an angle. The profile couldn’t be modified. Also, since a full step was needed the trajectory could not be raise the foot and then slide it onto the tread.

There is a option to do a two position trajectory move in the IHMC message but I didn’t have time to see how that worked or whether it would be sufficient for stair climbing, or descending.

Thanks for the elaborations @rmerriam! For step/leg motion types that are more complicated, arbitrary motions can be executed with EndEffectorTrajectory.msg from free_gait_msgs:

# Definition of a trajectory for the foot.

# Leg name ('LF_LEG', 'RH_LEG' etc.).
string name

# Trajectory for the end effector.
# Trajectory can contain transforms, twists, or accelerations,
# or combinations of these. 
trajectory_msgs/MultiDOFJointTrajectory trajectory

# Target surface normal.
# Leave empty of no contact is expected or not known.
geometry_msgs/Vector3Stamped surface_normal

# If contact of the end effector should be ignored.
# Default is false.
bool ignore_contact

# If pose adaptation should ignore this leg motion.
# Default is false.
bool ignore_for_pose_adaptation

Hello, from another humanoid user group perspective (LAAS, Gepetto)

IMHO I would be in favor of having a message close to what JSK is using because it is going in the direction of multi contact planning.
A multi-contact planner would provide at least a list of contact stances with the duration between each contact stance.

An extension of the limb values to the URDF body identifier would probably work if someone wants to do multicontact with other bodies.

At least one field would be missing removing the contact or going to contact.

If there is no simultaneous contact event that would be enough.

With respect to the issue raised by the end effector trajectory we have tried:

  • Half step ( y, z position)
  • Polynomials
  • Bezier curves
  • Joint trajectories

Because the connection between the planner and a real-time walking pattern generator often involves
generating a CoM trajectory after the planning the last solution is likely to be modify by the real time controller.
A field indicating the type of trajectories published in a different topic would be maybe more practical ?

Hello,

in our footstep planning framework, we are using nested msg types to model the level of detail required by the different processing steps for 3D planning:



The most difficult part is to cope with the distinct data required for the motion/walking software. Depending on the actual implementation of the walking algorithms, very different data is requested. From my experiences, I’ve seen that BDI wants to know the “lift” height in step mode, IHMC could handle foot contact area information and ROBOTIS THORMANG allows even to set timings for COM/ZMP trajectory calculation. We spend a lot of time to implement a framework which allows for handling this issue.

We have solved the variation of requested data by adding an uint8 array, to encode custom robot specific data. Furthermore, our step messages contain attributes for operator interfaces in order to enable supervised planning (“interactive planning”).

From previous comments, I can already see a lot of different use cases. I guess we should first collect a clear list of requirements by the community and then work out a proper design for the messages, so everyone could work with it.

At this early stage, we should already strongly consider to design the message in way that it is usable for n-legged robots.

@dkotfis: Yes, would like to use common messages in our footstep planning software. But the current humanoid message does not provide the required features.

We may first need to decide where the interface for this step message should sit.

Should this be primarily used as a goal for a controller? If so, many of the planning cost parameters could be dropped. But if it needs to be an interface between different planning layers they need to stay.

Is the controller responsible for computing CoM and foot trajectories, or is there a separate gait generator that does this? If we want to support multi-contact planning, it would be useful to split out the two so that foot (end effector) trajectories could be planned intelligently.