Planning in RViz works but setPoseTarget() fails.
Hello,
I'm working on making the PhantomX Reactor Arm work with MoveIt (Ubuntu 16.04. ROS Kinetic). I used a pre-made URDF model and used the setup assistant to make a MoveIt config package. Since the PhantomX arm works with the Arbotix-M microcontroller, I use the Arbotix-ROS package to initialize the follow_joint_trajectory topic which MoveIt can talk to. This setup works perfectly in RViz. I can plan to any random valid point and it the arm moves there smoothly. The robot model in RViz also actively responds when I manually move the arm.
However when I try to to move to verified valid coordinates by using the C++ interface, the planner fails.
EDIT
I have found something interesting just now when running rostopic echo on /move_group/goal When I set a planning goal from the RViz interface, I get the following ros message on the topic:
goal:
request:
workspace_parameters:
header:
seq: 0
stamp:
secs: 1578832766
nsecs: 260008148
frame_id: "/base_footprint"
min_corner:
x: -1.0
y: -1.0
z: -1.0
max_corner:
x: 1.0
y: 1.0
z: 1.0
start_state:
joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
name: []
position: []
velocity: []
effort: []
multi_dof_joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: []
transforms: []
twist: []
wrench: []
attached_collision_objects: []
is_diff: True
goal_constraints:
-
name: ''
joint_constraints:
-
joint_name: "shoulder_yaw_joint"
position: -2.07496997299
tolerance_above: 0.0001
tolerance_below: 0.0001
weight: 1.0
-
joint_name: "shoulder_pitch_joint"
position: -0.924272207835
tolerance_above: 0.0001
tolerance_below: 0.0001
weight: 1.0
-
joint_name: "elbow_pitch_joint"
position: 1.32824846404
tolerance_above: 0.0001
tolerance_below: 0.0001
weight: 1.0
-
joint_name: "wrist_pitch_joint"
position: 1.75904480582
tolerance_above: 0.0001
tolerance_below: 0.0001
weight: 1.0
-
joint_name: "wrist_roll_joint"
position: -2.9276301813
tolerance_above: 0.0001
tolerance_below: 0.0001
weight: 1.0
position_constraints: []
orientation_constraints: []
visibility_constraints: []
path_constraints:
name: ''
joint_constraints: []
position_constraints: []
orientation_constraints: []
visibility_constraints: []
trajectory_constraints:
constraints: []
planner_id: "RRTConnect"
group_name: "arm"
When I call a plan from the C++ interface, I receive the following message:
goal:
request:
workspace_parameters:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
min_corner:
x: 0.0
y: 0.0
z: 0.0
max_corner:
x: 0.0
y: 0.0
z: 0.0
start_state:
joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
name: []
position: []
velocity: []
effort: []
multi_dof_joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names: []
transforms: []
twist: []
wrench: []
attached_collision_objects: []
is_diff: True
goal_constraints:
-
name: ''
joint_constraints: []
position_constraints:
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "/base_footprint"
link_name: "gripper_guide_link"
target_point_offset:
x: 0.0
y: 0.0
z: 0.0
constraint_region:
primitives:
-
type: 2
dimensions: [0.0001]
primitive_poses:
-
position:
x: -0.0237
y: 0.0897
z: 0.3766
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
meshes: []
mesh_poses: []
weight: 1.0
orientation_constraints:
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "/base_footprint"
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
link_name: "gripper_guide_link"
absolute_x_axis_tolerance: 0.001
absolute_y_axis_tolerance: 0.001
absolute_z_axis_tolerance: 0.001
weight: 1.0
visibility_constraints: []
path_constraints:
name: ''
joint_constraints: []
position_constraints: []
orientation_constraints: []
visibility_constraints: []
trajectory_constraints:
constraints: []
planner_id: ''
group_name: "arm"
It seems to be empty at most places but I ...
There are a few things I can spot that might help. Firstly the second message from the C++ API doesn't include a
planner_id. Also the goal from the RVIZ interface is a joint goal, where the goal is specified in explicit joint angles, whereas the C++ goal is a position goal, where the goal is defined by the pose of thegripper_guide_linklink. So the two goals are actually very different, even though they may result in the arm being in the same place at the end!Thank you for your answer. I was able to solve the planner_id problem by explicitly setting this in the C++ code. As far as the other observation, I just noticed that as well. This makes me think that the IK solver is not able to find a solution for the problem it is given. I suppose this could be because the arm is 5-DOF (tried all the kinematics configs, someone is working on making a FastIK plugin now.). However I've also noted that maybe the transforms are wrong, the gripper guide link which I use as my EEF link seems to be static when I inspect it in the /tf topic. It does not change whenever I move the robot. I used the valid URDF model which has worked with MoveIt through another interface (USB2Dynamixel). When I call setToPose() with another link as EEF though (with a verified well ...(more)
It sounds like we're making some progress. Have you tried using the kinematic solver directly from the C++ API to verify that a solution can be found before passing the request to the planner? If you're working with a non-holonomic 5DOF arm then it may well be very tricky setting position goals that the planner will deem possible.
This seems to somewhat work! I now call the IK with setFromIK on the model and the set the returned joint targets via setJointValueTargets(). There are still some problems that arise though, the positions and orientations that I get from RViz don't seem to correspond with where the robot moves. I wanted to define a home position to move to, so I manually set the robot to this, and read out the coordinates from the RobotModel in RViz. I set these coordinates in the C++ file and run the code. IK solves it well and plans it too, but moves to a different position (elbow doesn't move, wrist pitch moves wrongly). I retried this by trying to set the orientation as well as found in RViz, but checking this time, the position and orientation we're completely different. This happened several times, even though RViz sees the arm ...(more)
Hi @MarijnStam ,it seems that I have the same issue. How did you finally slove this problem?
Hello @Heho
I took the approach which I shortly explained in my previous comment. Note that these solutions were somewhat desperate as I had a few days to finish my school project deadline at that time so I cannot surely say that these approaches are good or even correct. But I did get the arm moving in the end.
You can find the sourcecode for the working version here: https://github.com/MarijnStam/arm_con... Other notable files for the MoveIt configuration can also be found in this repository.
I hope this helps!
It works! Thank a lot!