Calulate zmp by using gazebo force torque sensor plugin

asked 2016-03-22 00:43:35 -0500

happylyang gravatar image

Hello, I want to calculate the zmp position of a legged robot which I created recently.So I add the gazebo force/torque sensor plugin to the ankle joint of the urdf as below:
<gazebo> <plugin name="ft_sensor_left" filename="libgazebo_ros_ft_sensor.so"> <updateRate>100</updateRate> <topicName>ft_sensor_left</topicName> <jointName>left_foot_joint_yaw</jointName> </plugin> </gazebo> <gazebo> <plugin name="ft_sensor_right" filename="libgazebo_ros_ft_sensor.so"> <updateRate>100</updateRate> <topicName>ft_sensor_right</topicName> <jointName>right_foot_joint_yaw</jointName> </plugin> </gazebo>
right_foot_joint_yaw/left_foot_joint_yaw is the last joint in the chain of a leg, and its child link is the foot.
I then use the below formula to calculate the zmp position in single foot support phase:
px = (−τy − fxh)/fz
py = (τx − fy
h)/fz
h is the height of each ankle to the ground
But the result I get is very noisy and wrong.It goes out of the support polygon even the robot is stable.I'm not sure if I add the sensor to the right place.Anybody know how to deal with this?

edit retag flag offensive close merge delete

Comments

Hi, happylyang, I have the same problem now. The ZMP position calculated according to the given formula is not as I expected. Have you figured out how to solve the problem? Thanks

percyliu_ gravatar imagepercyliu_ ( 2018-01-22 20:43:51 -0500 )edit