JointSpaceArmController.cpp

Go to the documentation of this file.

/*
 * JointSpaceArmController.cpp
 *
 *  Created on: Feb 9, 2012
 *      Author: hess
 */

#ifndef JOINTSPACEARMCONTROLLER_CPP_
#define JOINTSPACEARMCONTROLLER_CPP_

#include<coverage_3d_arm_navigation/JointSpaceArmController.h>
#include<trajectory_msgs/JointTrajectoryPoint.h>
#include <arm_navigation_msgs/FilterJointTrajectory.h>
#include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
#include <arm_navigation_msgs/SimplePoseConstraint.h>
#include <arm_navigation_msgs/MoveArmAction.h>
#include <arm_navigation_msgs/utils.h>
#include <coverage_3d_arm_navigation/iterative_smoother.h>
#include <std_srvs/Empty.h>
double denormalizeAngle(double angle1, double angle2) {

        Eigen::Vector3f v1 = Eigen::Vector3f(cos(angle1), sin(angle1), 0);
        Eigen::Vector3f v2 = Eigen::Vector3f(cos(angle2), sin(angle2), 0);
        Eigen::Vector3f cross_product = v1.cross(v2);

        cross_product.normalize();
        if (cross_product.norm() > 0.000000001) { //everything ok

                if (cross_product[2] > 0) {
                        //increment v2 until v2 larger v1
                        while (angle2 < angle1) {
                                angle2 = angle2 + 2 * M_PI;
                        }
                        while ((angle2 - angle1) > 2 * M_PI) {
                                angle2 = angle2 - 2 * M_PI;
                        }
                } else {
                        //increment v2 until v2 larger v1
                        while (angle2 > angle1) {
                                angle2 = angle2 - 2 * M_PI;
                        }
                        while ((angle2 - angle1) < -2 * M_PI) {
                                angle2 = angle2 + 2 * M_PI;
                        }

                }
        }
        return angle2;
}

//function needed to adapt to JointSplineTrajectory Controller --> turning always towards the smallest angle
void denormalizeJointTrajectory(const std::vector<std::vector<double> >& traj,
                std::vector<std::vector<double> >& res_traj) {

        res_traj.clear();
        for (unsigned int i = 0; i < traj.size(); ++i) {
                res_traj.push_back(traj[i]);
        }
        //  r_arm_joints_.push_back("r_shoulder_pan_joint");
        //  r_arm_joints_.push_back("r_shoulder_lift_joint");
        //  r_arm_joints_.push_back("r_upper_arm_roll_joint");
        //  r_arm_joints_.push_back("r_elbow_flex_joint");
        //  r_arm_joints_.push_back("r_forearm_roll_joint");
        //  r_arm_joints_.push_back("r_wrist_flex_joint");
        //  r_arm_joints_.push_back("r_wrist_roll_joint");
        for (unsigned int i = 1; i < res_traj.size(); ++i) {

                res_traj[i][4] = denormalizeAngle(res_traj[i - 1][4], res_traj[i][4]);
                res_traj[i][6] = denormalizeAngle(res_traj[i - 1][6], res_traj[i][6]);
        }

}

JointSpaceArmController::JointSpaceArmController(ros::NodeHandle& nh) :
                nh_(nh) {

        start_r_arm_.resize(7);
        start_r_arm_[0] = -1.68;
        start_r_arm_[1] = 0.052;
        start_r_arm_[2] = -1.48;
        start_r_arm_[3] = -1.65;
        start_r_arm_[4] = -1.60;
        start_r_arm_[5] = -1.57;
        start_r_arm_[6] = -3.02;

        r_arm_joints_.push_back("r_shoulder_pan_joint");
        r_arm_joints_.push_back("r_shoulder_lift_joint");
        r_arm_joints_.push_back("r_upper_arm_roll_joint");
        r_arm_joints_.push_back("r_elbow_flex_joint");
        r_arm_joints_.push_back("r_forearm_roll_joint");
        r_arm_joints_.push_back("r_wrist_flex_joint");
        r_arm_joints_.push_back("r_wrist_roll_joint");

        l_arm_joints_.push_back("l_shoulder_pan_joint");
        l_arm_joints_.push_back("l_shoulder_lift_joint");
        l_arm_joints_.push_back("l_upper_arm_roll_joint");
        l_arm_joints_.push_back("l_elbow_flex_joint");
        l_arm_joints_.push_back("l_forearm_roll_joint");
        l_arm_joints_.push_back("l_wrist_flex_joint");
        l_arm_joints_.push_back("l_wrist_roll_joint");

        traj_client_ = new actionlib::SimpleActionClient<
                        pr2_controllers_msgs::JointTrajectoryAction>(
                        "/r_arm_controller/joint_trajectory_action", true);
//      ros::service::waitForService("trajectory_filter/filter_trajectory");
        filter_trajectory_client_ = nh_.serviceClient<
                        arm_navigation_msgs::FilterJointTrajectory>(
                        "/trajectory_filter_smoother/filter_trajectory");
        reset_collider_client_ = nh_.serviceClient<std_srvs::Empty>(
                        "/collider_node/reset");
}

