Ask Your Question
0

Plan self motion in Moveit

asked 2013-05-08 05:50:22 -0500

flyingRobot78 gravatar image

updated 2013-05-08 05:57:12 -0500

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-05-15 10:46:32 -0500

Sachin Chitta gravatar image

Hey flyingRobot78,

Could you please ask future MoveIt! related questions on the moveit-users mailing list: moveit-users@googlegroups.com. Its easier for us to track MoveIt! specific questions there.

Regarding your question:

(1) Could you please mention which robot you are doing this with - you mention first roll joint but without knowing the robot its hard to put this in context.

(2) If you are trying to move only one joint of a robot, the right way to do this (if you want to do it with planning) is to have a group just consisting of that one joint and move it directly. The way you are doing it seems overly complicated for a 1 DOF motion.

(3) when you specify path constraints, you need to specify a "planning volume" by setting the workspace bounds in the motion plan request: workspace_parameters.min_corner and workspace_parameters.max_corner.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-05-08 05:50:22 -0500

Seen: 2,057 times

Last updated: May 15 '13