move_group_interface setposetarget() keeps getting aborted

[mfr...@vt.edu]

Hello,

My problem right now is that I cannot get my arm to move in RVIZ using the settargetpose() function. Here is the code I used to achieve this goal:

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group.h>

int main(int argc, char **argv)

{

  ros::init(argc, argv, "lesson_move_group");

  ros::AsyncSpinner spinner(1);

  spinner.start();

 

  move_group_interface::MoveGroup group("arm");

  geometry_msgs::Pose pose = group.getRandomPose().pose;

  group.setPoseTarget(pose);

  std::string i = group.getEndEffectorLink();

  ROS_INFO("eef %s",i.c_str());

  bool flag=group.setPoseTarget(pose);

  move_group_interface::MoveGroup::Plan plan;

  if (!group.plan(plan))

  {

    ROS_FATAL("Unable to create motion plan.  Aborting.");

    exit(-1);

  }

  sleep(1.0);

  group.stop();

  sleep(1.0);  // wait for stop command to be received

  return 0;

}

I am able to compile code,  the problem is at run time, I get this error:

[ WARN] [1390577959.580079003]: Fail: ABORTED: No motion plan found. No execution attempted.

[FATAL] [1390577959.580175337]: Unable to create motion plan.  Aborting.

atlasarm: /usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed.

Aborted (core dumped)

Why does motion plan get aborted? Shouldn't the pose be possible because I used the group.getRandomPose().pose to get it? 

When I used functions like setRandomtarget(), and  setJointValueTarget() my robot arm is able to use motionplanning to get to its destination correctly. 

The robot I am using is currently a 4DOF arm. 

If more information is needed to get to a solution please do not hesitate to ask.

Thanks in advance!

Mike

[Sachin Chitta]

Can you visualize the pose and make sure its not in collision?
The getRandomPose function does no currently check whether the pose is in collision or not.

Sachin

[mfr...@vt.edu]

Hey Sachin,

There shouldn't be any collisions because the arm is by itself in RVIZ. 

I am able to use the moveit plug through RVIZ and do motion planning using the start state and goal state tools if thats what you are asking about. I can also plan and execute through the planning tab of the plugin. 

I can also see that the setTargetPose() function successfully ran because I can see the ROS_INFO statement right below show up at run time. Only when it gets to the plan() do I get the FATAL error I mentioned in the original question,

[mfr...@vt.edu]

I also verified there should be no self collision either

[Sachin Chitta]

Instead of a random pose, can you try an actual pose that you know the robot can get to?

Sachin

[mfr...@vt.edu]

I did initially do that , and the same results came up. I used the following function to set the pose to a desired state: 

  Eigen::Affine3d pose = Eigen::Translation3d(-0.358, 0.043, 0.865)

                         * Eigen::Quaterniond(0.690, 0.108, 0.706, -0.112);

That is why I used the randomPose function because I assumed that had to set the pose to a reachable one.

I should mention that I currently need to use the approximate IK solution setting when moving the arm around in RVIZ, could this in any way be part of the problem?

[Sachin Chitta]

Yes, needing to use approximate ik is the problem. When you do it through the Rviz plugin, it will first use approximate IK to find a feasible joint space solution and then pass that solution on to the motion planner. Try the setJointValueTarget(pose, end_effector_link_name, target_frame, true) where the last true tells it to set approximate ik = true. Here target_frame is the frame in which the pose is specified.

Sachin

[mfr...@vt.edu]

Ok I have 4 questions.

1. When you say using the approximate IK is a problem , is it because I am trying to send a pose that only has the translation and orientation values, versus sending the target joint values? 

2. I tired using the function you gave me, the function had 4 arguments but none of the functions using that name had more than 2 arguments that are allowed to be passed, so at compile time the function did not work. I used the moveit::planning_interface::Movegroup api for reference. 

3. I cannot find the API for move_group_interface::Movegroup  its different from moveit::planning_interface::Movegroup  and I was wondering where I could find it.

4. Finally if the function did work , would I be able to use that motion plan  and send it to the actual arm controller, and have the arm move based on that motion plan? Or because I am using the approximate IK solution will the arm controller not follow the motion plan?

 

Again thanks for all the assistance!

Mike

[Michael Francis]

Also to add to those questions, would you be able to explain to me why my arm needs to allow approximate IK solution setting in order to move the start and goal state arms around using the interactive markers?

Without that setting enabled I am confined to moving only the last link of the chain. 

Thanks

Mike

[mfr...@vt.edu]

I was able to use this equation

bool setJointValueTarget(const Eigen::Affine3d &eef_pose, const std::string &end_effector_link = "");

gg

  It only takes the first 2 arguments however. The code now looks like this

  #include <ros/ros.h>

#include <moveit/move_group_interface/move_group.h>

int main(int argc, char **argv)

{

  ros::init(argc, argv, "lesson_move_group");

  // start a background "spinner", so our node can process ROS messages

  //  - this lets us know when the move is completed

  ros::AsyncSpinner spinner(1);

  spinner.start();

  move_group_interface::MoveGroup group("arm");

  

  std::string i = group.getEndEffectorLink();

  ROS_INFO("eef %s",i.c_str());

  Eigen::Affine3d pose = Eigen::Translation3d(-4.283, -0.332, 3.220)

                         * Eigen::Quaterniond(-0.147,-0.037, -0.005, 0.988);

  group.setJointValueTarget(pose,i);

  move_group_interface::MoveGroup::Plan plan;

  if (!group.plan(plan))

  {

    ROS_FATAL("Unable to create motion plan.  Aborting.");

    exit(-1);

  }

  // do non-blocking move request

  group.asyncExecute(plan);

  sleep(1.0);

  group.stop();

  sleep(1.0);  // wait for stop command to be received

  return 0;

}

 When I ran the code I didnt get a error message saying that the motion plan was aborted however my arm visually did not move to the target location. Still its better than constantly seeing the motion plan aborted.

[Sachin Chitta]

Sorry, my mistake. That's an internal function but its exposed through setApproximateJointValueTarget - see the code API for move_group_interface from here: http://moveit.ros.org/code-api/

Sachin

[mfr...@vt.edu]

Hey Sachin,

Thanks so much for the help the command setApproximateJointValueTarget() worked!

I do have a quick question though, after the rosnode completes its task I get this output and wanted to make sure that I was correctly exiting out of the rosnode when finishing with it. 

/usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed.

Aborted (core dumped)

Any thoughts?

Mike

[Sachin Chitta]

/usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26: boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*): Assertion `!pthread_mutex_lock(m)' failed.

Aborted (core dumped)

Yes, we are aware of this - it seems to happen when move_group_interface shuts down - we are looking into it.

Sachin

[Carlos Maestre]

Hi! I am just having the same problem in CPP when using MoveGroup (for example, moveit::planning_interface::MoveGroup group("right_arm"));

As this makes my program to be killed when exiting I cannot use Valgrind to seek for memory leaks. Any solutions?

 

[mfr...@vt.edu]

Hey Carlos,

I am not sure what your problem is, or how it relates to question in this thread.  I was having a problem with calling a method within movegroup  class. You seem to be having problems with making a call to the constructor of the class so more information is needed about your problem to get a better answer.

Thanks

Mike