Postural task on VirtualJoints for locomotion does not work
torydebra opened this issue · 10 comments
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/solutio
n 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
Can you check the stack you're using?
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
Ok but that stack has many other tasks other than the postural, which is lowest priority. Have you tried with just postural + steering + rolling?
aa right :D now it works
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
You should see the wheels moving in rviz as well. Check that your rolling and steering tasks are set correctly.
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)
Notice that having the postural first priority leads to all tasks on the second layer to be ignored
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
Closing since the original issue is not an issue