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

How to keep the robot level in motion

asked 2017-10-16 05:23:50 -0600

pdmike gravatar image

updated 2017-10-17 06:37:32 -0600

gvdhoorn gravatar image

Hello guys, I want to get my Universal Robot model keep level in the simulation in Moveit which means the rotation of X axis and Y axis of the robot's end-effector will not be changed and the the rotation of Z axis can be controlled by the planner. My code is listed as followed

int main(int argc, char **argv)
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;  
  ros::AsyncSpinner spinner(1);

  /* This sleep is ONLY to allow Rviz to come up */

  // Setup
  // ^^^^^
  // The :move_group_interface:`MoveGroup` class can be easily 
  // setup using just the name
  // of the group you would like to control and plan for.
  moveit::planning_interface::MoveGroup group("arm");

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to deal directly with the world.
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  

  // (Optional) Create a publisher for visualizing plans in Rviz.

     ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
      moveit_msgs::DisplayTrajectory display_trajectory;

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  // We can print the name of the reference frame for this robot.
  ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the 
  // end-effector.
  robot_state::RobotState start_state(*group.getCurrentState());
  geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
  //geometry_msgs::Pose start_pose2;


  moveit_msgs::OrientationConstraint ocm;  
  ocm.link_name = "tt_base_link";  
  ocm.header.frame_id = "base_link";
   ocm.orientation.x = 0.003;
  ocm.orientation.y = 0.002;
  //ocm.orientation.z = 0.101;
  //ocm.orientation.w = 0.949;
  //ocm.orientation.z = 0;
    ocm.absolute_x_axis_tolerance = 0.1;
    ocm.absolute_y_axis_tolerance = 0.1;
    ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

// Now, set it as the path constraint for the group.
  moveit_msgs::Constraints test_constraints;

  std::vector<geometry_msgs::Pose> waypoints;
  geometry_msgs::Pose target_pose1;
   //ocm.orientation.x = 0;
  //ocm.orientation.y = 0;

 // ocm.orientation.x = 0.003;
  //ocm.orientation.y = 0.002;
  //ocm.orientation.z = 0.101;
 // ocm.orientation.w = 0.949;
target_pose1.position.x = 0.53;
target_pose1.position.y = 0.24;
  target_pose1.position.z = 0.81;

   moveit_msgs::RobotTrajectory trajectory;
  double fraction = group.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);
  moveit::planning_interface::MoveGroup::Plan my_plan;
  bool success = group.plan(my_plan);
  // Now we will plan to the earlier pose target from the new 
  // start state that we have just created.
  success = group.plan(my_plan);

  ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED");
  /* Sleep to give Rviz time to visualize the plan. */

  // When done with the path constraint be sure to clear it.

  return 0;

So could you check the code and tell me about the mistakes?

Edit: @v4hn Thanks for your answer. I have used the computeCartesianPath and setPathConstraintsto constraint the orientation and let the robot move to a point next_pose. And the robot's orientation x,y,z,w did not changed in the process ... (more)

edit retag flag offensive close merge delete


what's the specific problem you're running into?

andymcevoy gravatar image andymcevoy  ( 2017-10-17 01:35:39 -0600 )edit

When the robot is moving from point A to point B, how can i keep the end effector level(constrainting the pitch angle and roll angle of the end effector equaling 0 and the yaw angle can be any value that is planned by Moveit)?

pdmike gravatar image pdmike  ( 2017-10-17 01:46:25 -0600 )edit

Do you have any idea?

pdmike gravatar image pdmike  ( 2017-10-17 02:39:58 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2017-10-17 03:53:23 -0600

v4hn gravatar image

updated 2017-11-29 09:14:31 -0600

If you know your trajectory can be executed by linear interpolation in work space, computeCartesianPath is the most straight-forward.

If that doesn't work, setPathConstraints is the right way to go here and should work. For some kinematics/solvers you might see invalid trajectories resulting from planning. In that case have a look at for a workaround. A Constraint approximation database can heavily reduce planning time here. Keep an eye at for a (hopefully) upcoming tutorial.

Edit: The tutorial is now online here.

Edit in reply to edit:

setPathConstraints only works well with plan and move, but not with computeCartesianPath. The latter "only" interpolates the end-effector pose in workspace - this includes the 3d rotation. The feasible trajectory is already fully defined in workspace. So your relaxed constraint does not give you more state space to exploit.

If you want general trajectories that satisfy the constraint, use setPathConstraints together with plan/move.

If you know your trajectory in advance up to the yaw angle, have a look at the descartes project. According to the authors, this only works well with analytical IK solvers though.

edit flag offensive delete link more


Hi, i just want to try to use the "Constraint approximation database " in link text that you recommended and i cannot find the file,

pdmike gravatar image pdmike  ( 2017-10-23 21:34:52 -0600 )edit

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp in my ROS. Have i missed to install some package?

pdmike gravatar image pdmike  ( 2017-10-23 21:35:49 -0600 )edit

To give a late update to this: There is now a tutorial online in

v4hn gravatar image v4hn  ( 2017-11-29 09:13:37 -0600 )edit

@v4hn Thank you for your reply. I have used the Trac-IK and `setPathConstraints and the end-effector has been kept horizontal to the ground. But it still has some joint jumping during the process. Please check.So Constraint Database will solve the p?

pdmike gravatar image pdmike  ( 2017-11-29 19:31:46 -0600 )edit

Question Tools



Asked: 2017-10-16 05:23:50 -0600

Seen: 618 times

Last updated: Nov 29 '17