Ask Your Question
2

end-effector constraint with moveIt

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

Carollour gravatar image

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

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);
    }


}
edit retag flag offensive close merge delete

Comments

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
1

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

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

Seen: 995 times

Last updated: Feb 29