Incorrect Velocity Framing

[jack33001]

I'm using Pinocchio (with ROS2) for kinematics on my quadrupedal robot, and I think I must be misunderstanding something. Specifically, when I get the velocity of the feet (regardless of frame alignment) using getFrameVelocity(), I get inaccurate values. For example, if the feet are in constant contact with the ground, and the body of the robot is moving directly downwards at .8 m/s, the Pinocchio foot frame velocity magnitudes will be about ~.75 m/s. Using oMf.translation(), however, gets me the expected distance between the foot and the body.

With different body velocities, the discrepancy will be different (both in magnitude and direction), though I have not been able to see much of a pattern. This poses a problem, because to estimate the speed the robot is walking at, I need to use the foot speeds relative to the body and the body's angular velocity with a no-slip condition.

The kinematic tree of the robot looks like this for each of the four legs:

Body --[Hip Revolute Joint]--> Thigh --[IKnee Revolute Joint]--> Shin --[Fixed Joint]--> Foot

and the whole robot is configured as a floating-base system. The floating base orientation and angular velocity, along with all leg joint positions and velocities, are loaded into the model before calling updateFramePlacement(); the floating base's Cartesian position and velocity are never put into the model.

If I understand everything correctly, this means that calling getFrameVelocity() with the LOCAL_WORLD_ALIGNED axes should give the relative velocity of the foot to the body, with axes aligned to the world's, but clearly it is not. Do I misunderstand something about how Pinocchio works?

Here is the code I'm using if you'd like to reference, there's a lot going on so I'm not sure how useful it may be. The functions are called in this order:

read_state_interfaces() --> update_model() --> estimate_orientation() --> pin_kinematics() --> detect_contact() --> estimate_base_position() --> foot_positions() --> update_odometry()

[jcarpent]

If I understand everything correctly, this means that calling getFrameVelocity() with the LOCAL_WORLD_ALIGNED axes should give the relative velocity of the foot to the body, with axes aligned to the world's, but clearly it is not. Do I misunderstand something about how Pinocchio works?

getFrameVelocity with LOCAL_WORLD_ALIGNED will give the velocity of the feet wrt the WORLD frame.
If you want to get the relative velocity, you need to do some algebraic operations.