0
$\begingroup$

Rosanswers logo

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 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?


Originally posted by flyingRobot78 on ROS Answers with karma: 1 on 2013-05-08

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hey flyingRobot78,

Could you please ask future MoveIt! related questions on the moveit-users mailing list: [email protected]. 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.


Originally posted by Sachin Chitta with karma: 1304 on 2013-05-15

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.