Move home

The node will set whole robot (both arms and ext axis) to the home position - all axes set to 0.0 rad. First, the dual arm planner is used to plan trajectory which is then executed. In the next step the ext axis trajectory is planned and executed. With ClopemaMove class the source code become smaller and more readable but when adding node to CMakeList.txt it is necessary to use the following way:

rosbuild_add_executable(move_home src/move_home.cpp)
target_link_libraries(move_home clopema_move)

Compete source code

#include "clopema_motoros/ClopemaMove.h" 

using namespace std;

int main(int argc, char **argv) {
    ros::init(argc, argv, "move_home");
    ros::NodeHandle nh;

    /* --------------------------------------------------------
     * Set both arms to the home position using dual arm joint planner (group = arms)
     * --------------------------------------------------------*/
    ClopemaMove cmove;
    clopema_arm_navigation::ClopemaMotionPlan mp;
    mp.request.motion_plan_req.group_name = "arms";
    mp.request.motion_plan_req.allowed_planning_time = ros::Duration(5.0);
    if (!cmove.getRobotState(mp.request.motion_plan_req.start_state))
        return -1;
    vector<string> joint_names = cmove.getJointsInGroup(mp.request.motion_plan_req.group_name);
    mp.request.motion_plan_req.goal_constraints.joint_constraints.resize(joint_names.size());
    for (unsigned int i = 0; i < mp.request.motion_plan_req.goal_constraints.joint_constraints.size(); ++i) {
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].joint_name = joint_names[i];
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].position = 0.0;
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
    }
    ROS_INFO("Planning");
    if (!cmove.plan(mp))
        return -1;
    ROS_INFO("Executing");
    control_msgs::FollowJointTrajectoryGoal goal;
    goal.trajectory = mp.response.joint_trajectory;
    cmove.doGoal(goal);

    /* --------------------------------------------------------
     * Set ext axis to the home position
     * --------------------------------------------------------*/
    mp.request.motion_plan_req.group_name = "ext_axis";
    mp.request.motion_plan_req.allowed_planning_time = ros::Duration(5.0);
    if (!cmove.getRobotState(mp.request.motion_plan_req.start_state))
        return -1;
    joint_names = cmove.getJointsInGroup(mp.request.motion_plan_req.group_name);
    mp.request.motion_plan_req.goal_constraints.joint_constraints.resize(joint_names.size());
    for (unsigned int i = 0; i < mp.request.motion_plan_req.goal_constraints.joint_constraints.size(); ++i) {
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].joint_name = joint_names[i];
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].position = 0.0;
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        mp.request.motion_plan_req.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
    }
    ROS_INFO("Planning");
    if (!cmove.plan(mp))
        return -1;
    ROS_INFO("Executing");
    goal.trajectory = mp.response.joint_trajectory;
    cmove.doGoal(goal);

    ros::shutdown();
    return 1;
}

Modification

The code below shows how we can change goal position of any axis. To try it, put that code before cmove.plan() command (line nu. 27 if you want to modify robots axis and line nu. 50 if you want to modify external axis). This modification (but with desired position setting from first command line argument) are stored in nodes move_joint_arms and move_joint_ext in clopema_planning_tutorials package.

double desPos = 0.5; //desired position
vector<string>::iterator posIt;
if ((posIt = find(joint_names.begin(), joint_names.end(), "r1_joint_r")) != joint_names.end()) {
    mp.request.motion_plan_req.goal_constraints.joint_constraints.at(std::distance(joint_names.begin(), posIt)).position = desPos;
}
if ((posIt = find(joint_names.begin(), joint_names.end(), "r2_joint_r")) != joint_names.end()) {
    mp.request.motion_plan_req.goal_constraints.joint_constraints.at(std::distance(joint_names.begin(), posIt)).position = desPos;
}

double desPos = 0.5; //desired position
vector<string>::iterator posIt;
if ((posIt = find(joint_names.begin(), joint_names.end(), "ext_axis")) != joint_names.end()) {
    mp.request.motion_plan_req.goal_constraints.joint_constraints.at(std::distance(joint_names.begin(), posIt)).position = desPos;
}