Clopema arm navigation

The package contains nodes and configuration files for motion planning, kinematics computation, trajectory filtering, etc.

Inverse kinematics tasks

There are two plugins for IKT, ma1400_ikt and xtion_ikt. This plugins are used to provide constraints aware inverse kinematics task using arm_navigation package. Also clopema_dual_ik node use this plugins to provide constraints aware IKT for both arms at the same time.

Collision checking

Configuration file clopema_planning_description.yaml define pairs which are not checking for collisions (adjacent in collision, never in collision).

Motion planning

  • Groups (kinematics chains), used for motion planning, are defined in file ompl_planning.yaml.
  • Node plan_out_of_vision provide service for planning arms out of the xtions vision (details: Plan out of vision).

Trajectory interpolation

Interpolation is providing by the two services /clopema_planner/joint_interpolation and /clopema_planner/linear_interpolation. This services are provided by the node which can be start using following command:

roslaunch clopema_arm_navigation interpolation.launch

Meaning of the parameters in the launch file:

  <param name="joint_step" value="0.01"/> <!-- Maximum angle [rad] between two points in the trajectory used for joint interpolation -->
  <param name="linear_step" value="0.01"/> <!-- Distance [m] between two poses in the trajectory used for linear interpolation -->
  <param name="max_joint_angle" value="0.05"/> <!-- Maximum angle [rad] between two points in the trajectory used for checking -->

Interpolation in joint coordinates

void joint_interpolation() {
    sensor_msgs::JointState f, g;
    f.name.push_back("r2_joint_s");
    f.name.push_back("r2_joint_l");
    f.name.push_back("r2_joint_u");
    f.name.push_back("r2_joint_r");
    f.name.push_back("r2_joint_b");
    f.name.push_back("r2_joint_t");
    f.header.frame_id = "base_link";
    f.position.resize(f.name.size());
    f.velocity.resize(f.name.size());
    f.effort.resize(f.name.size());

    g.name = f.name;
    g.header = f.header;
    g.position.resize(g.name.size());
    g.velocity.resize(g.name.size());
    g.effort.resize(g.name.size());

    g.position[0] = -0.5;
    g.position[1] = 0.5;
    g.position[2] = 0.5;
    g.position[3] = 0.5;
    g.position[4] = 0.5;
    g.position[5] = 0.5;

    clopema_arm_navigation::ClopemaJointInterpolation srv;
    srv.request.poses.push_back(f);
    srv.request.poses.push_back(g);
    srv.request.poses.push_back(f);
    srv.request.poses.push_back(g);
    srv.request.poses.push_back(f);

    if (!ros::service::call("/clopema_planner/joint_interpolation", srv)) {
        ROS_ERROR("Can't call joint_interpolation service.");
        return;
    }

    if (!srv.response.error.empty())
        ROS_WARN_STREAM("error: "<<srv.response.error);

    control_msgs::FollowJointTrajectoryGoal goal;
    if (srv.response.joint_trajectory.points.size() > 2) {
        goal.trajectory = srv.response.joint_trajectory;
        ROS_INFO("Sending trajectory size: %d", (int)goal.trajectory.points.size());
        cm->cmc_.sendGoal(goal, &doneCb);
        cm->cmc_.waitForResult(ros::Duration(60));
    }

}

Interpolation in Cartesian coordinates

void linear_interpolation() {
    clopema_arm_navigation::ClopemaLinearInterpolation srv;
    geometry_msgs::Pose f, g;
    f.position.x = 0.5;
    f.position.y = -1.0;
    f.position.z = 1.0;
    f.orientation = tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0, 0);

    g.position.x = 0.5;
    g.position.y = -1.0;
    g.orientation = tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0, 0);
    g.position.z = 0.75;

    srv.request.poses.push_back(f);
    srv.request.poses.push_back(g);
    /*srv.request.poses.push_back(f);
     srv.request.poses.push_back(g);
     srv.request.poses.push_back(f);*/
    srv.request.ik_link_name = "r2_ee";
    srv.request.header.frame_id = "base_link";

    if (!ros::service::call("/clopema_planner/linear_interpolation", srv)) {
        ROS_ERROR("Can't call linear_interpolation service.");
        return;
    }

    if (!srv.response.error.empty())
        ROS_WARN_STREAM("error: "<<srv.response.error);

    if (!moveTo(srv.request.poses[0], srv.request.header.frame_id, srv.request.ik_link_name, "r2_arm")) {
        ROS_ERROR_STREAM("Can't plan to the start position");
        return;
    }

    printTraj(srv.response.joint_trajectory);

    control_msgs::FollowJointTrajectoryGoal goal;
    if (srv.response.joint_trajectory.points.size() > 2) {
        goal.trajectory = srv.response.joint_trajectory;
        ROS_INFO("Sending trajectory size: %d", (int)goal.trajectory.points.size());
        cm->cmc_.sendGoal(goal, &doneCb);
        cm->cmc_.waitForResult(ros::Duration(60));
    }
}