Plan r1 arm joint goal

Node description

This node will plan and execute trajectory for the first arm. The node require one input parameter (double) which is desired s-axis position. Other axes are set to be equal with home position.

To run node from terminal type:

rosrun clopema_planning_tutorials move_joint_r1 0.5

Source code description

There are two functions in the source code, first for actual robot position reading and second for plan creation.

Get robot position

The robot position is read from the environment server via provided service.
If service calling failed the function returns false, otherwise robot state variable is set to current robot state and function returns true.

bool getRobotState(arm_navigation_msgs::RobotState & rs) {
    ros::service::waitForService("/environment_server/get_robot_state");
    arm_navigation_msgs::GetRobotState r;
    if (!ros::service::call("/environment_server/get_robot_state", r)) {
        ROS_ERROR("Can't get current robot state.");
        return false;
    }
    rs = r.response.robot_state;
    return true;
}

Create plan

To create a plan we must specifie group name (can be: r1_arm, r2_arm, arms, ext_axis), allowed planning time and goal constraints.
In this case, goal constraints are specified for joints. Constraint for all joints in the group have to be set otherwise planning will failed.

arm_navigation_msgs::MotionPlanRequest createPlan(double sAxis) {
    arm_navigation_msgs::MotionPlanRequest req;
    req.group_name = "r1_arm";
    req.allowed_planning_time = ros::Duration(5.0);
    std::vector<std::string> names;
    names.push_back("r1_joint_s");
    names.push_back("r1_joint_l");
    names.push_back("r1_joint_u");
    names.push_back("r1_joint_r");
    names.push_back("r1_joint_b");
    names.push_back("r1_joint_t");
    req.goal_constraints.joint_constraints.resize(names.size());
    for (unsigned int i = 0; i < req.goal_constraints.joint_constraints.size(); ++i) {
        req.goal_constraints.joint_constraints[i].joint_name = names[i];
        req.goal_constraints.joint_constraints[i].position = 0.0;
        req.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        req.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
    }

    req.goal_constraints.joint_constraints[0].position = sAxis;

    return req;
}

Main program

Main program can be divided into few parts.
Firstly, the number of input parameters is checked and then ros node is initialized.

if (argc < 2) {
    ROS_ERROR("Not enough arguments: send_traj <R1sAxisDesiredPosition>");
    return -1;
}
ros::init(argc, argv, "r1_arm_joint_goal");
ros::NodeHandle nh;

The next step is specifying motion plan and calling planning service which returns planned trajectory (or error code if it is impossible to plan).
To specify motion plan the previous declared functions are used.

clopema_arm_navigation::ClopemaMotionPlan mp;
std::stringstream argv1(argv[1]);
mp.request.motion_plan_req = createPlan(atof(argv1.str().data()));

if (!getRobotState(mp.request.motion_plan_req.start_state)) {
    ROS_ERROR("Can't get current robot state");
    return -1;
}

ros::service::waitForService("/clopema_planner/plan");
if (!ros::service::call("/clopema_planner/plan", mp)) {
    ROS_ERROR("Can't call service clopema_planner/plan");
    return -1;
}

The result of the planning (planned trajectory) is store in mp.response.trajectory variable. Firstly, we check if planning was successful and then the planned trajectory is sent to the action server. How to sent trajectory to the action server is described in Sending trajectory to the controller.

if (mp.response.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) {
    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> move("/clopema_controller/follow_joint_trajectory", true);
    move.waitForServer();
    control_msgs::FollowJointTrajectoryGoal goal;
    goal.trajectory = mp.response.joint_trajectory;
    move.sendGoal(goal);
    bool finished_within_time = move.waitForResult(ros::Duration(45.0));
    if (!finished_within_time) {
        move.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    } else {
        actionlib::SimpleClientGoalState state = move.getState();
        bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
        if (success)
            ROS_INFO("Action finished: %s", state.toString().c_str());
        else {
            ROS_INFO("Action failed: %s", state.toString().c_str());
            ROS_WARN("Addition information: %s", state.text_.c_str());
            control_msgs::FollowJointTrajectoryResult r;
            ROS_WARN("Error code: %d", move.getResult()->error_code);
        }
    }
} else {
    ROS_WARN("Can't plan trajectory: %s", arm_navigation_msgs::armNavigationErrorCodeToString(mp.response.error_code).data());
}

