00001 #include "move_arm.h"
00002
00003 #include <map>
00004 #include <cstdarg>
00005
00006 std::vector<double> doubles2vector(double j1, double j2, double j3, double j4, double j5, double j6, double j7)
00007 {
00008 std::vector<double> joints;
00009 joints.push_back(j1);
00010 joints.push_back(j2);
00011 joints.push_back(j3);
00012 joints.push_back(j4);
00013 joints.push_back(j5);
00014 joints.push_back(j6);
00015 joints.push_back(j7);
00016 return joints;
00017 }
00018
00020 RobotArm::RobotArm()
00021 {
00022
00023 left_traj_client_ = new TrajClient("/l_arm_controller/joint_trajectory_action", true);
00024 right_traj_client_ = new TrajClient("/r_arm_controller/joint_trajectory_action", true);
00025
00026
00027 goals["left up"] = createGoal("l", doubles2vector(1.492, -0.150, -0.078, -1.152, 0.080, -0.286, -0.056));
00028 goals["right up"] = createGoal("r", doubles2vector(-1.474, -0.193, 0.013, -1.189, 6.444, -0.168, -0.113));
00029
00030 goals["left fwd"] = createGoal("l", doubles2vector(0.055, -0.081, 1.731, -0.011, -0.036, -0.009, 1.493));
00031 goals["right fwd"] = createGoal("r", doubles2vector(-0.096, -0.054, -1.438, -0.079, 6.306, -0.003, -1.684));
00032
00033 goals["left broad"] = createGoal("l", doubles2vector(1.566, 0.162, 3.049, -2.153, 0.262, -0.004, 1.490));
00034 goals["right broad"] = createGoal("r", doubles2vector(-1.468, 0.143, -2.904, -2.197, 6.353, -0.001, -1.684));
00035
00036 goals["left tuck"] = createGoal("l", doubles2vector(0.0, 1.296, -0.0, -2.121, -9.265, -2.0, -6.282));
00037 goals["right tuck"] = createGoal("r", doubles2vector(0.0, 1.296, -0.0, -2.121, 3.028, -2.0, 0.015));
00038
00039
00040 while(!left_traj_client_->waitForServer(ros::Duration(5.0))){
00041 ROS_INFO("Waiting for the (left) joint_trajectory_action server");
00042 }
00043 while(!right_traj_client_->waitForServer(ros::Duration(5.0))){
00044 ROS_INFO("Waiting for the (right) joint_trajectory_action server");
00045 }
00046 }
00047
00048
00050 RobotArm::~RobotArm()
00051 {
00052
00053 delete right_traj_client_;
00054 delete left_traj_client_;
00055 }
00056
00058 void RobotArm::startTrajectory(std::string goal_name)
00059 {
00060
00061 if(goals.find(goal_name) == goals.end()){
00062 ROS_ERROR_STREAM("Unknown Goal: " << goal_name);
00063 return;
00064 }
00065 goals[goal_name]->trajectory.header.stamp = ros::Time::now() + ros::Duration(3.0);
00066 if(goal_name[0] == 'l')
00067 left_traj_client_->sendGoal(*goals[goal_name]);
00068 else if(goal_name[0] == 'r')
00069 right_traj_client_->sendGoal(*goals[goal_name]);
00070 else
00071 ROS_ERROR("Something is wrong: %c %s", goal_name[0], goal_name.c_str());
00072
00073 }
00074
00076 actionlib::SimpleClientGoalState RobotArm::getState(char lr)
00077 {
00078
00079 if(lr == 'l'){
00080 if(!left_traj_client_->waitForResult(ros::Duration(10.0)))
00081 ROS_INFO("Action did not finish before the time out.");
00082 return left_traj_client_->getState();
00083 } else {
00084 if(!right_traj_client_->waitForResult(ros::Duration(10.0)))
00085 ROS_INFO("Action did not finish before the time out.");
00086 return right_traj_client_->getState();
00087 }
00088 }
00089
00090 pr2_controllers_msgs::JointTrajectoryGoal* RobotArm::createGoal(std::string lr, std::vector<double> positions)
00091 {
00092
00093 pr2_controllers_msgs::JointTrajectoryGoal* goal = new pr2_controllers_msgs::JointTrajectoryGoal();
00094
00095
00096 goal->trajectory.joint_names.push_back(lr+"_shoulder_pan_joint");
00097 goal->trajectory.joint_names.push_back(lr+"_shoulder_lift_joint");
00098 goal->trajectory.joint_names.push_back(lr+"_upper_arm_roll_joint");
00099 goal->trajectory.joint_names.push_back(lr+"_elbow_flex_joint");
00100 goal->trajectory.joint_names.push_back(lr+"_forearm_roll_joint");
00101 goal->trajectory.joint_names.push_back(lr+"_wrist_flex_joint");
00102 goal->trajectory.joint_names.push_back(lr+"_wrist_roll_joint");
00103
00104
00105 goal->trajectory.points.resize(1);
00106
00107
00108
00109 int ind = 0;
00110 goal->trajectory.points[ind].positions.resize(7);
00111 goal->trajectory.points[ind].positions.swap(positions);
00112
00113
00114 goal->trajectory.points[ind].velocities.resize(7);
00115 for (size_t j = 0; j < 7; ++j)
00116 {
00117 goal->trajectory.points[ind].velocities[j] = 0.0;
00118 }
00119
00120 goal->trajectory.points[ind].time_from_start = ros::Duration(1.0);
00121
00122
00123 return goal;
00124 }
00125