Postural task on VirtualJoints for locomotion does not work

[torydebra]

I am trying to move centauro by applying desired joint position to the virtual joints 1-6 : I am doing:

• roslaunch centauro_cartesio centauro.launch 

• open rviz

• rostopic pub /cartesian/Postural/ference sensor_msgs/JointState "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
  name: ['VIRTUALJOINT_1']
  position: [1]
  velocity: [0]
  effort: [0]" 

Robot in rviz does not move, indeed, rostopic echo /cartesian/solution shows: position: [7.041869226543119e-18, -2.7451174071757855e-17, 0.0, 2.706062391873702e-16, 5.00623745088608e-17, -4.4164305980278976e-16 ...

even if rostopic echo /cartesian/current_reference shows correctly the sent command: position: [1.0, 0.0, 0.0, ...

Things work if I set a joint pose for a not virtual one (like one in the arm)

What I am missing? @alaurenzi

[alaurenzi]

Can you check the stack you're using?

[torydebra]

Ops sorry that launch has not the stack for loco. Anyway with correct launch and stack results are the same :

roslaunch centauro_cartesio_config centauro_wheeled_motion.launch gui:=true

which uses the stack from here https://github.com/ADVRHumanoids/iit-centauro-ros-pkg/blob/xbot2/centauro_cartesio_config/centauro_wheeled_motion_stack.yaml

In the stack, I also tried to remove the disable joint params in the Postural task

[alaurenzi]

Ok but that stack has many other tasks other than the postural, which is lowest priority. Have you tried with just postural + steering + rolling?

[torydebra]

aa right :D now it works

[torydebra]

Coming back because it is only the model that moves.

Indeed, if I command one of the virtual joints, it seems that only this joint move, so we do not see the wheels rolling. The robot in gazebo stay still probably because the one that is moving is a joint that does not exist in the gazebo/real robot

[alaurenzi]

You should see the wheels moving in rviz as well. Check that your rolling and steering tasks are set correctly.

[torydebra]

Stack is :

    - ["Postural",  "Wheel_FL", "Wheel_FR", "Wheel_HR", "Wheel_HL", "Steering_FL", "Steering_FR", "Steering_HL","Steering_HR", "LeftArm", "RightArm"] #, "Gaze"]
    - ["Rolling_FL", "Rolling_FR","Rolling_HL","Rolling_HR","Ankle_FL", "Ankle_FR", "Ankle_HR", "Ankle_HL"]

Just to check I change Postural with Waist and locomotion with wheels works correctly. (sending cartesian refs instead of joint pos of course)

[alaurenzi]

Notice that having the postural first priority leads to all tasks on the second layer to be ignored

[torydebra]

I tried to

• put all task on the same level,
• Put Postural in the second and third layer

But in these cases the robot has strange behaviour: the model inclines instead of going forward

[alaurenzi]

Closing since the original issue is not an issue