Plan self motion in Moveit
I am trying to plan two subsequent motions with Moveit. The first motion is a relatively unconstrained motion to an end effector pose. I would like the second motion to move the first roll joint to a specified position while maintaining the end effector pose. (self motion.)
I can plan the first motion just fine. I am trying to acheive the self motion by adding a goal constraint to the original motion plan request, and adding a path constraint that fixes the end effector pose.
Relevant code:
//First motion:
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("link_t", pose, tolerance_pose, tolerance_angle);
mp_request.goal_constraints.push_back(pose_goal);
mp_request.allowed_planning_time = 10.0;
moveit_msgs::GetMotionPlan mp_srv;
mp_srv.request.motion_plan_request = mp_request;
mp_srv.response.motion_plan_response = mp_response;
if(!mp_client.call(mp_srv)){
std::cout << "Motion planning failure: error_code: " << mp_srv.response.motion_plan_response.error_code.val << std::endl;
}
mp_response = mp_srv.response.motion_plan_response;
std::cout << "trajectory size: " << mp_response.trajectory.joint_trajectory.points.size() << std::endl;
kinematic_state->setStateValues(mp_response.trajectory.joint_trajectory.points.back().positions);
joint_state_msg.name = joint_state_group->getJointModelGroup()->getJointModelNames();
joint_state_msg.position = mp_response.trajectory.joint_trajectory.points.back().positions;
joint_state_publisher.publish(joint_state_msg);
ros::spinOnce();
current_joints_ = joint_state_msg;
mp_request.start_state.joint_state = current_joints_;
////////Self motion/////////////////////////
moveit_msgs::Constraints path_eef_constraints_, joint_constraints_;
moveit_msgs::JointConstraint goal_joint_constraint_;
moveit_msgs::OrientationConstraint eef_orientation_constraint_;
moveit_msgs::PositionConstraint eef_pos_constraint_;
//Set Joint_1 goal constraint and add to goal_constraints.
goal_joint_constraint_.joint_name = joint_state_msg.name.at(0);
goal_joint_constraint_.position = current_joints_.position.at(0)-1.57;
goal_joint_constraint_.position = 0;
goal_joint_constraint_.tolerance_above = .1;
goal_joint_constraint_.tolerance_below = .1;
goal_joint_constraint_.weight = .5;
//add joint goal constraint:
mp_request.goal_constraints.back().joint_constraints.push_back(goal_joint_constraint_);
//Set path constraint:
mp_request.path_constraints = pose_goal;
mp_srv.request.motion_plan_request = mp_request;
The first motion goes off without a hitch, and If I do the second motion with the joint specified, but no path constraint, it plans correctly as well. However, when I add the path constraint, I get the following errors:
[ERROR] [1368026713.167017264]: Poses vector must have size: 1
[ERROR] [1368026713.167240823]: Poses vector must have size: 1
[ INFO] [1368026713.168012375]: No planner specified. Using default.
[ WARN] [1368026713.168312286]: Skipping invalid start state (invalid bounds)
[ERROR] [1368026713.168460131]: Motion planning start tree could not be initialized!
[ INFO] [1368026713.168488571]: No solution found after 0.000251 seconds
[ WARN] [1368026713.177067471]: Goal sampling thread never did any work.
[ INFO] [1368026713.177400962]: Unable to solve the planning problem"
Motion planning failure: error_code: 1
I would guess that the Poses vector error is what is killing me, but I'm not sure what's triggering this error. Can anyone provide any insight?