JointSpaceArmController::~JointSpaceArmController() {
        delete traj_client_;
}

void JointSpaceArmController::followEntireTrajectory(
                const std::vector<std::vector<double> >& path, const std::string arm,
                const std::vector<unsigned int>&action,
                const std::vector<tf::Pose>& pose_path,
                std::vector<tf::Pose>& touch_points_allowed) {
        ROS_DEBUG_STREAM(
                        "JointSpaceArmController::followEntireTrajectory: " << path.size() << " " << action.size());
        ROS_ASSERT_MSG(path.size()==action.size(),
                        "Path size does not match action size.");
        ROS_ASSERT_MSG(arm=="right_arm",
                        "Currently only the right arm is supported.");

        //std_srvs::Empty collider_reset;
        //reset_collider_client_.call(collider_reset);
        std::vector<tf::Pose> tmp_touch_points;
        ROS_INFO_STREAM("JointSpaceArmController: Move arm to start");
        bool move_success = true;
        //bool move_success = moveArmToJointGoalUsingPlanner(arm, start_r_arm_, false,
        //tmp_touch_points); //move arm to start pose
        //ROS_WARN("POSE 1");
        //      tmp_touch_points.push_back(touch_points_allowed[0]);
        ROS_INFO_STREAM(
                        "JointSpaceArmController: Move arm to first trajectory point");
        move_success = moveArmToJointGoalUsingPlanner("right_arm", path[0], false,
                        tmp_touch_points); // move arm planned to first pose in the graph
        ROS_WARN("POSE 2");
        if (move_success) {

                tmp_touch_points.clear();

                std::vector<std::vector<double> > tmp_path;
                std::vector<tf::Pose> tmp_pose_path;
                for (unsigned int i = 0; i < path.size() - 1; ++i) {
                        ROS_DEBUG_STREAM(
                                        "JointSpaceArmController::followEntireTrajectory: Action  " << i << "  " << action[i]);
                }

                for (unsigned int i = 0; i < path.size() - 1; ++i) {
                        ROS_DEBUG_STREAM(
                                        "JointSpaceArmController::followEntireTrajectory: Action  " << i << "  " << action[i]);
                        if (action[i] == 0) { //
                                tmp_path.push_back(path[i + 1]);
                                tmp_pose_path.push_back(pose_path[i + 1]);
                        } else if (action[i] == 1) { //
                                if (tmp_path.size() > 0) {
                                        followTrajectory(tmp_path, tmp_pose_path, "right_arm");
                                        tmp_path.clear();
                                        tmp_pose_path.clear();
                                }
                                tmp_touch_points.push_back(touch_points_allowed[i]);
                                tmp_touch_points.push_back(touch_points_allowed[i + 1]);
                                move_success = moveArmToJointGoalUsingPlanner("right_arm",
                                                path[i + 1], false, tmp_touch_points);
                                if (!move_success) {
                                        return;
                                }
                                tmp_path.push_back(path[i + 1]);
                                tmp_pose_path.push_back(pose_path[i + 1]);
                                tmp_touch_points.clear();
                        }
                }
                if (tmp_path.size() > 0) {
                        followTrajectory(tmp_path, tmp_pose_path, "right_arm");
                }
                tmp_touch_points.push_back(
                                touch_points_allowed[touch_points_allowed.size() - 1]);
                moveArmToJointGoalUsingPlanner(arm, start_r_arm_, false,
                                tmp_touch_points);
        }
}

