00001
00013 #include "sr_mechanism_controllers/joint_spline_trajectory_action_controller.hpp"
00014 #include <controller_manager_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("controller_manager/list_controllers",20000) )
00174 {
00175 use_sendupdate=false;
00176 ros::ServiceClient controller_list_client = nh.serviceClient<controller_manager_msgs::ListControllers>("controller_manager/list_controllers");
00177 controller_manager_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.controller.size() ; i++ )
00184 {
00185 if(controller_list.response.controller[i].state=="running")
00186 {
00187 if (nh.getParam("/"+controller_list.response.controller[i].name+"/joint", controlled_joint_name))
00188 {
00189 ROS_DEBUG("controller %d:%s controls joint %s\n",i,controller_list.response.controller[i].name.c_str(),controlled_joint_name.c_str());
00190 jointControllerMap[controlled_joint_name]= controller_list.response.controller[i].name ;
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("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 void JointTrajectoryActionController::updateJointState()
00249 {
00250 sr_utilities::getJointState getState;
00251 sensor_msgs::JointState joint_state_msg;
00252 if(joint_state_client.call(getState))
00253 {
00254 joint_state_msg=getState.response.joint_state;
00255 if(joint_state_msg.name.size()>0)
00256 {
00257
00258 for(unsigned int i=0;i<joint_state_msg.name.size();i++)
00259 {
00260 joint_state_map[joint_state_msg.name[i]]=joint_state_msg.position[i];
00261 }
00262 }
00263 }
00264 }
00265
00266 bool JointTrajectoryActionController::getPosition(std::string joint_name, double &position)
00267 {
00268 std::map<std::string, double>::iterator iter = joint_state_map.find(joint_name);
00269 if (iter != joint_state_map.end())
00270 {
00271 position = iter->second;
00272 return true;
00273 }
00274 else
00275 {
00276 ROS_ERROR("Joint %s not found",joint_name.c_str());
00277 return false;
00278 }
00279 }
00280
00281
00282 void JointTrajectoryActionController::execute_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
00283 {
00284 bool success = true;
00285
00286 ros::Time time = ros::Time::now() - ros::Duration(0.05);
00287 last_time_ = time;
00288 ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
00289 time.toSec(), goal->trajectory.header.stamp.toSec());
00290
00291 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00292 SpecifiedTrajectory &traj = *new_traj_ptr;
00293
00294
00295
00296 std::vector<double> prev_positions(joint_names_.size());
00297 std::vector<double> prev_velocities(joint_names_.size());
00298 std::vector<double> prev_accelerations(joint_names_.size());
00299
00300 updateJointState();
00301
00302 ROS_DEBUG("Initial conditions for new set of splines:");
00303 for (size_t i = 0; i < joint_names_.size(); ++i)
00304 {
00305 double position;
00306 if(getPosition(joint_names_[i],position))
00307 prev_positions[joint_state_idx_map[joint_names_[i]]]=position;
00308 else
00309 {
00310 ROS_ERROR("Cannot get joint_state, not executing trajectory");
00311 return;
00312 }
00313 prev_velocities[i]=0.0;
00314 prev_accelerations[i]=0.0;
00315
00316 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
00317 prev_accelerations[i], joint_names_[i].c_str());
00318 }
00319
00320 std::vector<double> positions;
00321 std::vector<double> velocities;
00322 std::vector<double> accelerations;
00323
00324 std::vector<double> durations(goal->trajectory.points.size());
00325 if (goal->trajectory.points.size() > 0)
00326 durations[0] = goal->trajectory.points[0].time_from_start.toSec();
00327 for (size_t i = 1; i < goal->trajectory.points.size(); ++i)
00328 durations[i] = (goal->trajectory.points[i].time_from_start - goal->trajectory.points[i-1].time_from_start).toSec();
00329
00330
00331
00332
00333 for (size_t i = 0; i < goal->trajectory.points.size(); ++i)
00334 {
00335 Segment seg;
00336
00337 if(goal->trajectory.header.stamp == ros::Time(0.0))
00338 seg.start_time = (time + goal->trajectory.points[i].time_from_start).toSec() - durations[i];
00339 else
00340 seg.start_time = (goal->trajectory.header.stamp + goal->trajectory.points[i].time_from_start).toSec() - durations[i];
00341 seg.duration = durations[i];
00342 seg.splines.resize(joint_names_.size());
00343
00344
00345 if (goal->trajectory.points[i].accelerations.size() != 0 && goal->trajectory.points[i].accelerations.size() != joint_names_.size())
00346 {
00347 ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)goal->trajectory.points[i].accelerations.size());
00348 return;
00349 }
00350 if (goal->trajectory.points[i].velocities.size() != 0 && goal->trajectory.points[i].velocities.size() != joint_names_.size())
00351 {
00352 ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)goal->trajectory.points[i].velocities.size());
00353 return;
00354 }
00355 if (goal->trajectory.points[i].positions.size() != joint_names_.size())
00356 {
00357 ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)goal->trajectory.points[i].positions.size());
00358 return;
00359 }
00360
00361
00362 accelerations.resize(goal->trajectory.points[i].accelerations.size());
00363 velocities.resize(goal->trajectory.points[i].velocities.size());
00364 positions.resize(goal->trajectory.points[i].positions.size());
00365 for (size_t j = 0; j < goal->trajectory.joint_names.size(); ++j)
00366 {
00367 if (!accelerations.empty()) accelerations[ joint_state_idx_map[goal->trajectory.joint_names[j]] ] = goal->trajectory.points[i].accelerations[j];
00368 if (!velocities.empty()) velocities[ joint_state_idx_map[goal->trajectory.joint_names[j]] ] = goal->trajectory.points[i].velocities[j];
00369 if (!positions.empty()) positions[ joint_state_idx_map[goal->trajectory.joint_names[j]] ] = goal->trajectory.points[i].positions[j];
00370 }
00371
00372
00373 for (size_t j = 0; j < joint_names_.size(); ++j)
00374 {
00375 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00376 {
00377 getQuinticSplineCoefficients(
00378 prev_positions[j], prev_velocities[j], prev_accelerations[j],
00379 positions[j], velocities[j], accelerations[j],
00380 durations[i],
00381 seg.splines[j].coef);
00382 }
00383 else if (prev_velocities.size() > 0 && velocities.size() > 0)
00384 {
00385 getCubicSplineCoefficients(
00386 prev_positions[j], prev_velocities[j],
00387 positions[j], velocities[j],
00388 durations[i],
00389 seg.splines[j].coef);
00390 seg.splines[j].coef.resize(6, 0.0);
00391 }
00392 else
00393 {
00394 seg.splines[j].coef[0] = prev_positions[j];
00395 if (durations[i] == 0.0)
00396 seg.splines[j].coef[1] = 0.0;
00397 else
00398 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00399 seg.splines[j].coef[2] = 0.0;
00400 seg.splines[j].coef[3] = 0.0;
00401 seg.splines[j].coef[4] = 0.0;
00402 seg.splines[j].coef[5] = 0.0;
00403 }
00404 }
00405
00406
00407 traj.push_back(seg);
00408
00409
00410
00411 prev_positions = positions;
00412 prev_velocities = velocities;
00413 prev_accelerations = accelerations;
00414 }
00415
00416
00417
00418 if (!new_traj_ptr)
00419 {
00420 ROS_ERROR("The new trajectory was null!");
00421 return;
00422 }
00423
00424 ROS_DEBUG("The new trajectory has %d segments", (int)traj.size());
00425
00426 std::vector<sr_robot_msgs::joint> joint_vector_traj;
00427 unsigned int controller_pub_idx=0;
00428
00429 std_msgs::Float64 target_msg;
00430 sr_robot_msgs::sendupdate sendupdate_msg_traj;
00431
00432
00433
00434
00435 joint_vector_traj.clear();
00436
00437 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00438 {
00439 sr_robot_msgs::joint joint;
00440 joint.joint_name = joint_names_[i];
00441 joint_vector_traj.push_back(joint);
00442 }
00443
00444 if(use_sendupdate)
00445 {
00446 sendupdate_msg_traj.sendupdate_length = joint_vector_traj.size();
00447 ROS_DEBUG("Trajectory received: %d joints / %d msg length", (int)goal->trajectory.joint_names.size(), sendupdate_msg_traj.sendupdate_length);
00448 }
00449
00450 ros::Rate tmp_rate(1.0);
00451
00452
00453
00454
00455
00456 ros::Duration sleeping_time(0.0);
00457 ROS_DEBUG("Entering the execution loop");
00458
00459 last_time_ = ros::Time::now();
00460 while(ros::ok())
00461 {
00462 ros::Time time = ros::Time::now();
00463 ros::Duration dt = time - last_time_;
00464 last_time_ = time;
00465
00466
00467 ROS_DEBUG("Find current segment");
00468
00469
00470 int seg = -1;
00471 while (seg + 1 < (int)traj.size() && traj[seg+1].start_time < time.toSec())
00472 {
00473 ++seg;
00474 }
00475
00476
00477 if( (traj[traj.size()-1].start_time+traj[traj.size()-1].duration) < time.toSec())
00478 {
00479 ROS_DEBUG("trajectory is finished %f<%f",(traj[traj.size()-1].start_time+traj[traj.size()-1].duration),time.toSec());
00480 break;
00481 }
00482
00483 if (seg == -1)
00484 {
00485 if (traj.size() == 0)
00486 ROS_ERROR("No segments in the trajectory");
00487 else
00488 ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00489 success = false;
00490 break;
00491 }
00492
00493
00494 ROS_DEBUG("Sample the trajectory");
00495
00496 for (size_t i = 0; i < q.size(); ++i)
00497 {
00498 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00499 time.toSec() - traj[seg].start_time,
00500 q[i], qd[i], qdd[i]);
00501 }
00502 ROS_DEBUG("Sampled the trajectory");
00503
00504 if (action_server->isPreemptRequested() || !ros::ok())
00505 {
00506 ROS_INFO("Joint Trajectory Action Preempted");
00507
00508 action_server->setPreempted();
00509 success = false;
00510 break;
00511 }
00512 ROS_DEBUG("Update the targets");
00513
00514 sensor_msgs::JointState desired_joint_state_msg;
00515 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00516 {
00517 desired_joint_state_msg.name.push_back(joint_names_[i]);
00518 desired_joint_state_msg.position.push_back(q[i]);
00519 desired_joint_state_msg.velocity.push_back(qd[i]);
00520 desired_joint_state_msg.effort.push_back(0.0);
00521 if(!use_sendupdate)
00522 {
00523 if((controller_pub_idx=jointPubIdxMap[ joint_names_[i] ])>0)
00524 {
00525 target_msg.data=q[i];
00526 controller_publishers.at(controller_pub_idx-1).publish(target_msg);
00527 }
00528 }
00529 else
00530 {
00531 joint_vector_traj[i].joint_target = q[i] * 57.3;
00532 ROS_DEBUG("traj[%s]: %f", joint_vector_traj[i].joint_name.c_str(), joint_vector_traj[i].joint_target);
00533 }
00534 }
00535 ROS_DEBUG("Targets updated");
00536
00537 desired_joint_state_msg.header.stamp = ros::Time::now();
00538 desired_joint_state_pusblisher.publish(desired_joint_state_msg);
00539
00540 if(use_sendupdate)
00541 {
00542 sendupdate_msg_traj.sendupdate_list = joint_vector_traj;
00543 sr_arm_target_pub.publish(sendupdate_msg_traj);
00544 sr_hand_target_pub.publish(sendupdate_msg_traj);
00545 }
00546
00547 ROS_DEBUG("Now sleep and loop");
00548 sleeping_time.sleep();
00549 sleeping_time = ros::Duration(0.1);
00550 ROS_DEBUG("redo loop");
00551 }
00552
00553 control_msgs::FollowJointTrajectoryResult joint_trajectory_result;
00554
00555 if(success)
00556 action_server->setSucceeded(joint_trajectory_result);
00557 else
00558 action_server->setAborted(joint_trajectory_result);
00559 }
00560
00561 void JointTrajectoryActionController::sampleSplineWithTimeBounds(
00562 const std::vector<double>& coefficients, double duration, double time,
00563 double& position, double& velocity, double& acceleration)
00564 {
00565 if (time < 0)
00566 {
00567 double _;
00568 sampleQuinticSpline(coefficients, 0.0, position, _, _);
00569 velocity = 0;
00570 acceleration = 0;
00571 }
00572 else if (time > duration)
00573 {
00574 double _;
00575 sampleQuinticSpline(coefficients, duration, position, _, _);
00576 velocity = 0;
00577 acceleration = 0;
00578 }
00579 else
00580 {
00581 sampleQuinticSpline(coefficients, time,
00582 position, velocity, acceleration);
00583 }
00584 }
00585
00586 void JointTrajectoryActionController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
00587 {
00588 bool success = true;
00589
00590 ros::Time time = ros::Time::now()-ros::Duration(0.05);
00591 last_time_ = time;
00592
00593 ROS_ERROR("Figuring out new trajectory at %.3lf, with data from %.3lf with %zu waypoints",
00594 time.toSec(), msg->header.stamp.toSec(), msg->points.size());
00595
00596 boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
00597 SpecifiedTrajectory &traj = *new_traj_ptr;
00598
00599
00600
00601 std::vector<double> prev_positions(joint_names_.size());
00602 std::vector<double> prev_velocities(joint_names_.size());
00603 std::vector<double> prev_accelerations(joint_names_.size());
00604
00605 updateJointState();
00606
00607 ROS_DEBUG("Initial conditions for new set of splines:");
00608 for (size_t i = 0; i < joint_names_.size(); ++i)
00609 {
00610 double position;
00611 if(getPosition(joint_names_[i],position))
00612 prev_positions[i]=position;
00613 else
00614 {
00615 ROS_ERROR("Cannot get joint_state, not executing trajectory");
00616 return;
00617 }
00618 prev_velocities[i]=0.0;
00619 prev_accelerations[i]=0.0;
00620
00621 ROS_DEBUG(" %.2lf, %.2lf, %.2lf (%s)", prev_positions[i], prev_velocities[i],
00622 prev_accelerations[i], joint_names_[i].c_str());
00623 }
00624
00625 std::vector<double> positions;
00626 std::vector<double> velocities;
00627 std::vector<double> accelerations;
00628
00629 std::vector<double> durations(msg->points.size());
00630 if (msg->points.size() > 0)
00631 durations[0] = msg->points[0].time_from_start.toSec();
00632 for (size_t i = 1; i < msg->points.size(); ++i)
00633 durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();
00634
00635
00636
00637
00638 for (size_t i = 0; i < msg->points.size(); ++i)
00639 {
00640 Segment seg;
00641 ROS_DEBUG("Current time %f and header time %f",msg->header.stamp.toSec(),ros::Time(0.0).toSec());
00642 if(msg->header.stamp == ros::Time(0.0))
00643 {
00644 seg.start_time = (time + msg->points[i].time_from_start).toSec() - durations[i];
00645 ROS_DEBUG("Segment %zu start time A %f,time_from_start %f, duration, %f", i, seg.start_time,msg->points[i].time_from_start.toSec(), durations[i]);
00646 }
00647 else
00648 {
00649 seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
00650 ROS_DEBUG("Segment start time B %f",seg.start_time);
00651 }
00652 seg.duration = durations[i];
00653 seg.splines.resize(joint_names_.size());
00654
00655
00656 if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joint_names_.size())
00657 {
00658 ROS_DEBUG("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
00659 return;
00660 }
00661 if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joint_names_.size())
00662 {
00663 ROS_DEBUG("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
00664 return;
00665 }
00666 if (msg->points[i].positions.size() != joint_names_.size())
00667 {
00668 ROS_DEBUG("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
00669 return;
00670 }
00671
00672
00673 accelerations.resize(msg->points[i].accelerations.size());
00674 velocities.resize(msg->points[i].velocities.size());
00675 positions.resize(msg->points[i].positions.size());
00676 for (size_t j = 0; j < joint_names_.size(); ++j)
00677 {
00678 if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[j];
00679 if (!velocities.empty()) velocities[j] = msg->points[i].velocities[j];
00680 if (!positions.empty()) positions[j] = msg->points[i].positions[j];
00681 }
00682
00683
00684 for (size_t j = 0; j < joint_names_.size(); ++j)
00685 {
00686 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
00687 {
00688 getQuinticSplineCoefficients(
00689 prev_positions[j], prev_velocities[j], prev_accelerations[j],
00690 positions[j], velocities[j], accelerations[j],
00691 durations[i],
00692 seg.splines[j].coef);
00693 }
00694 else if (prev_velocities.size() > 0 && velocities.size() > 0)
00695 {
00696 getCubicSplineCoefficients(
00697 prev_positions[j], prev_velocities[j],
00698 positions[j], velocities[j],
00699 durations[i],
00700 seg.splines[j].coef);
00701 seg.splines[j].coef.resize(6, 0.0);
00702 }
00703 else
00704 {
00705 seg.splines[j].coef[0] = prev_positions[j];
00706 if (durations[i] == 0.0)
00707 seg.splines[j].coef[1] = 0.0;
00708 else
00709 seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
00710 seg.splines[j].coef[2] = 0.0;
00711 seg.splines[j].coef[3] = 0.0;
00712 seg.splines[j].coef[4] = 0.0;
00713 seg.splines[j].coef[5] = 0.0;
00714 }
00715 }
00716
00717
00718 traj.push_back(seg);
00719
00720
00721
00722 prev_positions = positions;
00723 prev_velocities = velocities;
00724 prev_accelerations = accelerations;
00725 }
00726
00727
00728
00729 if (!new_traj_ptr)
00730 {
00731 ROS_ERROR("The new trajectory was null!");
00732 return;
00733 }
00734
00735 ROS_DEBUG("The new trajectory has %d segments", (int)traj.size());
00736
00737 std::vector<sr_robot_msgs::joint> joint_vector_traj;
00738 unsigned int controller_pub_idx=0;
00739
00740 std_msgs::Float64 target_msg;
00741 sr_robot_msgs::sendupdate sendupdate_msg_traj;
00742
00743
00744
00745
00746 joint_vector_traj.clear();
00747
00748 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00749 {
00750 sr_robot_msgs::joint joint;
00751 joint.joint_name = joint_names_[i];
00752 joint_vector_traj.push_back(joint);
00753 }
00754
00755 if(use_sendupdate)
00756 {
00757 sendupdate_msg_traj.sendupdate_length = joint_vector_traj.size();
00758 ROS_DEBUG("Trajectory received: %d joints / %d msg length", (int)msg->joint_names.size(), sendupdate_msg_traj.sendupdate_length);
00759 }
00760
00761 ros::Rate tmp_rate(1.0);
00762
00763
00764
00765
00766
00767 ros::Duration sleeping_time(0.0);
00768 ROS_DEBUG("Entering the execution loop");
00769
00770 while(ros::ok())
00771 {
00772 ros::Time time = ros::Time::now();
00773 ros::Duration dt = time - last_time_;
00774 last_time_ = time;
00775
00776
00777 ROS_DEBUG("Find current segment");
00778
00779
00780 int seg = -1;
00781 while (seg + 1 < (int)traj.size() && traj[seg+1].start_time < time.toSec())
00782 {
00783 ++seg;
00784 }
00785
00786
00787 if( (traj[traj.size()-1].start_time+traj[traj.size()-1].duration) < time.toSec())
00788 {
00789 ROS_DEBUG("trajectory is finished %f<%f",(traj[traj.size()-1].start_time+traj[traj.size()-1].duration),time.toSec());
00790 break;
00791 }
00792
00793 if (seg == -1)
00794 {
00795 if (traj.size() == 0)
00796 ROS_ERROR("No segments in the trajectory");
00797 else
00798 ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
00799 return;
00800 }
00801
00802
00803 ROS_DEBUG("Sample the trajectory");
00804
00805 for (size_t i = 0; i < q.size(); ++i)
00806 {
00807 sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
00808 time.toSec() - traj[seg].start_time,
00809 q[i], qd[i], qdd[i]);
00810 }
00811 ROS_DEBUG("Sampled the trajectory");
00812
00813 if (!ros::ok())
00814 {
00815 ROS_INFO("Joint Trajectory Stopping");
00816
00817
00818 success = false;
00819 break;
00820 }
00821 ROS_DEBUG("Update the targets");
00822
00823 sensor_msgs::JointState desired_joint_state_msg;
00824 for(unsigned int i = 0; i < joint_names_.size(); ++i)
00825 {
00826 desired_joint_state_msg.name.push_back(joint_names_[i]);
00827 desired_joint_state_msg.position.push_back(q[i]);
00828 desired_joint_state_msg.velocity.push_back(qd[i]);
00829 desired_joint_state_msg.effort.push_back(0.0);
00830 if(!use_sendupdate)
00831 {
00832 if((controller_pub_idx=jointPubIdxMap[ joint_names_[i] ])>0)
00833 {
00834 target_msg.data=q[i];
00835 controller_publishers.at(controller_pub_idx-1).publish(target_msg);
00836 }
00837 }
00838 else
00839 {
00840 joint_vector_traj[i].joint_target = q[i] * 57.3;
00841 ROS_DEBUG("traj[%s]: %f", joint_vector_traj[i].joint_name.c_str(), joint_vector_traj[i].joint_target);
00842 }
00843 }
00844 ROS_DEBUG("Targets updated");
00845
00846 desired_joint_state_msg.header.stamp = ros::Time::now();
00847 desired_joint_state_pusblisher.publish(desired_joint_state_msg);
00848
00849 if(use_sendupdate)
00850 {
00851 sendupdate_msg_traj.sendupdate_list = joint_vector_traj;
00852 sr_arm_target_pub.publish(sendupdate_msg_traj);
00853 sr_hand_target_pub.publish(sendupdate_msg_traj);
00854 }
00855
00856 ROS_DEBUG("Now sleep and loop");
00857 sleeping_time.sleep();
00858 sleeping_time = ros::Duration(0.1);
00859 ROS_DEBUG("redo loop");
00860 }
00861
00862 return;
00863 }
00864
00865 }
00866
00867
00868 int main(int argc, char** argv)
00869 {
00870 ros::init(argc, argv, "sr_joint_trajectory_action_controller");
00871
00872 ros::AsyncSpinner spinner(1);
00873 spinner.start();
00874 shadowrobot::JointTrajectoryActionController jac;
00875 ros::spin();
00876
00877
00878 return 0;
00879 }
00880
00881
00882
00883
00884
00885
00886
00887
00888
00889