CODEWITHSUNDEEP
c++rosmoveit
Carollour
Carollour

Reputation: 85

ROS moveit constraints

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


}

Upvotes: 4

Views: 4686

Answers (1)

alextoind
alextoind

Reputation: 1203

I believe that the problem is this one:

ocm.orientation.w = 0.0;

If you look at moveit_msgs::OrientationConstraint reference you find the interpretation of that orientation field.

# The desired orientation of the robot link specified as a quaternion
geometry_msgs/Quaternion orientation

However you are setting all the fields of the quaternion to 0 (imaginary x,y and z are initialized with 0 if not specified) which could cause unexpected behaviour.

If you've followed this tutorial, you could have seen that the author set ocm.orientation.w = 1.0; which means no change in orientation (i.e. roll pitch and yaw angles are equal to 0). Thus, try to specify a realistic orientation for your constraint.

Last but not least, for the sake of clearness it could be better to write concisely the start_pose initialization:

geometry_msgs::Pose start_pose = group_r.getCurrentPose().pose;

Upvotes: 2

Related Questions