bool JointSpaceArmController::followEntireTrajectory(
                const std::vector<std::vector<double> >& path, const std::string arm,
                const std::vector<unsigned int>&action,
                const std::vector<tf::Pose>& pose_path,
                std::vector<tf::Pose>& touch_points_allowed,
                std::vector<tf::Pose>& poses_wide_above_surface) {
        ROS_DEBUG_STREAM(
                        "JointSpaceArmController::followEntireTrajectory: " << path.size() << " " << action.size());
        ROS_ASSERT_MSG(path.size()==action.size(),
                        "Path size does not match action size.");
        ROS_ASSERT_MSG(arm=="right_arm",
                        "Currently only the right arm is supported.");

//      ROS_INFO("Collider reset");
//      std_srvs::Empty collider_reset;
        //reset_collider_client_.call(collider_reset);
        std::vector<tf::Pose> tmp_touch_points;

        ROS_INFO("Moving arm to start");
        //bool move_success = moveArmToJointGoalUsingPlanner(arm, start_r_arm_, true,tmp_touch_points);
        //remove arm from surface
        ROS_INFO("Moving arm to start");
        bool move_success = moveArmToPoseGoalUsingPlanner("right_arm",
                        poses_wide_above_surface[1], true);
        ROS_INFO("Moving arm to first trajectory point");
        move_success = move_success
                        && moveArmToJointGoalUsingPlanner("right_arm", path[1], false,
                                        tmp_touch_points); // move arm planned to first pose in the graph

                        //we have reached the first position of the coverage path
        if (move_success) {

                std::vector<std::vector<double> > tmp_path;
                std::vector<tf::Pose> tmp_pose_path;

                for (unsigned int i = 1; i < path.size() - 1; ++i) {
                        tmp_path.push_back(path[i]);
                        tmp_pose_path.push_back(pose_path[i]);
                }

                followTrajectory(tmp_path, tmp_pose_path, "right_arm");

                ROS_INFO("Moving arm up");
                tmp_touch_points.clear();
                //remove arm to the start position
                tmp_touch_points.push_back(
                                touch_points_allowed[touch_points_allowed.size() - 2]);
                move_success = move_success
                                && moveArmToPoseGoalUsingPlanner("right_arm",
                                                poses_wide_above_surface[poses_wide_above_surface.size()
                                                                - 2], false);

//              ROS_INFO("Moving arm to start");
//              move_success = move_success && moveArmToJointGoalUsingPlanner(arm, start_r_arm_, true,
//                              tmp_touch_points);
        }
        return move_success;
}

void JointSpaceArmController::followTrajectory(
                const std::vector<std::vector<double> >& path2,
                const std::vector<tf::Pose>& pose_traj, const std::string arm) {

        ROS_INFO("followTrajectory: Following coverage trajectory.");
        std::string action_name;
        std::vector<std::string> joint_names;
        if (arm == "right_arm") {
                joint_names = r_arm_joints_;
                action_name = "/r_arm_controller/joint_trajectory_action";
        } else if (arm == "left_arm") {
                joint_names = l_arm_joints_;
                action_name = "/l_arm_controller/joint_trajectory_action";
        }

        std::vector<std::vector<double> > traj;
        denormalizeJointTrajectory(path2, traj);

        bool compute_vel = true;
        std::vector<std::vector<double> > traj_vel;
        std::vector<double> traj_time;

        //parameter values for the spline smoother
        double scale_vel = 1.0 / 2.0;
        double scale_time = (1.0 / scale_vel) + (0.5 * (1.0 / scale_vel));

        arm_navigation_msgs::FilterJointTrajectory::Request req;
        arm_navigation_msgs::FilterJointTrajectory::Request req2;
        arm_navigation_msgs::FilterJointTrajectory::Response res;

        arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request r1;
        arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request r2;
        double time_diff = 2.0;
        if (compute_vel) {
                traj_planner_.planTrajectoryVelocity(traj, pose_traj, traj_vel,
                                traj_time, scale_time, scale_vel);

                req.trajectory.joint_names = joint_names;
                r1.trajectory.joint_names = joint_names;

                for (unsigned int i = 0; i < traj.size(); i++) {
                        trajectory_msgs::JointTrajectoryPoint tmp;
                        //currently only set positions
                        tmp.positions = traj[i];
                        tmp.velocities = traj_vel[i];
                        tmp.time_from_start = ros::Duration(traj_time[i]);

                        tmp.accelerations = std::vector<double>(7, 0);
//                      tmp.velocities = std::vector<double> (7,0); //traj_vel[i];
//                      tmp.time_from_start = ros::Duration((i + 1) * time_diff); //ros::Duration(traj_time[i]);
                        req.trajectory.points.push_back(tmp);
                        r1.trajectory.points.push_back(tmp);
                }
                r1.limits.resize(joint_names.size());
                for (int i = 0; i < joint_names.size(); ++i) {
                        r1.limits[i].has_velocity_limits = true;
                        r1.limits[i].max_velocity = 1.0;
                        r1.limits[i].has_acceleration_limits = true;
                        r1.limits[i].max_acceleration = 0.3;
                        std::cout << joint_names[i] << std::endl;
                }

                IterativeParabolicSmoother<
                                arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request> smoother;
                smoother.smooth(r1, r2);

//              req.allowed_time = ros::Duration(5.0);
//
//              if (filter_trajectory_client_.call(req, res)) {
//                      if (res.error_code.val == res.error_code.SUCCESS) {
//                              ROS_INFO("Requested trajectory was filtered");
//                      } else
//                              ROS_INFO("Requested trajectory was not filtered. Error code: %d", res.error_code.val);
//              }
        }

        //construct the client
        ROS_INFO("Waiting for action server to start.");

        // wait for the action server to start
        traj_client_->waitForServer(); //will wait for infinite time

        //our goal variable
        pr2_controllers_msgs::JointTrajectoryGoal goal;
        // First, the joint names, which apply to all waypoints
        goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(2.0);
        goal.trajectory.joint_names = joint_names;
        goal.trajectory = r2.trajectory;
//      goal.trajectory = req.trajectory;
//      goal.trajectory = res.trajectory;
//      // We will have two waypoints in this goal trajectory
//      goal.trajectory.points.resize(traj.size());
//
//      //fill the goal
//      double time_diff = 2.0;
//      trajectory_msgs::JointTrajectoryPoint tmp;
//
//      for (unsigned int i = 0; i < traj.size(); ++i) {
//
//              //currently only set positions
//              tmp.positions = traj[i];
//              if (compute_vel) {
//                      tmp.velocities = traj_vel[i];
//                      tmp.time_from_start = ros::Duration(traj_time[i]);
//              } else {
//                      tmp.velocities.resize(7, 0);
//                      tmp.time_from_start = ros::Duration((i + 1) * time_diff);
//              }
//              goal.trajectory.points[i] = tmp;
//      }

        //send the goal and wait for the result
        traj_client_->sendGoal(goal);

        //wait for the action to return
        bool exec_okay = traj_client_->waitForResult();

        if (exec_okay) {
                actionlib::SimpleClientGoalState state = traj_client_->getState();
                ROS_INFO("Action finished: %s", state.toString().c_str());
        } else
                ROS_INFO("Action did not finish before the time out.");
}

