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

end-effector constraint with moveIt

asked 2015-04-21 11:18:44 -0500

Carollour gravatar image

updated 2015-04-21 12:02:12 -0500


I’m trying to use move it to move an arm vertically ONLY. The idea is to keep the tip of the end-effector to always keep the x and y-axis position and change the z-axis position in each iteration, keeping its orientation as well. I also want to constrain the movement from start-position to end-position in each iteration to follow this constraints (x and y fixed, z changing only). I don’t care about how much the joints change as long as the gripper (my end-effector) only moves upwards.

I tried to do it as presented bellow, but i don’t see any constraints being followed? What am I doing wrong?

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 */

    moveit::planning_interface::MoveGroup group_r("right_arm");

    robot_state::RobotState start_state(*group_r.getCurrentState());
    geometry_msgs::Pose start_pose;
    start_pose.orientation.x = group_r.getCurrentPose().pose.orientation.x;
    start_pose.orientation.y = group_r.getCurrentPose().pose.orientation.y;
    start_pose.orientation.z = group_r.getCurrentPose().pose.orientation.z;
    start_pose.orientation.w = group_r.getCurrentPose().pose.orientation.w;
    start_pose.position.x = group_r.getCurrentPose().pose.position.x;
    start_pose.position.y = group_r.getCurrentPose().pose.position.y;
    start_pose.position.z = group_r.getCurrentPose().pose.position.z;

    //const robot_state::JointModelGroup *joint_model_group = start_state.getJointModelGroup(group_r.getName());
    //start_state.setFromIK(joint_model_group, start_pose);

    moveit_msgs::OrientationConstraint ocm;

    ocm.link_name = "r_wrist_roll_link";
    ocm.header.frame_id = "base_link";
    ocm.orientation.w = 0.0;
    ocm.absolute_x_axis_tolerance = 0.0;
    ocm.absolute_y_axis_tolerance = 0.0;
    ocm.absolute_z_axis_tolerance = 0.1;
    ocm.weight = 1.0;
    moveit_msgs::Constraints test_constraints;

    geometry_msgs::Pose next_pose = start_pose;
        std::vector<geometry_msgs::Pose> waypoints;
        next_pose.position.z -= 0.03;
        waypoints.push_back(next_pose);  // up and out

        moveit_msgs::RobotTrajectory trajectory;
        double fraction = group_r.computeCartesianPath(waypoints, 0.005, 0.0, trajectory);

        /* Sleep to give Rviz time to visualize the plan. */

edit retag flag offensive close merge delete


Hi I have the same problem. Have you solved it? Would you mind sharing the solution?

pdmike gravatar image pdmike  ( 2017-10-26 00:38:00 -0500 )edit

Hi, am also having the same issue! @pdmike, @Carollour have you guys found a solution for that? Would be appreciated if you can share your thoughts about it. The only think I thought about, is to keep x and y axis fixed and change the Z value with too short values for example, so the endeffector doesn't change the x and y axis while it moving to the new z value.

Rony_s gravatar image Rony_s  ( 2019-12-19 18:45:20 -0500 )edit

1 Answer

Sort by » oldest newest most voted

answered 2020-02-29 06:34:05 -0500

fvd gravatar image

It is unclear what you mean by "I don't see any constraints being followed". I also don't see why the loop reinitializes the waypoints vector. The code you posted should move the robot's end effector upwards if it can. Please describe the error and paste your terminal output if it doesn't.

Either way, computeCartesianPath does not plan with constraints, it only uses a linear interpolation between the waypoints. You can decrease the value of the second argument eef_step if you find that the motions do not respect the orientation sufficiently.

You could also use the Descartes library for cartesian planning under constraints.

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2015-04-21 11:18:44 -0500

Seen: 1,690 times

Last updated: Feb 29 '20