The last step is to turn off the servo power. It is also done via service.

clopema_motoros::SetPowerOff soff;
soff.request.force = false;
ros::service::waitForService("/joint_trajectory_action/set_power_off");
if (!ros::service::call("/joint_trajectory_action/set_power_off", soff)) {
    ROS_ERROR("Can't call service set_power_off");
    return -1;
}

Complete code

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <clopema_arm_navigation/ClopemaMotionPlan.h>
#include <arm_navigation_msgs/GetRobotState.h>
#include <arm_navigation_msgs/convert_messages.h>
#include <clopema_motoros/SetPowerOff.h>

bool getRobotState(arm_navigation_msgs::RobotState & rs) {
    ros::service::waitForService("/environment_server/get_robot_state");
    arm_navigation_msgs::GetRobotState r;
    if (!ros::service::call("/environment_server/get_robot_state", r)) {
        ROS_ERROR("Can't get current robot state.");
        return false;
    }
    rs = r.response.robot_state;
    return true;
}

arm_navigation_msgs::MotionPlanRequest createPlan(double sAxis) {
    arm_navigation_msgs::MotionPlanRequest req;
    req.group_name = "r1_arm";
    req.planner_id = std::string("");
    req.allowed_planning_time = ros::Duration(5.0);
    std::vector<std::string> names;
    names.push_back("r1_joint_s");
    names.push_back("r1_joint_l");
    names.push_back("r1_joint_u");
    names.push_back("r1_joint_r");
    names.push_back("r1_joint_b");
    names.push_back("r1_joint_t");
    req.goal_constraints.joint_constraints.resize(names.size());
    for (unsigned int i = 0; i < req.goal_constraints.joint_constraints.size(); ++i) {
        req.goal_constraints.joint_constraints[i].joint_name = names[i];
        req.goal_constraints.joint_constraints[i].position = 0.0;
        req.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        req.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
    }

    req.goal_constraints.joint_constraints[0].position = sAxis;

    return req;
}

int main(int argc, char **argv) {
    if (argc < 2) {
        ROS_ERROR("Not enough arguments: send_traj <R1sAxisDesiredPosition>");
        return -1;
    }
    ros::init(argc, argv, "send_traj");
    ros::NodeHandle nh;

    clopema_arm_navigation::ClopemaMotionPlan mp;
    std::stringstream argv1(argv[1]);
    mp.request.motion_plan_req = createPlan(atof(argv1.str().data()));

    if (!getRobotState(mp.request.motion_plan_req.start_state)) {
        ROS_ERROR("Can't get current robot state");
        return -1;
    }

    ros::service::waitForService("/clopema_planner/plan");
    if (!ros::service::call("/clopema_planner/plan", mp)) {
        ROS_ERROR("Can't call service clopema_planner/plan");
        return -1;
    }

    if (mp.response.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) {
        actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> move("/clopema_controller/follow_joint_trajectory", true);
        move.waitForServer();
        control_msgs::FollowJointTrajectoryGoal goal;
        goal.trajectory = mp.response.joint_trajectory;
        move.sendGoal(goal);
        bool finished_within_time = move.waitForResult(ros::Duration(45.0));
        if (!finished_within_time) {
            move.cancelGoal();
            ROS_INFO("Timed out achieving goal");
        } else {
            actionlib::SimpleClientGoalState state = move.getState();
            bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
            if (success)
                ROS_INFO("Action finished: %s", state.toString().c_str());
            else {
                ROS_INFO("Action failed: %s", state.toString().c_str());
                ROS_WARN("Addition information: %s", state.text_.c_str());
                control_msgs::FollowJointTrajectoryResult r;
                ROS_WARN("Error code: %d", move.getResult()->error_code);
            }
        }
    } else {
        ROS_WARN("Can't plan trajectory: %s", arm_navigation_msgs::armNavigationErrorCodeToString(mp.response.error_code).data());
    }

    //Set servo power off
    clopema_motoros::SetPowerOff soff;
    soff.request.force = false;
    ros::service::waitForService("/joint_trajectory_action/set_power_off");
    if (!ros::service::call("/joint_trajectory_action/set_power_off", soff)) {
        ROS_ERROR("Can't call service set_power_off");
        return -1;
    }
    return 1;
}