bool JointSpaceArmController::moveArmToJointGoalUsingPlanner(
                const std::string arm, const std::vector<double> & state,
                const bool collision_check,
                std::vector<tf::Pose>& touch_points_allowed) {

        double planning_tolerance = 0.02;
        int n_joints = state.size();
        std::vector<std::string> joint_names;
        if (arm == "right_arm") {
                joint_names = r_arm_joints_;
        } else if (arm == "left_arm") {
                joint_names = l_arm_joints_;
        }
        std::string action_name("/move_");
        action_name.append(arm);

        ROS_INFO("moveArmToJointGoalUsingPlanner: connecting to move arm action");
        actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(
                        action_name, true);
        move_arm.waitForServer();
        ROS_INFO("moveArmToJointGoalUsingPlanner: Connected to move_arm server");
        arm_navigation_msgs::MoveArmGoal arm_goal;

        arm_goal.motion_plan_request.group_name = arm;
        arm_goal.motion_plan_request.num_planning_attempts = 3;
        arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
        arm_goal.motion_plan_request.planner_id = std::string("");
        arm_goal.planner_service_name = "/ompl_planning/plan_kinematic_path";
        arm_goal.accept_invalid_goals = false;
        arm_goal.accept_partial_plans = false;
        arm_goal.motion_plan_request.goal_constraints.joint_constraints.resize(
                        n_joints);

        //attach sponge
        arm_navigation_msgs::AttachedCollisionObject att_object;
        att_object.link_name = "r_gripper_r_finger_tip_link";

        att_object.object.id = "attached_sponge";
        att_object.object.operation.operation =
                        arm_navigation_msgs::CollisionObjectOperation::ADD;

        att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
        att_object.object.header.stamp = ros::Time::now();
        arm_navigation_msgs::Shape object;
        object.type = arm_navigation_msgs::Shape::BOX;
        object.dimensions.resize(3);
        object.dimensions[0] = .05;
        object.dimensions[1] = .05;
        object.dimensions[2] = .05;

        geometry_msgs::Pose pose;
        pose.position.x = 0.0;
        pose.position.y = 0.0;
        pose.position.z = 0.0;
        pose.orientation.x = 0;
        pose.orientation.y = 0;
        pose.orientation.z = 0;
        pose.orientation.w = 1;

        att_object.object.shapes.push_back(object);
        att_object.object.poses.push_back(pose);
        att_object.touch_links.push_back("r_end_effector");

//      arm_goal.planning_scene_diff.attached_collision_objects.push_back(att_object);

        for (unsigned int i = 0; i < touch_points_allowed.size(); ++i) {

                //allowed contacts
                arm_navigation_msgs::AllowedContactSpecification all_contact;
                all_contact.name = "start_contact";
                arm_navigation_msgs::Shape object1;
                object1.type = arm_navigation_msgs::Shape::SPHERE;
                object1.dimensions.resize(1);
                object1.dimensions[0] = .03;
//              object1.dimensions[1] = .05;
//              object1.dimensions[2] = .05;
                all_contact.shape = object1;
                geometry_msgs::PoseStamped pose_stamped;
                tf::poseTFToMsg(touch_points_allowed[i], pose_stamped.pose);
                pose_stamped.header.frame_id = "/torso_lift_link";

                all_contact.pose_stamped = pose_stamped;
                all_contact.penetration_depth = 0.03;
                all_contact.link_names.push_back("r_gripper_r_finger_tip_link");
                all_contact.link_names.push_back("collision_map");
                arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
                all_contact.link_names.clear();

                all_contact.link_names.push_back("r_gripper_l_finger_tip_link");
                all_contact.link_names.push_back("collision_map");
                arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
                all_contact.link_names.clear();

                all_contact.link_names.push_back("r_gripper_l_finger_link");
                all_contact.link_names.push_back("collision_map");
                arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
                all_contact.link_names.clear();

                all_contact.link_names.push_back("r_gripper_r_finger_link");
                all_contact.link_names.push_back("collision_map");
                arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
                all_contact.link_names.clear();

//              all_contact.link_names.push_back("attached_sponge");
//              all_contact.link_names.push_back("collision_map");
//              arm_goal.planning_scene_diff.allowed_contacts.push_back(all_contact);
//              all_contact.link_names.clear();
        }

        if (!collision_check) {
                arm_navigation_msgs::CollisionOperation op;
                op.operation = op.DISABLE;

                op.object1 = "collision_map";
                op.object2 = "r_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object1 = "collision_map";
                op.object2 = "r_gripper_r_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object2 = "collision_map";
                op.object1 = "r_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object2 = "collision_map";
                op.object1 = "r_gripper_r_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object1 = "collision_map";
                op.object2 = "l_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object1 = "collision_map";
                op.object2 = "l_gripper_r_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object2 = "collision_map";
                op.object1 = "l_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

//              op.object2 = "collision_map";
//              op.object1 = "attached_sponge";
//              arm_goal.operations.collision_operations.push_back(op);
//
//              op.object1 = "collision_map";
//              op.object2 = "attached_sponge";
//              arm_goal.operations.collision_operations.push_back(op);

                op.object1 = op.COLLISION_SET_ALL;
                op.object2 = op.COLLISION_SET_ALL;
                arm_goal.operations.collision_operations.push_back(op);
        }

        // set desired values for goal
        for (int i = 0; i < n_joints; i++) {
                arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name =
                                joint_names[i];
                arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].position =
                                state[i];
                arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below =
                                planning_tolerance;
                arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above =
                                planning_tolerance;
        }

        // connect to the planning server
        ROS_INFO("moveArmToJointGoalUsingPlanner: Waiting for move_arm server");
        move_arm.waitForServer();
        ROS_INFO("moveArmToJointGoalUsingPlanner: Connected to move_arm server");

        // send goal to server
        move_arm.sendGoal(arm_goal);

        // and wait for some time for completion of task
        bool finished_within_time = move_arm.waitForResult();

        // check whether task was fulfilled in time
        if (!finished_within_time) {
                move_arm.cancelGoal();
                actionlib::SimpleClientGoalState state = move_arm.getState();
                ROS_INFO_STREAM(
                                "moveArmToJointGoalUsingPlanner: " << state.toString());
                return false;
        } else {
                actionlib::SimpleClientGoalState state = move_arm.getState();

                if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
                        ROS_INFO(
                                        "moveArmToJointGoalUsingPlanner: %s reached goal.", arm.c_str());
                        return true;
                } else {
                        ROS_INFO("moveArmToJointGoalUsingPlanner: Something went wrong:");
                        ROS_INFO("%s status: %s", arm.c_str(), state.toString().c_str());
                        return false;
                }
        }

        return true;

}

