init_pos in controller configuration file does not work

jrl-umi3218 / mc_rtc

mc_rtc is an interface for simulated and real robotic systems suitable for real-time control

BSD 2-Clause "Simplified" License

122 stars 37 forks source link

init_pos in controller configuration file does not work #284

Closed mmurooka closed 2 years ago

mmurooka commented 2 years ago

I added the following line to the controller configuration file to reproduce this but it seems not working.

init_pos:
  translation: [20, 0.0, 0.8275]

I assume that the robot is displayed away from the origin on Rviz, but it is displayed at the origin. I tried it in several kinds of controllers (LIPMStabilizer, LIPMWalking, and my own controller) and it didn't seem to work in any of them.

There is the following message in controller output.

[warning] [KinematicInertial] Real anchor frame jumped from [   20.0621 0.00402126 0.00279068] to [ 0.0639299 0.00400111 -0.0651317] (error norm 0.000 > threshold 0.020)

arntanguy commented 2 years ago

There indeed seems to be issues. I just tried on a simple controller, and:

In choreonoid:

the real robot pose gets set to init_pos
the control robot pose remains the default one from the robot module

In ticker: it works.

This fails after the controller's reset, before that the pose is properly set.

arntanguy commented 2 years ago

https://github.com/jrl-umi3218/mc_openrtm/pull/21 fixes the init_pos issue.
https://github.com/jrl-umi3218/mc_rtc/pull/286 fixes initialization of the real robot's attitude when using
```
InitAttitudeFromSensor: true
InitAttitudeSensor: FloatingBase
```

With these patches you can test either:

Initialize the robot (control/real) to the specified pose regardless of where it is in choreonoid
```
init_pos:
translation: # ...
```

Initialize the robot to wherever it is in choreonoid

InitAttitudeFromSensor: true
InitAttitudeSensor: FloatingBase

From a quick trial of LIPMStabilizer controller with both JVRC1 and HRP2KAI (using the Attitude observer), I could not reproduce https://github.com/jrl-umi3218/mc_rtc/issues/177, neither by setting the robot's init_pos or by changing the robot initial pose in choreonoid and using InitAttitudeFromSensor.

mmurooka commented 2 years ago

From a quick trial of LIPMStabilizer controller with both JVRC1 and HRP2KAI (using the Attitude observer), I could not reproduce https://github.com/jrl-umi3218/mc_rtc/issues/177

That is different from the behavior I have tried in my local environment. With putting control/real robot to 20m forward from the origin with init_pos thanks to https://github.com/jrl-umi3218/mc_openrtm/pull/21, JVRC1 falls down forward after around 10 secs in choreonoid with lipm_walking_controller. And https://github.com/jrl-umi3218/mc_rtc/pull/285 solved it.

gergondet commented 2 years ago

Thanks @mmurooka for the report and @arntanguy for the quick fix!