search

corlab / rtt-gazebo-embedded

Gazebo wrapped in an Orocos Component
0 stars 3 forks source link

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

Open ndehio opened 8 years ago

ndehio commented 8 years ago

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);

coman_init

ndehio commented 8 years ago

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 commented 8 years ago

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

ndehio notifications@github.com 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 https://github.com/corlab/rtt-gazebo-embedded/issues/17#issuecomment-259489617, or mute the thread https://github.com/notifications/unsubscribe-auth/AHenuD0sjxOP0Z21UcGlpGsnbAQvAtOGks5q8hKdgaJpZM4J1HwK .

ndehio commented 8 years ago

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

xwavex commented 8 years ago

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 commented 8 years ago

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