ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

[MoveIt!] Plan to a goal without specifying all axes

asked 2016-11-26 04:09:32 -0500

F4bich gravatar image


is there a way to plan a path to a goal which is defined with a position (x, y, z) and a value for pitch and roll, but leave yaw open for variation? For my use case the yaw angle is not relevant, so I don't want to restrict possible paths by defining a value for it.

As far as I understand not giving a value for the axis will not work, because the value is initialized to zero.

Another approach would be to set the goal_orientation_tolerance. However, this is quite unsatisfying as this sets the tolerance for the whole pose and not only one specific axis.

edit retag flag offensive close merge delete



Not an answer, but this would seem like something descartes is very good at:

Descartes is a ROS-Industrial project for performing path-planning on under-defined Cartesian trajectories

Cartesian only though.

gvdhoorn gravatar image gvdhoorn  ( 2016-11-26 04:46:13 -0500 )edit

I think this could be implemented with a OrientationConstraint. I haven't used it yet, but I think you could set the absolute_z_axis_tolerance to something small and the other two much larger.

NEngelhard gravatar image NEngelhard  ( 2016-11-26 06:36:21 -0500 )edit

OrientationConstraints are also definitely possible. Would remove the need for any additional planners. I just wanted to give a pointer to a planner that might be better suited for underconstrained planning problems.

gvdhoorn gravatar image gvdhoorn  ( 2016-11-27 04:27:16 -0500 )edit

As far as I understand these are constraints one can limit the possible paths with. Therefore, an OrientationConstraint would only solve the problem, when the scenario is something like moving in a straight line, otherwise the constraint has a high chance of preventing us to find any path.

F4bich gravatar image F4bich  ( 2016-11-28 04:19:23 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2016-11-29 16:55:57 -0500

The OMPL's RRTConnectkConfigDefault planner should handle this, you just need to setup the constraint at the goal pose such that the orientation about the desired tool axis is underconstrained, below is pseudo code of how you'd set this up:

  // Create the Motion Plan request object
  moveit_msgs::MotionPlanRequest req;
  req.motion_plan_request.group_name = group_name;
  req.motion_plan_request.workspace_parameters.header.stamp = ros::Time::now();
  req.motion_plan_request.start_state = start_state;

  // Set the tool pose
  geometry_msgs::PoseStamped stamped;
  stamped.pose = pose;
  stamped.header.stamp = ros::Time::now();
  stamped.header.frame_id = req.motion_plan_request.workspace_parameters.header.frame_id;

  // defining constraints such that tool z orientation constraint has a large tolerance of 1.0 
  std::vector<double> pos_constraint = {1e-3,1e-3,1e-3};
  std::vector<double> orient_constraint =  {0.01,0.01,1.0}; 
  moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
    tool_link_name, stamped,pos_constraint,orient_constraint);

 // Add the constraint to the motion planning service request 

// call the motion planning service, res)
edit flag offensive delete link more



Is it possible to do this with the python api aswell? I don't know how to do the step

moveit_msgs::Constraints c = kinematic_constraints::constructGoalConstraints(
    tool_link_name, stamped,pos_constraint,orient_constraint);

with python

F4bich gravatar image F4bich  ( 2016-12-27 08:06:04 -0500 )edit

Question Tools



Asked: 2016-11-26 04:09:32 -0500

Seen: 1,517 times

Last updated: Nov 29 '16