move_arm.cpp

Go to the documentation of this file.

#include "move_arm.h"

#include <map>
#include <cstdarg>

std::vector<double> doubles2vector(double j1, double j2, double j3, double j4, double j5, double j6, double j7)
{
  std::vector<double> joints;
  joints.push_back(j1);
  joints.push_back(j2);
  joints.push_back(j3);
  joints.push_back(j4);
  joints.push_back(j5);
  joints.push_back(j6);
  joints.push_back(j7);
  return joints;
}

RobotArm::RobotArm() 
{
  // tell the action client that we want to spin a thread by default
  left_traj_client_ = new TrajClient("/l_arm_controller/joint_trajectory_action", true);
  right_traj_client_ = new TrajClient("/r_arm_controller/joint_trajectory_action", true);

  
  goals["left up"]  = createGoal("l", doubles2vector(1.492, -0.150, -0.078, -1.152, 0.080, -0.286, -0.056));
  goals["right up"] = createGoal("r", doubles2vector(-1.474, -0.193, 0.013, -1.189, 6.444, -0.168, -0.113));

  goals["left fwd"]  = createGoal("l", doubles2vector(0.055, -0.081, 1.731, -0.011, -0.036, -0.009, 1.493));
  goals["right fwd"] = createGoal("r", doubles2vector(-0.096, -0.054, -1.438, -0.079, 6.306, -0.003, -1.684));

  goals["left broad"]  = createGoal("l", doubles2vector(1.566, 0.162, 3.049, -2.153, 0.262, -0.004, 1.490));
  goals["right broad"] = createGoal("r", doubles2vector(-1.468, 0.143, -2.904, -2.197, 6.353, -0.001, -1.684));

  goals["left tuck"]  = createGoal("l", doubles2vector(0.0, 1.296, -0.0, -2.121, -9.265, -2.0, -6.282));
  goals["right tuck"] = createGoal("r", doubles2vector(0.0, 1.296, -0.0, -2.121, 3.028, -2.0, 0.015));

  // wait for action server to come up
  while(!left_traj_client_->waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the (left) joint_trajectory_action server");
  }
  while(!right_traj_client_->waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the (right) joint_trajectory_action server");
  }
}


RobotArm::~RobotArm()
{
  //TODO: Delete all goals in the maps 
  delete right_traj_client_;
  delete left_traj_client_;
}

void RobotArm::startTrajectory(std::string goal_name)
{
  // When to start the trajectory: 1s from now
  if(goals.find(goal_name) == goals.end()){
    ROS_ERROR_STREAM("Unknown Goal: " << goal_name);
    return;
  }
  goals[goal_name]->trajectory.header.stamp = ros::Time::now() + ros::Duration(3.0);
  if(goal_name[0] == 'l')
    left_traj_client_->sendGoal(*goals[goal_name]);
  else if(goal_name[0] == 'r')
    right_traj_client_->sendGoal(*goals[goal_name]);
  else
    ROS_ERROR("Something is wrong: %c %s", goal_name[0], goal_name.c_str());
   
}

actionlib::SimpleClientGoalState RobotArm::getState(char lr)
{

  if(lr == 'l'){
    if(!left_traj_client_->waitForResult(ros::Duration(10.0)))
      ROS_INFO("Action did not finish before the time out.");
    return left_traj_client_->getState();
  } else {
    if(!right_traj_client_->waitForResult(ros::Duration(10.0)))
      ROS_INFO("Action did not finish before the time out.");
    return right_traj_client_->getState();
  }
}

pr2_controllers_msgs::JointTrajectoryGoal* RobotArm::createGoal(std::string lr, std::vector<double> positions)
{
  //our goal variable
  pr2_controllers_msgs::JointTrajectoryGoal* goal = new pr2_controllers_msgs::JointTrajectoryGoal();

  // First, the joint names, which apply to all waypoints
  goal->trajectory.joint_names.push_back(lr+"_shoulder_pan_joint");
  goal->trajectory.joint_names.push_back(lr+"_shoulder_lift_joint");
  goal->trajectory.joint_names.push_back(lr+"_upper_arm_roll_joint");
  goal->trajectory.joint_names.push_back(lr+"_elbow_flex_joint");
  goal->trajectory.joint_names.push_back(lr+"_forearm_roll_joint");
  goal->trajectory.joint_names.push_back(lr+"_wrist_flex_joint");
  goal->trajectory.joint_names.push_back(lr+"_wrist_roll_joint");

  // We will have two waypoints in this goal trajectory
  goal->trajectory.points.resize(1);

  // First trajectory point
  // Positions
  int ind = 0;
  goal->trajectory.points[ind].positions.resize(7);
  goal->trajectory.points[ind].positions.swap(positions);
  
  // Velocities
  goal->trajectory.points[ind].velocities.resize(7);
  for (size_t j = 0; j < 7; ++j)
  {
    goal->trajectory.points[ind].velocities[j] = 0.0;
  }
  // To be reached 1 second after starting along the trajectory
  goal->trajectory.points[ind].time_from_start = ros::Duration(1.0);

  //we are done; return the goal
  return goal;
}