search

TechmanRobotInc / tmr_ros1

TM Robots supporting ROS1 drivers and some extended external applications. (experimental) (not support the new TM S-Series)
Other
49 stars 21 forks source link

Can't get moveit to work with the tool_pose pose output #20

Open BenRJG opened 2 years ago

BenRJG commented 2 years ago

I'm trying to setup moveit control so that I can use tool_pose to get the positions of the robot, then use these values for setting the position of the robot through moveit. The tool_pose output (which it states the frame_id is "base"):

  position: 
    x: -0.4265612487792969
    y: 0.12488037109375
    z: 0.319642578125
  orientation: 
    x: -0.0005005435936691659
    y: 0.9999888259891793
    z: 0.004190976781786781
    w: 0.002129099929145922

And the same position with what moveit outputs from move_group_interface.getCurrentPose() which is relative to the world

  position: 
    x: -0.34069897263606064
    y: 0.16769051188681533
    z: 0.461088310767565
  orientation: 
    x: 0.9238760637932933
    y: -0.3826917892535815
    z: -0.0001079185120021367
    w: 3.925936506249497e-05

When using the tool_pose in moveit, it puts it in (what seems to be) a 45° offset from it's actual position.

My assumption from this is to use setPoseReferenceFrame("base"), however this still sends it to the same position, and when looking at RViz you can see that the apparent world and base frames are in the same position. The only other logical reference I could think of was "base_link", but this is rotated 90° in the wrong axis so I'm not sure what tool_pose is actually in relation to?

Any help on this would be much appreciated :)

TechmanRobotIncOwner commented 1 year ago

May I ask why do you use moveit control? We can not duplicate your situation on TM side. Is it possible to use "FeedbackState: tool_pose" please? The below link is a demo get_status__demo_src => demo_get_feedback https://github.com/TechmanRobotInc/tmr_ros1/blob/master/demo/get_status_demo_src/demo_get_feedback.cpp