End effector/last link pose of a 7dof manipulator with MoveIt

asked 2021-05-06 05:38:59 -0500

carbo97 gravatar image

Hey there,

I'm working with a 7dof manipulator using MoveIt with no end effector. I can plan and excecute the position and orientation of the last joint without problems when I use rviz and I can move the joints with the mouse. To see the current pose of the 7th link reached by using rviz, I use rosrun tf tf_echo world kuka_lwr_left_7_link.

The problem is that when I commad from c++ over the move_group interface the same pose extracted by tf, most of the time no motion plan is found.

To do an example both the poses that follow respect the limits and are executed correctly in rviz, but when I command them with c++, just for the second one the plan is found.

1:
  pose.position.x=0.676;
  pose.position.y=0.104;
  pose.position.z=0.952;
  pose.orientation.x=-0.309;
  pose.orientation.y=-0.438;
  pose.orientation.z=0.690;
  pose.orientation.w=-0.486;

2:
  pose.position.x=0.676;
  pose.position.y=0.104;
  pose.position.z=0.952;
  pose.orientation.x=-0.000;
  pose.orientation.y=0.000;
  pose.orientation.z=1.000;
  pose.orientation.w=0.000;

I am using RRT as planner (the same in rviz) and I also set group.setPlanningTime(10);

edit retag flag offensive close merge delete

Comments

What is the end effector link? You can check with getEndEffectorLink(). How are you sending the command and what is the error?

fvd gravatar image fvd  ( 2021-05-07 08:56:27 -0500 )edit

I am not using an end effector so getEndEffectorLink() retrieve the last link, namely "kuka_lwr_left_7_link". The message I receive is:

ABORTED: No motion plan found. No execution attempted.

I have almost solved the problem but it is still not working as I would like (as I said before all these poses are valide since I can achieve them using rviz). The code I was using was:

moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit::planning_interface::MoveGroupInterface group(kuka_lwr_left_7_link);
group.setPoseTarget(initial_pose);
group.move();

But it worked only for poses with "simple" orientation like the second one I reported on the question.

For more articulated poses (as the first one I reported on the question), I figure out that I have to use: 

moveit::planning_interface::MoveGroupInterface group(kuka_lwr_left_7_link);
group.setApproximateJointValueTarget(initial_pose, "kuka_lwr_left_7_link");
group.move();
carbo97 gravatar image carbo97  ( 2021-05-09 12:33:06 -0500 )edit

The problem is that with the command setApproximateJointValueTarget, the pose reached by the link 7 is not very precise and sometimes it is even totally wrong. Is there another way to reach the desired pose more precisely even with more articulated poses (with different orientations)? I already tried to use setGoalTolerance, but it doesn’t work.

carbo97 gravatar image carbo97  ( 2021-05-09 12:33:28 -0500 )edit

Please add more of the error message to the original post, especially if there is a no valid states found for start state or similar message. What is the start state? Is a JointTarget set as well? Is the PoseTarget set correctly, with the correct header.frame_id? It would probably help if you added more of your code and your context/setup to the question. I'm pretty sure the KUKA LWR can't reach pose.position.z=0.952 unless it's mounted on a platform, for example.

fvd gravatar image fvd  ( 2021-05-09 19:39:58 -0500 )edit

I'm not sure what you mean by "more articulated poses" either, but I don't see why setJointValueTarget wouldn't work. I've had no problem with a KUKA iiwa in the past.

fvd gravatar image fvd  ( 2021-05-09 19:54:04 -0500 )edit
add a comment