Robotics StackExchange | Archived questions

end-effector constraint with moveIt

Hi,

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);
    spinner.start();


    /* This sleep is ONLY to allow Rviz to come up */
    sleep(20.0);

    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);
    group_r.setStartState(start_state);

    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;
    test_constraints.orientation_constraints.push_back(ocm);
    group_r.setPathConstraints(test_constraints);



    geometry_msgs::Pose next_pose = start_pose;
    while(1){
        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. */
        sleep(5.0);
    }


}

Asked by Carollour on 2015-04-21 11:18:44 UTC

Comments

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

Asked by pdmike on 2017-10-26 00:38:00 UTC

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.

Asked by Rony_s on 2019-12-19 19:45:20 UTC

Answers

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.

Asked by fvd on 2020-02-29 07:34:05 UTC

Comments