simple_spline_trajectory.cpp
Go to the documentation of this file.
00001 // http://www.ros.org/wiki/pr2_controllers/Tutorials/Moving%20the%20arm%20using%20the%20Joint%20Trajectory%20Action
00002 
00003 #include <ros/ros.h>
00004 #include <control_msgs/FollowJointTrajectoryAction.h>
00005 #include <actionlib/client/simple_action_client.h>
00006 
00007 typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
00008 
00009 class ShadowTrajectory
00010 {
00011 private:
00012   // Action client for the joint trajectory action
00013   // used to trigger the arm movement action
00014   TrajClient* traj_client_;
00015 
00016 public:
00018   ShadowTrajectory()
00019   {
00020     // tell the action client that we want to spin a thread by default
00021     traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
00022 
00023     // wait for action server to come up
00024     while(!traj_client_->waitForServer(ros::Duration(5.0))){
00025       ROS_INFO("Waiting for the joint_trajectory_action server");
00026     }
00027   }
00028 
00030   ~ShadowTrajectory()
00031   {
00032     delete traj_client_;
00033   }
00034 
00036   void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
00037   {
00038     // When to start the trajectory: 1s from now
00039     goal.trajectory.header.stamp = ros::Time::now()- ros::Duration(0.01);
00040     traj_client_->sendGoal(goal);
00041   }
00042 
00044   void waitTrajectory() {
00045     while(!getState().isDone() && ros::ok()) { usleep(50000); }
00046   }
00047 
00049 
00054   control_msgs::FollowJointTrajectoryGoal arm_movement()
00055   {
00056     //our goal variable
00057     control_msgs::FollowJointTrajectoryGoal goal;
00058 
00059     // First, the joint names, which apply to all waypoints
00060     goal.trajectory.joint_names.push_back("ShoulderJRotate");
00061     goal.trajectory.joint_names.push_back("ShoulderJSwing");
00062     goal.trajectory.joint_names.push_back("ElbowJSwing");
00063     goal.trajectory.joint_names.push_back("ElbowJRotate");
00064     goal.trajectory.joint_names.push_back("WRJ2");
00065     goal.trajectory.joint_names.push_back("WRJ1");
00066 
00067     // Set number of waypoints in this goal trajectory
00068     goal.trajectory.points.resize(3);
00069 
00070     // First trajectory point
00071     // Positions
00072     int ind = 0;
00073     goal.trajectory.points[ind].positions.resize(6);
00074     goal.trajectory.points[ind].positions[0] = 0.0;
00075     goal.trajectory.points[ind].positions[1] = 0.0;
00076     goal.trajectory.points[ind].positions[2] = 1.57;
00077     goal.trajectory.points[ind].positions[3] = -0.78;
00078     goal.trajectory.points[ind].positions[4] = 0.0;
00079     goal.trajectory.points[ind].positions[5] = 0.0;
00080 
00081     // Points also have velocities
00082     goal.trajectory.points[ind].velocities.resize(6);
00083     goal.trajectory.points[ind].velocities[0] = 0.0;
00084     goal.trajectory.points[ind].velocities[1] = 0.0;
00085     goal.trajectory.points[ind].velocities[2] = 0.0;
00086     goal.trajectory.points[ind].velocities[3] = 0.0;
00087     goal.trajectory.points[ind].velocities[4] = 0.0;
00088     goal.trajectory.points[ind].velocities[5] = 0.0;
00089 
00090     // To be reached 4.0 second after starting along the trajectory
00091     goal.trajectory.points[ind].time_from_start = ros::Duration(10.0);
00092 
00093     // 2nd trajectory point
00094     ind += 1;
00095     goal.trajectory.points[ind].positions.resize(6);
00096     goal.trajectory.points[ind].positions[0] = 0.4;
00097     goal.trajectory.points[ind].positions[1] = 0.78;
00098     goal.trajectory.points[ind].positions[2] = 0.78;
00099     goal.trajectory.points[ind].positions[3] = 0.78;
00100     goal.trajectory.points[ind].positions[4] = 0.1;
00101     goal.trajectory.points[ind].positions[5] = 0.1;
00102 
00103     // Points also have velocities
00104     goal.trajectory.points[ind].velocities.resize(6);
00105     goal.trajectory.points[ind].velocities[0] = 0.3;
00106     goal.trajectory.points[ind].velocities[1] = 0.0;
00107     goal.trajectory.points[ind].velocities[2] = 0.0;
00108     goal.trajectory.points[ind].velocities[3] = 0.0;
00109     goal.trajectory.points[ind].velocities[4] = 0.0;
00110     goal.trajectory.points[ind].velocities[5] = 0.0;
00111 
00112     // To be reached 4.0 second after starting along the trajectory
00113     goal.trajectory.points[ind].time_from_start = ros::Duration(16.0);
00114     // 3rd trajectory point
00115     ind += 1;
00116     goal.trajectory.points[ind].positions.resize(6);
00117     goal.trajectory.points[ind].positions[0] = 0.6;
00118     goal.trajectory.points[ind].positions[1] = 0.48;
00119     goal.trajectory.points[ind].positions[2] = 0.78;
00120     goal.trajectory.points[ind].positions[3] = 0.78;
00121     goal.trajectory.points[ind].positions[4] = 0.1;
00122     goal.trajectory.points[ind].positions[5] = 0.1;
00123 
00124     // Points also have velocities
00125     goal.trajectory.points[ind].velocities.resize(6);
00126     goal.trajectory.points[ind].velocities[0] = 0.0;
00127     goal.trajectory.points[ind].velocities[1] = 0.0;
00128     goal.trajectory.points[ind].velocities[2] = 0.0;
00129     goal.trajectory.points[ind].velocities[3] = 0.0;
00130     goal.trajectory.points[ind].velocities[4] = 0.0;
00131     goal.trajectory.points[ind].velocities[5] = 0.0;
00132 
00133     // To be reached 4.0 second after starting along the trajectory
00134     goal.trajectory.points[ind].time_from_start = ros::Duration(20.0);
00135 
00136     return goal;
00137   }
00138 
00140   actionlib::SimpleClientGoalState getState()
00141   {
00142     return traj_client_->getState();
00143   }
00144 
00145 };
00146 
00147 int main(int argc, char** argv)
00148 {
00149   // Init the ROS node
00150   ros::init(argc, argv, "shadow_trajectory_driver");
00151 
00152   ShadowTrajectory sj;
00153   sj.startTrajectory(sj.arm_movement());
00154   sj.waitTrajectory();
00155 }
00156 
00157 // vim: sw=2:ts=2


sr_mechanism_controllers
Author(s): Ugo Cupcic / ugo@shadowrobot.com , contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:06:50