bool JointSpaceArmController::moveArmToPoseGoalUsingPlanner(
                const std::string arm, const tf::Pose& pose,
                const bool collision_check) {

        double planning_tolerance = 0.02;
        std::vector<std::string> joint_names;
        if (arm == "right_arm") {
                joint_names = r_arm_joints_;
        } else if (arm == "left_arm") {
                joint_names = l_arm_joints_;
        }
        std::string action_name("/move_");
        action_name.append(arm);

        ROS_INFO("moveArmToPoseGoalUsingPlanner: connecting to move arm action");
        actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(
                        action_name, true);
        move_arm.waitForServer();
        ROS_INFO("moveArmToPoseGoalUsingPlanner: connected to move_arm server");
        arm_navigation_msgs::MoveArmGoal arm_goal;

        arm_goal.motion_plan_request.group_name = arm;
        arm_goal.motion_plan_request.num_planning_attempts = 3;
        arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
        arm_goal.motion_plan_request.planner_id = std::string("");
        arm_goal.planner_service_name = "/ompl_planning/plan_kinematic_path";

        if (!collision_check) {
                arm_navigation_msgs::CollisionOperation op;
                op.operation = op.DISABLE;

                op.object1 = "collision_map";
                op.object2 = "r_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object1 = "collision_map";
                op.object2 = "r_gripper_r_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object2 = "collision_map";
                op.object1 = "r_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object2 = "collision_map";
                op.object1 = "r_gripper_r_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object1 = "collision_map";
                op.object2 = "l_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object1 = "collision_map";
                op.object2 = "l_gripper_r_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

                op.object2 = "collision_map";
                op.object1 = "l_gripper_l_finger_tip_link";
                arm_goal.operations.collision_operations.push_back(op);

//              op.object2 = "collision_map";
//              op.object1 = "attached_sponge";
//              arm_goal.operations.collision_operations.push_back(op);
//
//              op.object1 = "collision_map";
//              op.object2 = "attached_sponge";
//              arm_goal.operations.collision_operations.push_back(op);

                op.object1 = op.COLLISION_SET_ALL;
                op.object2 = op.COLLISION_SET_ALL;
                arm_goal.operations.collision_operations.push_back(op);
        }

        arm_navigation_msgs::SimplePoseConstraint desired_pose;
        desired_pose.header.frame_id = "torso_lift_link";
        desired_pose.link_name = "r_wrist_roll_link";

        tf::Pose tmp_pose = pose;
        tf::Quaternion q = tmp_pose.getRotation();
        q.normalize();
        tmp_pose.setRotation(q);
        tf::poseTFToMsg(tmp_pose, desired_pose.pose);
        /*
         desired_pose.absolute_position_tolerance.x = 0.02;
         desired_pose.absolute_position_tolerance.y = 0.02;
         desired_pose.absolute_position_tolerance.z = 0.02;
         desired_pose.absolute_roll_tolerance = 0.04;
         desired_pose.absolute_pitch_tolerance = 0.04;
         desired_pose.absolute_yaw_tolerance = 0.04;
         */
        desired_pose.absolute_position_tolerance.x = 0.03;
        desired_pose.absolute_position_tolerance.y = 0.03;
        desired_pose.absolute_position_tolerance.z = 0.03;
        desired_pose.absolute_roll_tolerance = 0.2;
        desired_pose.absolute_pitch_tolerance = 0.2;
        desired_pose.absolute_yaw_tolerance = 0.2;

        arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose, arm_goal);

        bool finished_within_time = false;
        move_arm.sendGoal(arm_goal);
        finished_within_time = move_arm.waitForResult(ros::Duration(20.0));
        if (!finished_within_time) {
                move_arm.cancelGoal();
                ROS_INFO(
                                "moveArmToPoseGoalUsingPlanner: Timed out achieving pose goal");
        } else {
                actionlib::SimpleClientGoalState state = move_arm.getState();
                bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
                if (success) {
                        ROS_INFO(
                                        "moveArmToPoseGoalUsingPlanner: Pose Goal Action finished: %s", state.toString().c_str());
                        return true;
                } else {
                        ROS_INFO(
                                        "moveArmToPoseGoalUsingPlanner: Pose Goal Action failed: %s", state.toString().c_str());
                        return false;
                }
        }

        return true;
}

#endif /* JOINTSPACEARMCONTROLLER_CPP_ */