Syncing Move Group joint states with robot in simulation

asked 2021-01-23 11:33:30 -0600

Henning Luo
58 ●3 ●6 ●10

updated 2021-01-23 13:39:51 -0600

Hi,

I have been trying to use a physics engine to do experiments instead of my real robot. As I was connecting MoveIt with the physics engine, one problem that I encountered was that move group is not changing the joint states according to my robot in simulation.

All joint states are published with correct names to the topic "/yumi/joint_states", and I have joint_state_publisher and robot_state_publisher launched as well.

<node name="yumi_joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
  <rosparam param="source_list">["/yumi/joint_states"]</rosparam>
</node>
<node name="yumi_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

Just in case that the move group did not subscribe to the joint states correctly, I echoed "/move_group/monitored_planning_scene/robot_state/joint_state". By manipulating the robot in the physics engine, I am affirmative that the joint states fed into MoveIt are from the physics engine. Here is a sample result of the echo command:

header: 
seq: 0
stamp: 
  secs: 1611415632
  nsecs: 622175931
frame_id: "yumi_base_link"
name: [yumi_joint_1_l, yumi_joint_2_l, yumi_joint_7_l, yumi_joint_3_l, yumi_joint_4_l, yumi_joint_5_l,
    yumi_joint_6_l, gripper_l_joint, gripper_l_joint_m, yumi_joint_1_r, yumi_joint_2_r,
    yumi_joint_7_r, yumi_joint_3_r, yumi_joint_4_r, yumi_joint_5_r, yumi_joint_6_r,
    gripper_r_joint, gripper_r_joint_m]
position: [6.463619115493202e-07, -0.039591364562511444, 0.04112724959850311, 0.009112260304391384, 8.23559503260185e-07, 0.01217526476830244, -3.0866064548717986e-07, 0.0, 0.0, -3.3755272852431517e-06, -0.03959168866276741, -0.04112748056650162, 0.009136912412941456, 5.3803930200047034e-08, 0.012184608727693558, 3.047164298664029e-09, 0.0, 0.0]
velocity: []
effort: []

However, the robot in RVIZ always returns to its default shape after executing trajectories. And I don't see robot moving in RVIZ when I move it in the physics engine as well.

Has anybody worked on connecting MoveIt with real robots? Is there anything else I need to do to get the joint states updated in MoveIt?

I am using ROS melodic on Ubuntu 18.04.

Any idea is appreciated! Thanks!

edit retag flag offensive close merge delete

Comments

Please clarify what "echo command" you are using, how you are starting up MoveIt (which launch files?), and any other details that might be relevant from your setup.

fvd ( 2021-01-27 06:42:42 -0600 )edit

Also, you say "the robot in Rviz always returns to its default shape after executing trajectories". Can the trajectories be executed? Does the robot in your physics engine move when you do? Are there errors in your terminal when you execute a trajectory?

fvd ( 2021-01-27 17:12:59 -0600 )edit

out of curiosity: which "physics engine" are you using?

gvdhoorn ( 2021-02-02 02:24:45 -0600 )edit

I was using Unity3D. Sorry for the late reply. Hope this helps.

Henning Luo ( 2021-04-01 08:13:01 -0600 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2021-02-01 18:01:37 -0600

Henning Luo
58 ●3 ●6 ●10

Just realised that the joint states from the physics engine is faulty, they were transformed from rad to deg twice and hence all the joint states were close to zero, which led to the weird behaviour.

edit flag offensive delete link

Comments

Thanks for updating your question.

fvd ( 2021-02-01 19:16:50 -0600 )edit

add a comment

answered 2021-01-26 22:28:37 -0600

fvd
2180 ●15 ●98 ●56

Your physical robot (or physics engine) needs to publish the joint states to the topic that MoveIt listens to. By default, that is /joint_states. Have you tried remapping from /yumi/joint_states to /joint_states?

edit flag offensive delete link

Comments

Thanks for reply! Yes I remmaped '/yumi/joint_states' to '/joint_states' with 'joint_state_publisher'. I have also checked the joint states in moveit monitored planning scene by echoing '/move_group/monitored_planning_scene/robot_ate/joint_state', the state printed was correct. However, the states I visualise in RVIZ remained at default, and in physics engine I can see that the next action is planned starting from default joint position as well. Any ideas why this might be the case? Thank you very much!

Henning Luo ( 2021-01-27 05:09:01 -0600 )edit

Are you sure that you remapped it properly? Does rostopic echo /joint_states print the joint states? It looks like the planning scene in the Rviz plugin does not receive them. Are you running the node with your move_group in a namespace?

fvd ( 2021-01-27 05:12:17 -0600 )edit

Yes I have checked that joint_states multiple times, and within the monitored_planning_scene the joint state published by move goup was actually correct! I originally thought that this may be because that my published joint states was incorrect and therefore moveit did not receive all the joint states. However, after checking the names multiple times, I don't think there are any mistakes...

Henning Luo ( 2021-01-27 05:20:14 -0600 )edit

In your OP you wrote that you checked /yumi/joint_states, not /joint_states. Do the joint states you see published on the topic correspond when the robot moves in your physics engine? Is your move_group node in a namespace?

fvd ( 2021-01-27 05:42:51 -0600 )edit

Yes, the states correspond to the moves. I tested both the arms and the grippers. my move_group is not in a namespace.

Henning Luo ( 2021-01-27 05:54:03 -0600 )edit

Can you confirm that those states are published to /joint_states (note the leading /) and not just /yumi/joint_states? Is there anything else that is non-standard in your system?

fvd ( 2021-01-27 06:41:45 -0600 )edit

Thanks for reminding about the topic name, but I think it was correct. Regarding things that are non-standard, the only thing that I can think of is that I am using a TCP server to send the messages to ROS from the physics engine. However, I can rostopic echo /joint_states and the result seems to correspond to the real states well. So that shouldn't be the problem... Is there any possibility that this is related to publish rate? joint_states publishes at 10hz.

Henning Luo ( 2021-01-27 14:24:14 -0600 )edit

Thanks for taking your precious time and replying, fvd. I did not check the joint_states carefully enough. Enclosed my solution in another answer. Thanks again.

Henning Luo ( 2021-02-01 18:05:18 -0600 )edit

add a comment

Syncing Move Group joint states with robot in simulation

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Syncing Move Group joint states with robot in simulation edit

Comments

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Syncing Move Group joint states with robot in simulation