Set Initial Joint Configuration for Coman legs with feet in ground contact

[ndehio]

I just tried the new feature setInitialConfigurationForModel with Coman.
It seems that joints are moved assuming a fixed base.
This way it is impossible to bend the knees without loosing ground contact.
Furthermore, the joint-id-numbering seems to be wrong.
In its current state, the feature is not helpful for setting the initial configuration for the Coman lowerbody.

Here a piece of code from my ops file:
var rstrt.kinematics.JointAngles initJointAngles = rstrt.kinematics.JointAngles(numjoints_fullbody)

left leg

initJointAngles.angles[2] = +0.70;

right leg

initJointAngles.angles[8] = +0.70;
gazebo.setInitialConfigurationForModel("robotmodel", initJointAngles);

[ndehio]

Does anybody have any idea how to solve this issue? Starting the simulation with stretched legs in a singularity configuration is not a good idea, but bending knees and starting without foot-ground-contact with the current implementation also is very bad...

[xwavex]

Calculate bbox of current visual and align with ground_plane, or more
general with next collision point in line.

ndehio [email protected] schrieb am Mi., 9. Nov. 2016, 19:36:

Does anybody have any idea how to solve this issue? Starting the
simulation with stretched legs in a singularity configuration is not a good
idea, but bending knees and starting without foot-ground-contact with the
current implementation also is very bad...

—
You are receiving this because you were assigned.

Reply to this email directly, view it on GitHub
#17 (comment),
or mute the thread
https://github.com/notifications/unsubscribe-auth/AHenuD0sjxOP0Z21UcGlpGsnbAQvAtOGks5q8hKdgaJpZM4J1HwK
.

[ndehio]

Sorry, I don't understand what you are meaning. Could you please explain this idea a bit more in detail?

[xwavex]

Well, as I said, you need to calculate the bbox for the current visual that results from the commanded joint configuration of the robot. Then you need to find the object in the world that is closest to the robot and -- assuming gravity points in -Z -- is located below the robot. Then you calculate the distance between the lower part of the robot's bb from the other object's bb. Finally you shift the robot down by the calculated amount. In the simplest case it is just the ground_plane.

[xwavex]

The first part just projects the origin of the robot to its lowest part depending on the joint configuration.