PositionConstraint goal for robot arm: Unable to construct goal representation

Question

I have a setting of ROS indigo, Gazebo under Ubuntu 14.04. Under ROS, moveit node is running. A robot arm IRB120 is simulated and standing in Gazebo. I have a node that uses moveit (move_group node) to plan a path (trajectory) for for the destination that Bob wants. The planned trajectory will be sent to Gazebo to be shown later.

There is two approaches that Bob can use to describe the destination:

1. Angles of each joints of the arm: using an array of six numbers (for six joints of the arm), the form of each joint and shin is defined. This approach works fine. It uses the JointConstraint class:

   double goal_poses [] = {0.52, 0.50, 0.73, -0.02, 0.31, 6.83}; for(int i = 0 ; i < 6; i++) // iterate over joints of the arm. { moveit_msgs::JointConstraint jc; jc.weight = 1.0; jc.tolerance_above = 0.0001; jc.tolerance_below = 0.0001; jc.position = goal_poses[i]; jc.joint_name = names[i]; goal_constraint.joint_constraints.push_back(jc); }

2. Define the location and direction of the end effector only. I can not use this approach. I have used the PositionConstraint class.

Problem in short: I can describe a destination using JointConstraint class, But I don't know how to describe it in PositionConstraint class. How to describe a goal, by just pointing out where the end effector should be?

How i describe the goal in PositionConstraint format: (I point out where the end effector should be and what it's orientation should be.)

  moveit_msgs::PositionConstraint pc;
  pc.weight = 1.0;
  geometry_msgs::Pose p;
  p.position.x = 0.3; // not sure if feasible position
  p.position.y = 0.3; // not sure if feasible position
  p.position.z = 0.3; // not sure if feasible position
  pc.link_name="tool0";
  p.orientation.x = 0;
  p.orientation.y = 0;
  p.orientation.z = 0;
  p.orientation.w = 1;
  pc.constraint_region.mesh_poses.push_back(p);
  goal_constraint.position_constraints.push_back(pc);

But When the request is sent, server responds with:

[ERROR] [1527689581.951677797, 295.242000000]: Unable to construct goal representation

Note:

In both cases, I add the goal_constraint to the trajectory_request:

trajectory_request.goal.request.goal_constraints.push_back(goal_constraint);
// add other details to trajectory_request here...

trajectory_request is to be sent to the move_group. (by publishing the trajectory_request on the /move_group/goal topic)

Answer 1

A slightly different solution solved the problem of describing goal with end-effector orientation and location:

Instead of publishing the goal on a topic for another node to parse and read, we can use the moveit library function computeCartesianPath. (In this example the code to publish the trajectory is commented and partially missing)

void planTo(std::vector<double> coordinate, std::vector<double> orientation){

  geometry_msgs::Pose p;
  p.orientation.w = 1.0;
  p.position.x = coordinate[0];
  p.position.y = coordinate[1];
  p.position.z = coordinate[2];

  tf::Quaternion q = tf::createQuaternionFromRPY(
      orientation[0],orientation[1],orientation[2]);

  p.orientation.x = q.getX();
  p.orientation.y = q.getY();
  p.orientation.z = q.getZ();
  p.orientation.w = q.getW();

  std::vector<geometry_msgs::Pose> goals;
  goals.push_back(p);

  moveit::planning_interface::MoveGroup mg("manipulator");
  mg.setStartStateToCurrentState();

  // load the path in the `trajectory` variable:
  moveit_msgs::RobotTrajectory trajectory;
  mg.computeCartesianPath(goals, 0.01, 0.0, trajectory);
  // publish to gazebo:
  // trajectory.joint_trajectory.header.stamp = ros::Time::now();
  // publisher.publish(trajectory.joint_trajectory);
}

I solved this a few months ago and unfortunately i do not remember the exact source/tutorial.