Mismatch in z position (references) of wheel links, although all in the same horizontal plane

[IoannisDadiotis]

Hi @alaurenzi . This is an issue about the different z components of the first and last published trajectory commands of a swing leg with activated early contact detection.

In the following figure you can see an example of this issue (orange trajectory, difference more than 2.5 cm), although the starting and landing positions are on the same horizontal plane and have the same height. As you can see in the corresponding node, this orange trajectory is published at the /cartesian/${swing_leg}/reference topic.

In the following you can see a snapshot of the robot after a stepping with similar results (all feet are in contact).

Next, you can see the corresponding snapshot on RViz where the feet of the robot do not seem to be on same height.

This is confirmed with rostopic echo /cartesian/${every_leg}/current_reference where the right feet have references of lower height (-0.703 and - 0.74 against -0.636 and -0.673, respectively)

Do you have an idea of where the problem comes from?

P.S for some reason I cannot assign Enrico in my issues.

[alaurenzi]

Hi Ioannis,
Do you have any idea about why this could happen?

Share your thoughts!

[IoannisDadiotis]

Cartesio seems not to consider any terrain collision at all.

For example, if you move any foot among the z direction towards pushing more and more the ground (can be easily tried using the interactive markers), you can see in RViz that the foot is always lowering its height (which is observer also from the /cartesian/${wheel}/current_reference topic), although in gazebo the foot collides with the terrain and this movement will result in loss of contact for some of the other feet. As a result, for Cartesio the moving foot has reduced its z component position while the feet that lost contact seem to remain in the same terrain height. This is in contrast with the gazebo simulation where the moving foot inevitably remains in contact with the ground and the feet that lose contact increase their height.
I don't know if these are feasible but some suggestions would be to:

• introduce the terrain (and any other links with collision in our world) somehow in cartesio so that we capture the interaction between this and the robot.
• restrict the movement of the robot to be higher that a specific height value (terrain height). However, this may not solve the problem for more complex terrains, when e.g. there is an inclined terrain or a terrain with stairs, obstacles etc.
• Modify somehow Cartesio to follow simulation, when launched together. Right now, simulation follows cartesio (through rosservice /xbotcore/ros_control) but cartesio does not follow simulation.

Hope this helped.

[alaurenzi]

Since Cartesio is (not only actually) a library for inverse kinematics, this is expected. It "merely" transforms cartesian space references into joint space references. To make sure that those references

• are feasible under the hypothesised force constraints
• are tracked successfully by the lower level controllers

it's out of its scope.

Moreover, in the real world, constraints such as ground contacts are unknown (not measured).

[IoannisDadiotis]

I see. Since this is the case, I think we end up that /cartesian/${link}/current_reference is not appropriate for receiving link positions (as we considered until now these references).