$search
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 // These functions are pulled from the spline_smoother package. 00022 // They've been moved here to avoid depending on packages that aren't 00023 // mature yet. 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 // create powers of time: 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 // boost::bind(&JointTrajectoryActionController::cancelCB, this, _1) )); 00126 00127 std::vector<std::string> joint_labels; 00128 00129 /* // Gets all of the joints 00130 XmlRpc::XmlRpcValue joint_names; 00131 00132 if (!nh.getParam("joints", joint_names)) 00133 { 00134 ROS_ERROR("No joints given. (namespace: %s)", nh.getNamespace().c_str()); 00135 return; 00136 } 00137 00138 if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) 00139 { 00140 ROS_ERROR("Malformed joint specification. (namespace: %s)", nh.getNamespace().c_str()); 00141 return ; 00142 } 00143 00144 for (int i = 0; i < joint_names.size(); ++i) 00145 { 00146 XmlRpcValue &name_value = joint_names[i]; 00147 00148 if (name_value.getType() != XmlRpcValue::TypeString) 00149 { 00150 ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)", 00151 nh.getNamespace().c_str()); 00152 return ; 00153 } 00154 00155 joint_labels.push_back((std::string)name_value); 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 //look for controllers and build controller name to joint map 00167 if( ros::service::waitForService("sr_controller_manager/list_controllers",20000) ) 00168 { 00169 use_sendupdate=false; 00170 ros::ServiceClient controller_list_client = nh.serviceClient<pr2_mechanism_msgs::ListControllers>("sr_controller_manager/list_controllers"); 00171 pr2_mechanism_msgs::ListControllers controller_list; 00172 std::string controlled_joint_name; 00173 00174 // query the list 00175 controller_list_client.call(controller_list); 00176 // build the map 00177 for (unsigned int i=0; i<controller_list.response.controllers.size() ; i++ ) 00178 { 00179 if(controller_list.response.state[i]=="running") 00180 { 00181 if (nh.getParam("/"+controller_list.response.controllers[i]+"/joint", controlled_joint_name)) 00182 { 00183 ROS_DEBUG("controller %d:%s controls joint %s\n",i,controller_list.response.controllers[i].c_str(),controlled_joint_name.c_str()); 00184 jointControllerMap[controlled_joint_name]= controller_list.response.controllers[i] ; 00185 } 00186 } 00187 } 00188 00189 //build controller publisher to joint map 00190 for(unsigned int i=0; i < joint_labels.size(); i ++) 00191 { 00192 std::string controller_name=jointControllerMap[joint_labels[i]]; 00193 00194 if(controller_name.compare("")!=0) //if exist, set idx to controller number + 1 00195 { 00196 controller_publishers.push_back(nh.advertise<std_msgs::Float64>("/"+jointControllerMap[joint_labels[i]]+"/command", 2)); 00197 jointPubIdxMap[joint_labels[i]]=controller_publishers.size(); //we want the index to be above zero all the time 00198 } 00199 else // else put a zero in order to detect when this is empty 00200 { 00201 ROS_WARN("Could not find a controller for joint %s",joint_labels[i].c_str()); 00202 jointPubIdxMap[joint_labels[i]]=0; //we want the index to be above zero all the time 00203 } 00204 } 00205 } 00206 else 00207 { 00208 ROS_WARN("sr_controller_manager not found, switching back to sendupdates"); 00209 use_sendupdate=true; 00210 sr_arm_target_pub = nh.advertise<sr_robot_msgs::sendupdate>("/sr_arm/sendupdate", 2); 00211 sr_hand_target_pub = nh.advertise<sr_robot_msgs::sendupdate>("/srh/sendupdate", 2); 00212 } 00213 00214 ROS_INFO("waiting for getJointState"); 00215 if( ros::service::waitForService("getJointState",20000)) 00216 { 00217 // open persistent link to joint_state service 00218 joint_state_client = nh.serviceClient<sr_utilities::getJointState>("getJointState",true); 00219 } 00220 else 00221 { 00222 ROS_ERROR("Cannot access service: Check if joint_state service is launched"); 00223 ros::shutdown(); 00224 exit(-1); 00225 } 00226 ROS_INFO("Got getJointState"); 00227 joint_names_=joint_labels; 00228 00229 q.resize(joint_names_.size()); 00230 qd.resize(joint_names_.size()); 00231 qdd.resize(joint_names_.size()); 00232 00233 desired_joint_state_pusblisher = nh.advertise<sensor_msgs::JointState> ("/desired_joint_states", 2); 00234 00235 command_sub = nh.subscribe("command", 1, &JointTrajectoryActionController::commandCB, this); 00236 ROS_INFO("Listening to commands"); 00237 00238 action_server->start(); 00239 ROS_INFO("Action server started"); 00240 } 00241 00242 JointTrajectoryActionController::~JointTrajectoryActionController() 00243 { 00244 00245 } 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 //fill up the lookup map with updated positions 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 // Finds the end conditions of the final segment 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[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 // ------ Tacks on the new segments 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 // no continuous joints so do not check if we should wrap 00331 00332 // extract the traj 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 // Checks that the incoming segment has the right number of elements. 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 // Re-orders the joints in the command to match the internal joint order. 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 < joint_names_.size(); ++j) 00366 { 00367 if (!accelerations.empty()) accelerations[j] = goal->trajectory.points[i].accelerations[j]; 00368 if (!velocities.empty()) velocities[j] = goal->trajectory.points[i].velocities[j]; 00369 if (!positions.empty()) positions[j] = goal->trajectory.points[i].positions[j]; 00370 } 00371 00372 // Converts the boundary conditions to splines. 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 // Pushes the splines onto the end of the new trajectory. 00406 00407 traj.push_back(seg); 00408 00409 // Computes the starting conditions for the next segment 00410 00411 prev_positions = positions; 00412 prev_velocities = velocities; 00413 prev_accelerations = accelerations; 00414 } 00415 00416 // ------ Commits the new trajectory 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 //only one of these 2 will be used 00429 std_msgs::Float64 target_msg; 00430 sr_robot_msgs::sendupdate sendupdate_msg_traj; 00431 00432 //initializes the joint names 00433 //TODO check if traj only contains joint that we control 00434 //joint_names_ = goal->trajectory.joint_names; 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 // std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points = goal->trajectory.points; 00453 // trajectory_msgs::JointTrajectoryPoint trajectory_step; 00454 00455 //loop through the steps 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 // ------ Finds the current segment 00467 ROS_DEBUG("Find current segment"); 00468 00469 // Determines which segment of the trajectory to use. (Not particularly realtime friendly). 00470 int seg = -1; 00471 while (seg + 1 < (int)traj.size() && traj[seg+1].start_time < time.toSec()) 00472 { 00473 ++seg; 00474 } 00475 00476 // if the last trajectory is already in the past, stop the servoing 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 // ------ Trajectory Sampling 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 //check if preempted 00504 if (action_server->isPreemptRequested() || !ros::ok()) 00505 { 00506 ROS_INFO("Joint Trajectory Action Preempted"); 00507 // set the action state to preempted 00508 action_server->setPreempted(); 00509 success = false; 00510 break; 00511 } 00512 ROS_DEBUG("Update the targets"); 00513 //update the targets and publish target joint_states 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) // if a controller exist for this joint 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 %d 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 // Finds the end conditions of the final segment 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 // ------ Tacks on the new segments 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 // no continuous joints so do not check if we should wrap 00636 00637 // extract the traj 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 %d 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 // Checks that the incoming segment has the right number of elements. 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 // Re-orders the joints in the command to match the internal joint order. 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 // Converts the boundary conditions to splines. 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 // Pushes the splines onto the end of the new trajectory. 00717 00718 traj.push_back(seg); 00719 00720 // Computes the starting conditions for the next segment 00721 00722 prev_positions = positions; 00723 prev_velocities = velocities; 00724 prev_accelerations = accelerations; 00725 } 00726 00727 // ------ Commits the new trajectory 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 //only one of these 2 will be used 00740 std_msgs::Float64 target_msg; 00741 sr_robot_msgs::sendupdate sendupdate_msg_traj; 00742 00743 //initializes the joint names 00744 //TODO check if traj only contains joint that we control 00745 //joint_names_ = goal->trajectory.joint_names; 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 // std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points = goal->trajectory.points; 00764 // trajectory_msgs::JointTrajectoryPoint trajectory_step; 00765 00766 //loop through the steps 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 // ------ Finds the current segment 00777 ROS_DEBUG("Find current segment"); 00778 00779 // Determines which segment of the trajectory to use. (Not particularly realtime friendly). 00780 int seg = -1; 00781 while (seg + 1 < (int)traj.size() && traj[seg+1].start_time < time.toSec()) 00782 { 00783 ++seg; 00784 } 00785 00786 // if the last trajectory is already in the past, stop the servoing 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 // ------ Trajectory Sampling 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 //check if preempted 00813 if (!ros::ok()) 00814 { 00815 ROS_INFO("Joint Trajectory Stopping"); 00816 // set the action state to preempted 00817 //action_server->setPreempted(); 00818 success = false; 00819 break; 00820 } 00821 ROS_DEBUG("Update the targets"); 00822 //update the targets and publish target joint_states 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) // if a controller exist for this joint 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); //Use 1 thread 00873 spinner.start(); 00874 shadowrobot::JointTrajectoryActionController jac; 00875 ros::spin(); 00876 //ros::waitForShutdown(); 00877 00878 return 0; 00879 } 00880 00881 00882 /* For the emacs weenies in the crowd. 00883 Local Variables: 00884 c-basic-offset: 2 00885 End: 00886 */ 00887