Moveit C++ interface and pose goal

rj2021 · August 31, 2022, 1:42am

Hello,

I am trying to control the stretch robot in gazebo using moveit c++ interface. However the IK fails to find any goal positions from the start to goal when using the moveit c++ interface. When i send commands via rviz any goal position is achieved. The error I get is as follows:

[ WARN] [1661909681.434391358, 2306.008000000]: Fail: ABORTED: No motion plan found. No execution attempted.
[ INFO] [1661909681.434506353, 2306.008000000]: Visualizing plan 1 (pose goal) FAILED
[ INFO] [1661909686.466198011, 2307.533000000]: ABORTED: No motion plan found. No execution attempted.

[ WARN] [1661909681.446695963, 2306.011000000]: Unable to transform object from frame 'map' to planning frame 'base_link' (Could not find a connection between 'base_link' and 'map' because they are not part of the same tree.Tf has two or more unconnected trees.)
[ INFO] [1661909681.446839722, 2306.011000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1661909681.447224877, 2306.011000000]: Planning attempt 1 of at most 1
[ INFO] [1661909681.447366232, 2306.011000000]: Starting state is just outside bounds (joint 'joint_arm_l3'). Assuming within bounds.
[ INFO] [1661909681.447418610, 2306.011000000]: Starting state is just outside bounds (joint 'joint_arm_l2'). Assuming within bounds.
[ INFO] [1661909681.447469916, 2306.011000000]: Starting state is just outside bounds (joint 'joint_arm_l1'). Assuming within bounds.
[ INFO] [1661909681.449398610, 2306.011000000]: Planner configuration 'stretch_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1661909681.449707680, 2306.011000000]: stretch_arm/stretch_arm: Starting planning with 1 states already in datastructure
[ERROR] [1661909686.450354799, 2307.528000000]: stretch_arm/stretch_arm: Unable to sample any valid states for goal tree
[ INFO] [1661909686.450476116, 2307.528000000]: stretch_arm/stretch_arm: Created 1 states (1 start + 0 goal)
[ INFO] [1661909686.450504620, 2307.528000000]: No solution found after 5.000941 seconds
[ INFO] [1661909686.463289628, 2307.532000000]: Unable to solve the planning problem

I am able to control the joints from my c++ file when I set joint target goals instead of target pose goals. This seems to be a case with the IK not working with stretch effectively. Any help in setting this up will be useful.

Thank you

chintujaguar · September 1, 2022, 4:12am

Hello @rj2021,

Thank you for posting your query here! This is a known issue while using MoveIt with Stretch. The task space of the stretch_arm planning group is constrained to a volume enclosed by the lift for the height dimension, the telescopic arm for the length dimension, and the wrist yaw for the width dimension as roughly shown in the figure below.

Goals defined outside of this task space are unreachable and hence unsolvable. Before proceeding, it might be a good idea to ensure that the pose goal you are passing is indeed within the task space of the arm. One way to do this is by visualizing Stretch in RViz, manually moving the end effector to the desired pose using the interactive markers, and noting the pose reported by the TF tree as seen in the left-hand pane in the image below. You can then pass the same as an argument to the setPoseTarget() method.

If that’s not possible, you could use IK approximation to relax the solver constraints so that it has a better chance to converge. The way to do this is by using the setApproximateJointValueTarget() method instead of setPoseTarget(). I am attaching the MoveGroup C++ API reference here for your perusal.

Let me know if this helps or if you need further assistance.

Best,
Chintan