00001
00013 #include "sr_mechanism_controllers/joint_spline_trajectory_action_controller.hpp"
00014 #include <pr2_mechanism_msgs/ListControllers.h>
00015 #include <sr_utilities/getJointState.h>
00016 #include <std_msgs/Float64.h>
00017 #include <sr_robot_msgs/sendupdate.h>
00018
00019 namespace shadowrobot
00020 {
00021
00022
00023
00024
00025
00026 static inline void generatePowers(int n, double x, double* powers)
00027 {
00028 powers[0] = 1.0;
00029
00030 for (int i=1; i<=n; i++)
00031 {
00032 powers[i] = powers[i-1]*x;
00033 }
00034 }
00035
00036 static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
00037 double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
00038 {
00039 coefficients.resize(6);
00040
00041 if (time == 0.0)
00042 {
00043 coefficients[0] = end_pos;
00044 coefficients[1] = end_vel;
00045 coefficients[2] = 0.5*end_acc;
00046 coefficients[3] = 0.0;
00047 coefficients[4] = 0.0;
00048 coefficients[5] = 0.0;
00049 }
00050 else
00051 {
00052 double T[6];
00053 generatePowers(5, time, T);
00054
00055 coefficients[0] = start_pos;
00056 coefficients[1] = start_vel;
00057 coefficients[2] = 0.5*start_acc;
00058 coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
00059 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
00060 coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
00061 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
00062 coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
00063 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
00064 }
00065 }
00069 static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
00070 double& position, double& velocity, double& acceleration)
00071 {
00072
00073 double t[6];
00074 generatePowers(5, time, t);
00075
00076 position = t[0]*coefficients[0] +
00077 t[1]*coefficients[1] +
00078 t[2]*coefficients[2] +
00079 t[3]*coefficients[3] +
00080 t[4]*coefficients[4] +
00081 t[5]*coefficients[5];
00082
00083 velocity = t[0]*coefficients[1] +
00084 2.0*t[1]*coefficients[2] +
00085 3.0*t[2]*coefficients[3] +
00086 4.0*t[3]*coefficients[4] +
00087 5.0*t[4]*coefficients[5];
00088
00089 acceleration = 2.0*t[0]*coefficients[2] +
00090 6.0*t[1]*coefficients[3] +
00091 12.0*t[2]*coefficients[4] +
00092 20.0*t[3]*coefficients[5];
00093 }
00094 static void getCubicSplineCoefficients(double start_pos, double start_vel,
00095 double end_pos, double end_vel, double time, std::vector<double>& coefficients)
00096 {
00097 coefficients.resize(4);
00098
00099 if (time == 0.0)
00100 {
00101 coefficients[0] = end_pos;
00102 coefficients[1] = end_vel;
00103 coefficients[2] = 0.0;
00104 coefficients[3] = 0.0;
00105 }
00106 else
00107 {
00108 double T[4];
00109 generatePowers(3, time, T);
00110
00111 coefficients[0] = start_pos;
00112 coefficients[1] = start_vel;
00113 coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
00114 coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
00115 }
00116 }
00117
00118 JointTrajectoryActionController::JointTrajectoryActionController() :
00119 nh_tilde("~"),use_sendupdate(false)
00120 {
00121 using namespace XmlRpc;
00122 action_server = boost::shared_ptr<JTAS> (new JTAS("/r_arm_controller/joint_trajectory_action",
00123 boost::bind(&JointTrajectoryActionController::execute_trajectory, this, _1),
00124 false ));
00125
00126
00127 std::vector<std::string> joint_labels;
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 joint_labels.push_back("ShoulderJRotate");
00160 joint_labels.push_back("ShoulderJSwing");
00161 joint_labels.push_back("ElbowJSwing");
00162 joint_labels.push_back("ElbowJRotate");
00163 joint_labels.push_back("WRJ2");
00164 joint_labels.push_back("WRJ1");
00165
00166
00167 for(unsigned int i=0;i<joint_labels.size();i++)
00168 {
00169 joint_state_idx_map[joint_labels[i]]=i;
00170 }
00171
00172
00173 if( ros::service::waitForService("sr_controller_manager/list_controllers",20000) )
00174 {
00175 use_sendupdate=false;
00176 ros::ServiceClient controller_list_client = nh.serviceClient<pr2_mechanism_msgs::ListControllers>("sr_controller_manager/list_controllers");
00177 pr2_mechanism_msgs::ListControllers controller_list;
00178 std::string controlled_joint_name;
00179
00180
00181 controller_list_client.call(controller_list);
00182
00183 for (unsigned int i=0; i<controller_list.response.controllers.size() ; i++ )
00184 {
00185 if(controller_list.response.state[i]=="running")
00186 {
00187 if (nh.getParam("/"+controller_list.response.controllers[i]+"/joint", controlled_joint_name))
00188 {
00189 ROS_DEBUG("controller %d:%s controls joint %s\n",i,controller_list.response.controllers[i].c_str(),controlled_joint_name.c_str());
00190 jointControllerMap[controlled_joint_name]= controller_list.response.controllers[i] ;
00191 }
00192 }
00193 }
00194
00195
00196 for(unsigned int i=0; i < joint_labels.size(); i ++)
00197 {
00198 std::string controller_name=jointControllerMap[joint_labels[i]];
00199
00200 if(controller_name.compare("")!=0)
00201 {
00202 controller_publishers.push_back(nh.advertise<std_msgs::Float64>("/"+jointControllerMap[joint_labels[i]]+"/command", 2));
00203 jointPubIdxMap[joint_labels[i]]=controller_publishers.size();
00204 }
00205 else
00206 {
00207 ROS_WARN("Could not find a controller for joint %s",joint_labels[i].c_str());
00208 jointPubIdxMap[joint_labels[i]]=0;
00209 }
00210 }
00211 }
00212 else
00213 {
00214 ROS_WARN("sr_controller_manager not found, switching back to sendupdates");
00215 use_sendupdate=true;
00216 sr_arm_target_pub = nh.advertise<sr_robot_msgs::sendupdate>("/sr_arm/sendupdate", 2);
00217 sr_hand_target_pub = nh.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2);
00218 }
00219
00220 ROS_INFO("waiting for getJointState");
00221 if( ros::service::waitForService("getJointState",20000))
00222 {
00223
00224 joint_state_client = nh.serviceClient<sr_utilities::getJointState>("getJointState",true);
00225 }
00226 else
00227 {
00228 ROS_ERROR("Cannot access service: Check if joint_state service is launched");
00229 ros::shutdown();
00230 exit(-1);
00231 }
00232 ROS_INFO("Got getJointState");
00233 joint_names_=joint_labels;
00234
00235 q.resize(joint_names_.size());
00236 qd.resize(joint_names_.size());
00237 qdd.resize(joint_names_.size());
00238
00239 desired_joint_state_pusblisher = nh.advertise<sensor_msgs::JointState> ("/desired_joint_states", 2);
00240
00241 command_sub = nh.subscribe("command", 1, &JointTrajectoryActionController::commandCB, this);
00242 ROS_INFO("Listening to commands");
00243
00244 action_server->start();
00245 ROS_INFO("Action server started");
00246 }
00247
00248 JointTrajectoryActionController::~JointTrajectoryActionController()
00249 {
00250
00251 }
00252
00253
00254 void JointTrajectoryActionController::updateJointState()
00255 {
00256 sr_utilities::getJointState getState;
00257 sensor_msgs::JointState joint_state_msg;
00258 if(joint_state_client.call(getState))
00259 {
00260 joint_state_msg=getState.response.joint_state;
00261 if(joint_state_msg.name.size()>0)
00262 {
00263
00264 for(unsigned int i=0;i<joint_state_msg.name.size();i++)
00265 {
00266 joint_state_map[joint_state_msg.name[i]]=joint_state_msg.position[i];
00267 }
00268 }
00269 }
00270 }
00271
00272 bool JointTrajectoryActionController::getPosition(std::string joint_name, double &position)
00273 {
00274 std::map<std::string, double>::iterator iter = joint_state_map.find(joint_name);
00275 if (iter != joint_state_map.end())
00276 {
00277 position = iter->second;
00278 return true;
00279 }
00280 else
00281 {
00282 ROS_ERROR("Joint %s not found",joint_name.c_str());
00283 return false;
00284 }
00285 }
00286
00287
00288 void JointTrajectoryActionController::execute_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
00289 {
00290 bool success = true;
00291
00292 ros::Time time = ros::Time::now() - ros::Duration(0.05);
00293 last_time_ = time;
00294 ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
00295 time.toSec(), goal->trajectory.header.stamp.toSec());
00296
00297 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00298 SpecifiedTrajectory &traj = *new_traj_ptr;
00299
00300
00301
00302 std::vector<double> prev_positions(joint_names_.size());
00303 std::vector<double> prev_velocities(joint_names_.size());
00304 std::vector<double> prev_accelerations(joint_names_.size());
00305
00306 updateJointState();
00307
00308 ROS_DEBUG("Initial conditions for new set of splines:");
00309 for (size_t i = 0; i < joint_names_.size(); ++i)
00310 {
00311 double position;
00312 if(getPosition(joint_names_[i],position))
00313 prev_positions[joint_state_idx_map[joint_names_[i]]]=position;
00314 else
00315 {
00316 ROS_ERROR("Cannot get joint_state, not executing trajectory");
00317 return;
00318 }
00319 prev_velocities[i]=0.0;
00320 prev_accelerations[i]=0.0;
00321
00322 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
00323 prev_accelerations[i], joint_names_[i].c_str());
00324 }
00325
00326 std::vector<double> positions;
00327 std::vector<double> velocities;
00328 std::vector<double> accelerations;
00329
00330 std::vector<double> durations(goal->trajectory.points.size());
00331 if (goal->trajectory.points.size() > 0)
00332 durations[0] = goal->trajectory.points[0].time_from_start.toSec();
00333 for (size_t i = 1; i < goal->trajectory.points.size(); ++i)
00334 durations[i] = (goal->trajectory.points[i].time_from_start - goal->trajectory.points[i-1].time_from_start).toSec();
00335
00336
00337
00338
00339 for (size_t i = 0; i < goal->trajectory.points.size(); ++i)
00340 {
00341 Segment seg;
00342
00343 if(goal->trajectory.header.stamp == ros::Time(0.0))
00344 seg.start_time = (time + goal->trajectory.points[i].time_from_start).toSec() - durations[i];
00345 else
00346 seg.start_time = (goal->trajectory.header.stamp + goal->trajectory.points[i].time_from_start).toSec() - durations[i];
00347 seg.duration = durations[i];
00348 seg.splines.resize(joint_names_.size());
00349
00350
00351 if (goal->trajectory.points[i].accelerations.size() != 0 && goal->trajectory.points[i].accelerations.size() != joint_names_.size())
00352 {
00353 ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)goal->trajectory.points[i].accelerations.size());
00354 return;
00355 }
00356 if (goal->trajectory.points[i].velocities.size() != 0 && goal->trajectory.points[i].velocities.size() != joint_names_.size())
00357 {
00358 ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)goal->trajectory.points[i].velocities.size());
00359 return;
00360 }
00361 if (goal->trajectory.points[i].positions.size() != joint_names_.size())
00362 {
00363 ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)goal->trajectory.points[i].positions.size());
00364 return;
00365 }
00366
00367
00368 accelerations.resize(goal->trajectory.points[i].accelerations.size());
00369 velocities.resize(goal->trajectory.points[i].velocities.size());
00370 positions.resize(goal->trajectory.points[i].positions.size());
00371 for (size_t j = 0; j < goal->trajectory.joint_names.size(); ++j)
00372 {
00373 if (!accelerations.empty()) accelerations[ joint_state_idx_map[goal->trajectory.joint_names[j]] ] = goal->trajectory.points[i].accelerations[j];
00374 if (!velocities.empty()) velocities[ joint_state_idx_map[goal->trajectory.joint_names[j]] ] = goal->trajectory.points[i].velocities[j];
00375 if (!positions.empty()) positions[ joint_state_idx_map[goal->trajectory.joint_names[j]] ] = goal->trajectory.points[i].positions[j];
00376 }
00377
00378
00379 for (size_t j = 0; j < joint_names_.size(); ++j)
00380 {
00381 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00382 {
00383 getQuinticSplineCoefficients(
00384 prev_positions[j], prev_velocities[j], prev_accelerations[j],
00385 positions[j], velocities[j], accelerations[j],
00386 durations[i],
00387 seg.splines[j].coef);
00388 }
00389 else if (prev_velocities.size() > 0 && velocities.size() > 0)
00390 {
00391 getCubicSplineCoefficients(
00392 prev_positions[j], prev_velocities[j],
00393 positions[j], velocities[j],
00394 durations[i],
00395 seg.splines[j].coef);
00396 seg.splines[j].coef.resize(6, 0.0);
00397 }
00398 else
00399 {
00400 seg.splines[j].coef[0] = prev_positions[j];
00401 if (durations[i] == 0.0)
00402 seg.splines[j].coef[1] = 0.0;
00403 else
00404 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00405 seg.splines[j].coef[2] = 0.0;
00406 seg.splines[j].coef[3] = 0.0;
00407 seg.splines[j].coef[4] = 0.0;
00408 seg.splines[j].coef[5] = 0.0;
00409 }
00410 }
00411
00412
00413 traj.push_back(seg);
00414
00415
00416
00417 prev_positions = positions;
00418 prev_velocities = velocities;
00419 prev_accelerations = accelerations;
00420 }
00421
00422
00423
00424 if (!new_traj_ptr)
00425 {
00426 ROS_ERROR("The new trajectory was null!");
00427 return;
00428 }
00429
00430 ROS_DEBUG("The new trajectory has %d segments", (int)traj.size());
00431
00432 std::vector<sr_robot_msgs::joint> joint_vector_traj;
00433 unsigned int controller_pub_idx=0;
00434
00435 std_msgs::Float64 target_msg;
00436 sr_robot_msgs::sendupdate sendupdate_msg_traj;
00437
00438
00439
00440
00441 joint_vector_traj.clear();
00442
00443 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00444 {
00445 sr_robot_msgs::joint joint;
00446 joint.joint_name = joint_names_[i];
00447 joint_vector_traj.push_back(joint);
00448 }
00449
00450 if(use_sendupdate)
00451 {
00452 sendupdate_msg_traj.sendupdate_length = joint_vector_traj.size();
00453 ROS_DEBUG("Trajectory received: %d joints / %d msg length", (int)goal->trajectory.joint_names.size(), sendupdate_msg_traj.sendupdate_length);
00454 }
00455
00456 ros::Rate tmp_rate(1.0);
00457
00458
00459
00460
00461
00462 ros::Duration sleeping_time(0.0);
00463 ROS_DEBUG("Entering the execution loop");
00464
00465 last_time_ = ros::Time::now();
00466 while(ros::ok())
00467 {
00468 ros::Time time = ros::Time::now();
00469 ros::Duration dt = time - last_time_;
00470 last_time_ = time;
00471
00472
00473 ROS_DEBUG("Find current segment");
00474
00475
00476 int seg = -1;
00477 while (seg + 1 < (int)traj.size() && traj[seg+1].start_time < time.toSec())
00478 {
00479 ++seg;
00480 }
00481
00482
00483 if( (traj[traj.size()-1].start_time+traj[traj.size()-1].duration) < time.toSec())
00484 {
00485 ROS_DEBUG("trajectory is finished %f<%f",(traj[traj.size()-1].start_time+traj[traj.size()-1].duration),time.toSec());
00486 break;
00487 }
00488
00489 if (seg == -1)
00490 {
00491 if (traj.size() == 0)
00492 ROS_ERROR("No segments in the trajectory");
00493 else
00494 ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00495 success = false;
00496 break;
00497 }
00498
00499
00500 ROS_DEBUG("Sample the trajectory");
00501
00502 for (size_t i = 0; i < q.size(); ++i)
00503 {
00504 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00505 time.toSec() - traj[seg].start_time,
00506 q[i], qd[i], qdd[i]);
00507 }
00508 ROS_DEBUG("Sampled the trajectory");
00509
00510 if (action_server->isPreemptRequested() || !ros::ok())
00511 {
00512 ROS_INFO("Joint Trajectory Action Preempted");
00513
00514 action_server->setPreempted();
00515 success = false;
00516 break;
00517 }
00518 ROS_DEBUG("Update the targets");
00519
00520 sensor_msgs::JointState desired_joint_state_msg;
00521 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00522 {
00523 desired_joint_state_msg.name.push_back(joint_names_[i]);
00524 desired_joint_state_msg.position.push_back(q[i]);
00525 desired_joint_state_msg.velocity.push_back(qd[i]);
00526 desired_joint_state_msg.effort.push_back(0.0);
00527 if(!use_sendupdate)
00528 {
00529 if((controller_pub_idx=jointPubIdxMap[ joint_names_[i] ])>0)
00530 {
00531 target_msg.data=q[i];
00532 controller_publishers.at(controller_pub_idx-1).publish(target_msg);
00533 }
00534 }
00535 else
00536 {
00537 joint_vector_traj[i].joint_target = q[i] * 57.3;
00538 ROS_DEBUG("traj[%s]: %f", joint_vector_traj[i].joint_name.c_str(), joint_vector_traj[i].joint_target);
00539 }
00540 }
00541 ROS_DEBUG("Targets updated");
00542
00543 desired_joint_state_msg.header.stamp = ros::Time::now();
00544 desired_joint_state_pusblisher.publish(desired_joint_state_msg);
00545
00546 if(use_sendupdate)
00547 {
00548 sendupdate_msg_traj.sendupdate_list = joint_vector_traj;
00549 sr_arm_target_pub.publish(sendupdate_msg_traj);
00550 sr_hand_target_pub.publish(sendupdate_msg_traj);
00551 }
00552
00553 ROS_DEBUG("Now sleep and loop");
00554 sleeping_time.sleep();
00555 sleeping_time = ros::Duration(0.1);
00556 ROS_DEBUG("redo loop");
00557 }
00558
00559 control_msgs::FollowJointTrajectoryResult joint_trajectory_result;
00560
00561 if(success)
00562 action_server->setSucceeded(joint_trajectory_result);
00563 else
00564 action_server->setAborted(joint_trajectory_result);
00565 }
00566
00567 void JointTrajectoryActionController::sampleSplineWithTimeBounds(
00568 const std::vector<double>& coefficients, double duration, double time,
00569 double& position, double& velocity, double& acceleration)
00570 {
00571 if (time < 0)
00572 {
00573 double _;
00574 sampleQuinticSpline(coefficients, 0.0, position, _, _);
00575 velocity = 0;
00576 acceleration = 0;
00577 }
00578 else if (time > duration)
00579 {
00580 double _;
00581 sampleQuinticSpline(coefficients, duration, position, _, _);
00582 velocity = 0;
00583 acceleration = 0;
00584 }
00585 else
00586 {
00587 sampleQuinticSpline(coefficients, time,
00588 position, velocity, acceleration);
00589 }
00590 }
00591
00592 void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00593 {
00594 bool success = true;
00595
00596 ros::Time time = ros::Time::now()-ros::Duration(0.05);
00597 last_time_ = time;
00598
00599 ROS_ERROR("Figuring out new trajectory at %.3lf, with data from %.3lf with %d waypoints",
00600 time.toSec(), msg->header.stamp.toSec(),msg->points.size());
00601
00602 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00603 SpecifiedTrajectory &traj = *new_traj_ptr;
00604
00605
00606
00607 std::vector<double> prev_positions(joint_names_.size());
00608 std::vector<double> prev_velocities(joint_names_.size());
00609 std::vector<double> prev_accelerations(joint_names_.size());
00610
00611 updateJointState();
00612
00613 ROS_DEBUG("Initial conditions for new set of splines:");
00614 for (size_t i = 0; i < joint_names_.size(); ++i)
00615 {
00616 double position;
00617 if(getPosition(joint_names_[i],position))
00618 prev_positions[i]=position;
00619 else
00620 {
00621 ROS_ERROR("Cannot get joint_state, not executing trajectory");
00622 return;
00623 }
00624 prev_velocities[i]=0.0;
00625 prev_accelerations[i]=0.0;
00626
00627 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
00628 prev_accelerations[i], joint_names_[i].c_str());
00629 }
00630
00631 std::vector<double> positions;
00632 std::vector<double> velocities;
00633 std::vector<double> accelerations;
00634
00635 std::vector<double> durations(msg->points.size());
00636 if (msg->points.size() > 0)
00637 durations[0] = msg->points[0].time_from_start.toSec();
00638 for (size_t i = 1; i < msg->points.size(); ++i)
00639 durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
00640
00641
00642
00643
00644 for (size_t i = 0; i < msg->points.size(); ++i)
00645 {
00646 Segment seg;
00647 ROS_DEBUG("Current time %f and header time %f",msg->header.stamp.toSec(),ros::Time(0.0).toSec());
00648 if(msg->header.stamp == ros::Time(0.0))
00649 {
00650 seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
00651 ROS_DEBUG("Segment %d start time A %f,time_from_start %f, duration, %f",i,seg.start_time,msg->points[i].time_from_start.toSec(),durations[i]);
00652 }
00653 else
00654 {
00655 seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
00656 ROS_DEBUG("Segment start time B %f",seg.start_time);
00657 }
00658 seg.duration = durations[i];
00659 seg.splines.resize(joint_names_.size());
00660
00661
00662 if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joint_names_.size())
00663 {
00664 ROS_DEBUG("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
00665 return;
00666 }
00667 if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joint_names_.size())
00668 {
00669 ROS_DEBUG("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
00670 return;
00671 }
00672 if (msg->points[i].positions.size() != joint_names_.size())
00673 {
00674 ROS_DEBUG("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
00675 return;
00676 }
00677
00678
00679 accelerations.resize(msg->points[i].accelerations.size());
00680 velocities.resize(msg->points[i].velocities.size());
00681 positions.resize(msg->points[i].positions.size());
00682 for (size_t j = 0; j < joint_names_.size(); ++j)
00683 {
00684 if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[j];
00685 if (!velocities.empty()) velocities[j] = msg->points[i].velocities[j];
00686 if (!positions.empty()) positions[j] = msg->points[i].positions[j];
00687 }
00688
00689
00690 for (size_t j = 0; j < joint_names_.size(); ++j)
00691 {
00692 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00693 {
00694 getQuinticSplineCoefficients(
00695 prev_positions[j], prev_velocities[j], prev_accelerations[j],
00696 positions[j], velocities[j], accelerations[j],
00697 durations[i],
00698 seg.splines[j].coef);
00699 }
00700 else if (prev_velocities.size() > 0 && velocities.size() > 0)
00701 {
00702 getCubicSplineCoefficients(
00703 prev_positions[j], prev_velocities[j],
00704 positions[j], velocities[j],
00705 durations[i],
00706 seg.splines[j].coef);
00707 seg.splines[j].coef.resize(6, 0.0);
00708 }
00709 else
00710 {
00711 seg.splines[j].coef[0] = prev_positions[j];
00712 if (durations[i] == 0.0)
00713 seg.splines[j].coef[1] = 0.0;
00714 else
00715 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00716 seg.splines[j].coef[2] = 0.0;
00717 seg.splines[j].coef[3] = 0.0;
00718 seg.splines[j].coef[4] = 0.0;
00719 seg.splines[j].coef[5] = 0.0;
00720 }
00721 }
00722
00723
00724 traj.push_back(seg);
00725
00726
00727
00728 prev_positions = positions;
00729 prev_velocities = velocities;
00730 prev_accelerations = accelerations;
00731 }
00732
00733
00734
00735 if (!new_traj_ptr)
00736 {
00737 ROS_ERROR("The new trajectory was null!");
00738 return;
00739 }
00740
00741 ROS_DEBUG("The new trajectory has %d segments", (int)traj.size());
00742
00743 std::vector<sr_robot_msgs::joint> joint_vector_traj;
00744 unsigned int controller_pub_idx=0;
00745
00746 std_msgs::Float64 target_msg;
00747 sr_robot_msgs::sendupdate sendupdate_msg_traj;
00748
00749
00750
00751
00752 joint_vector_traj.clear();
00753
00754 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00755 {
00756 sr_robot_msgs::joint joint;
00757 joint.joint_name = joint_names_[i];
00758 joint_vector_traj.push_back(joint);
00759 }
00760
00761 if(use_sendupdate)
00762 {
00763 sendupdate_msg_traj.sendupdate_length = joint_vector_traj.size();
00764 ROS_DEBUG("Trajectory received: %d joints / %d msg length", (int)msg->joint_names.size(), sendupdate_msg_traj.sendupdate_length);
00765 }
00766
00767 ros::Rate tmp_rate(1.0);
00768
00769
00770
00771
00772
00773 ros::Duration sleeping_time(0.0);
00774 ROS_DEBUG("Entering the execution loop");
00775
00776 while(ros::ok())
00777 {
00778 ros::Time time = ros::Time::now();
00779 ros::Duration dt = time - last_time_;
00780 last_time_ = time;
00781
00782
00783 ROS_DEBUG("Find current segment");
00784
00785
00786 int seg = -1;
00787 while (seg + 1 < (int)traj.size() && traj[seg+1].start_time < time.toSec())
00788 {
00789 ++seg;
00790 }
00791
00792
00793 if( (traj[traj.size()-1].start_time+traj[traj.size()-1].duration) < time.toSec())
00794 {
00795 ROS_DEBUG("trajectory is finished %f<%f",(traj[traj.size()-1].start_time+traj[traj.size()-1].duration),time.toSec());
00796 break;
00797 }
00798
00799 if (seg == -1)
00800 {
00801 if (traj.size() == 0)
00802 ROS_ERROR("No segments in the trajectory");
00803 else
00804 ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00805 return;
00806 }
00807
00808
00809 ROS_DEBUG("Sample the trajectory");
00810
00811 for (size_t i = 0; i < q.size(); ++i)
00812 {
00813 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00814 time.toSec() - traj[seg].start_time,
00815 q[i], qd[i], qdd[i]);
00816 }
00817 ROS_DEBUG("Sampled the trajectory");
00818
00819 if (!ros::ok())
00820 {
00821 ROS_INFO("Joint Trajectory Stopping");
00822
00823
00824 success = false;
00825 break;
00826 }
00827 ROS_DEBUG("Update the targets");
00828
00829 sensor_msgs::JointState desired_joint_state_msg;
00830 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00831 {
00832 desired_joint_state_msg.name.push_back(joint_names_[i]);
00833 desired_joint_state_msg.position.push_back(q[i]);
00834 desired_joint_state_msg.velocity.push_back(qd[i]);
00835 desired_joint_state_msg.effort.push_back(0.0);
00836 if(!use_sendupdate)
00837 {
00838 if((controller_pub_idx=jointPubIdxMap[ joint_names_[i] ])>0)
00839 {
00840 target_msg.data=q[i];
00841 controller_publishers.at(controller_pub_idx-1).publish(target_msg);
00842 }
00843 }
00844 else
00845 {
00846 joint_vector_traj[i].joint_target = q[i] * 57.3;
00847 ROS_DEBUG("traj[%s]: %f", joint_vector_traj[i].joint_name.c_str(), joint_vector_traj[i].joint_target);
00848 }
00849 }
00850 ROS_DEBUG("Targets updated");
00851
00852 desired_joint_state_msg.header.stamp = ros::Time::now();
00853 desired_joint_state_pusblisher.publish(desired_joint_state_msg);
00854
00855 if(use_sendupdate)
00856 {
00857 sendupdate_msg_traj.sendupdate_list = joint_vector_traj;
00858 sr_arm_target_pub.publish(sendupdate_msg_traj);
00859 sr_hand_target_pub.publish(sendupdate_msg_traj);
00860 }
00861
00862 ROS_DEBUG("Now sleep and loop");
00863 sleeping_time.sleep();
00864 sleeping_time = ros::Duration(0.1);
00865 ROS_DEBUG("redo loop");
00866 }
00867
00868 return;
00869 }
00870
00871 }
00872
00873
00874 int main(int argc, char** argv)
00875 {
00876 ros::init(argc, argv, "sr_joint_trajectory_action_controller");
00877
00878 ros::AsyncSpinner spinner(1);
00879 spinner.start();
00880 shadowrobot::JointTrajectoryActionController jac;
00881 ros::spin();
00882
00883
00884 return 0;
00885 }
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895