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 followjointtrajectory 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 do not understand how that's possible?
This is the code I use to set a target pose (directly copied from the official MoveIt tutorial)
static const std::string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
const robot_state::JointModelGroup* joint_model_group =
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
moveit::planning_interface::MoveGroupInterface::Plan plan;
geometry_msgs::Pose target_pose1;
target_pose1.position.x = -0.0237;
target_pose1.position.y = 0.0897;
target_pose1.position.z = 0.3766;
target_pose1.orientation.w = 1;
target_pose1.orientation.x = 0;
target_pose1.orientation.y = 0;
target_pose1.orientation.z = 0;
move_group.setPoseTarget(target_pose1, move_group.getEndEffectorLink().c_str());
bool success = (move_group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");
I have verified these coordinates to be valid. I did this by moving to a random valid position through RViz and then noting the end effector link coordinates displayed in RViz. These coordinates matched-up with the coordinates I retrieve in my code by checking the current pose.
This is the output I get when I run my node.
You can start planning now!
[ INFO] [1578761035.404785316]: Ready to take commands for planning group arm.
Group 'arm' using 5 variables
* Joints:
'shoulder_yaw_joint' (Revolute)
'shoulder_pitch_joint' (Revolute)
'elbow_pitch_joint' (Revolute)
'wrist_pitch_joint' (Revolute)
'wrist_roll_joint' (Revolute)
'gripper_guide_joint' (Fixed)
* Variables:
'shoulder_yaw_joint', index 0 in full state, index 0 in group state
P.bounded [-3.14159, 3.14159]; V.bounded [-6.17847, 6.17847]; A.unbounded [0, 0];
'shoulder_pitch_joint', index 1 in full state, index 1 in group state
P.bounded [-1.63625, 1.63625]; V.bounded [-6.17847, 6.17847]; A.unbounded [0, 0];
'elbow_pitch_joint', index 2 in full state, index 2 in group state
P.bounded [-2, 1.7]; V.bounded [-6.17847, 6.17847]; A.unbounded [0, 0];
'wrist_pitch_joint', index 3 in full state, index 3 in group state
P.bounded [-1.7, 1.9]; V.bounded [-6.17847, 6.17847]; A.unbounded [0, 0];
'wrist_roll_joint', index 4 in full state, index 4 in group state
P.bounded [-3.14159, 3.14159]; V.bounded [-6.17847, 6.17847]; A.unbounded [0, 0];
* Variables Index List:
0 1 2 3 4 (contiguous)
* Kinematics solver bijection:
0 1 2 3 4
[ INFO] [1578761035.693196908]: Reference frame: /base_footprint
[ INFO] [1578761035.693270081]: End effector link: gripper_guide_link
Reference frame: /base_footprint
Robot position : 0.0264299 -0.0174667 0.269457
Robot Orientation : 0.0926051 -0.604176 -0.0283573 0.790943
[ INFO] [1578761035.893952679]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1578761035.895699417]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional
configuration parameters will be set when the planner is constructed.
[ INFO] [1578761035.896540521]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1578761035.896618363]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1578761035.896703533]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1578761035.896820058]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1578761037.153950966]: Loading robot model 'phantomx_reactor_arm'...
[ INFO] [1578761037.215782514]: Loading robot model 'phantomx_reactor_arm'...
[ INFO] [1578761037.360564841]: Starting scene monitor
[ INFO] [1578761037.365022124]: Listening to '/move_group/monitored_planning_scene'
[ERROR] [1578761040.897764298]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1578761040.897841811]: RRTConnect: Created 1 states (1 start + 0 goal)
[ERROR] [1578761040.901611060]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1578761040.901677997]: RRTConnect: Created 1 states (1 start + 0 goal)
[ERROR] [1578761040.901800535]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1578761040.901855483]: RRTConnect: Created 1 states (1 start + 0 goal)
[ERROR] [1578761040.901995690]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1578761040.902051879]: RRTConnect: Created 1 states (1 start + 0 goal)
[ WARN] [1578761040.902183565]: ParallelPlan::solve(): Unable to find solution by any of the threads in 5.006007 seconds
[ INFO] [1578761075.933491097]: Unable to solve the planning problem
[ WARN] [1578761075.934073223]: Fail: ABORTED: No motion plan found. No execution attempted.
Then when I try planning a random state in RViz which is still open from the same launch, I get the following output. The main difference I see is that the planner name is prefixed by the MoveGroup name, i.e. arm[RRTConnect]. In the failed example you just see RRTConnect, however I do not know what this could indicate.
[ INFO] [1578761294.275854156]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1578761294.276895503]: Planner configuration 'arm[RRTConnect]' will use planner 'geometric::RRTConnect'.
Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1578761294.277344996]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.277459190]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.277527351]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.277683322]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.295058959]: arm[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1578761294.298774925]: arm[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1578761294.306404451]: arm[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1578761294.308152450]: arm[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1578761294.308729324]: ParallelPlan::solve(): Solution found by one or more threads in 0.031605 seconds
[ INFO] [1578761294.308987981]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.314495395]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.314696391]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.315319472]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.320224654]: arm[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1578761294.321954015]: arm[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1578761294.324122961]: arm[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1578761294.326396188]: arm[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1578761294.333165921]: ParallelPlan::solve(): Solution found by one or more threads in 0.024242 seconds
[ INFO] [1578761294.333395748]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.333537334]: arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1578761294.340018295]: arm[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1578761294.351122043]: arm[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1578761294.351400070]: ParallelPlan::solve(): Solution found by one or more threads in 0.018090 seconds
[ INFO] [1578761294.410643696]: SimpleSetup: Path simplification took 0.058959 seconds and changed from 3 to 2 states
I find it strange how RViz is able to simply solve these planning problems using all the same parameters and configs while the C++ interface can't solve it. One interesting thing to note is that one time I accidently configured an invalid Kinematics plugin in kinematics.yaml. The C++ interface trying to plan immediately encountered a fatal error and aborted, but the remaining RViz window was still able to plan perfectly fine. This makes me wonder, do these two use different solvers/planners?
You can find all my files at https://github.com/MarijnStam/arm_controller The launch files I use are:
- arm_bringup/launch/arm.launch
- arm_interface/launch/arm.launch
Any help with this issue would be greatly appreciated. I am at somewhat of a loss with this issue. Thanks in advance.
EDIT
Not sure if relevant but this warning is also sometimes spit out:
[ WARN] [1578767397.336301289]: Interactive marker 'EE:goal_gripper_guide_link' contains unnormalized
quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for
ros.rviz.quaternions to see more details.
EDIT2
When I solely launch the arm.bringup along with the state publisher (this also publishes the robot description) and load the Robot Model in RViz I get an error saying:
No transform from [gripper_right_link] to [base_footprint]
No transform from [gripper_left_link] to [base_footprint]
Here's a picture with the error: https://imgur.com/a/xqAgcte You can see that the two gripper links are not shown.
I'm not sure if this relates to the issue but I figure it might. I don't understand what could be wrong with the URDF, I've checked it all but nothing seems out of place. Also when loading the model with MoveIt launch files, the model does show properly along with the two links which throw an error here.
Asked by MarijnStam on 2020-01-11 12:03:45 UTC
Comments
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!Asked by PeteBlackerThe3rd on 2020-01-13 05:44:22 UTC
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 transformed link, wrist_2_link e.g.), it still fails.
Asked by MarijnStam on 2020-01-13 06:19:05 UTC
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.
Asked by PeteBlackerThe3rd on 2020-01-13 08:15:38 UTC
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 as it is in the real world. Here's the tf tree for the arm: https://drive.google.com/drive/folders/1kpXye9bcFakw62yP8xhMDeaUqx0x4Hqf?usp=sharing
Asked by MarijnStam on 2020-01-13 10:22:36 UTC
Hi @MarijnStam ,it seems that I have the same issue. How did you finally slove this problem?
Asked by Heho on 2020-10-09 21:09:54 UTC
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_controller/blob/master/arm_interface/src/armMain.cpp Other notable files for the MoveIt configuration can also be found in this repository.
I hope this helps!
Asked by MarijnStam on 2020-10-10 03:36:20 UTC
It works! Thank a lot!
Asked by Heho on 2020-10-10 23:18:46 UTC