min_max_trajectory.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <control_msgs/FollowJointTrajectoryAction.h>
00003 #include <actionlib/client/simple_action_client.h>
00004 
00005 typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajClient;
00006 
00007 static const double MIN_POSITIONS [5] = {-3.025528, -0.135228, -1.0, -2.033309, -2.993240};
00008 static const double MAX_POSITIONS [5] = {2.891097, 2.168572, 2.054223, 1.876133, 2.870985};
00009 
00010 static const size_t NUM_TRAJ_POINTS = 7;
00011 static const size_t NUM_JOINTS = 5;
00012 
00013 class RobotArm
00014 {
00015 private:
00016 
00017   // Action client for the joint trajectory action
00018   // used to trigger the arm movement action
00019   TrajClient* traj_client_;
00020 
00021 public:
00023   RobotArm()
00024   {
00025     // tell the action client that we want to spin a thread by default
00026     traj_client_ = new TrajClient("katana_arm_controller/follow_joint_trajectory", true);
00027 
00028     // wait for action server to come up
00029     while (!traj_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
00030     {
00031       ROS_INFO("Waiting for the follow_joint_trajectory server");
00032     }
00033   }
00034 
00036   ~RobotArm()
00037   {
00038     delete traj_client_;
00039   }
00040 
00042   void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
00043   {
00044     // When to start the trajectory: 1s from now
00045     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00046     traj_client_->sendGoal(goal);
00047   }
00048 
00049   control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory(size_t moving_joint)
00050   {
00051     //our goal variable
00052     control_msgs::FollowJointTrajectoryGoal goal;
00053 
00054     // First, the joint names, which apply to all waypoints
00055     goal.trajectory.joint_names.push_back("katana_motor1_pan_joint");
00056     goal.trajectory.joint_names.push_back("katana_motor2_lift_joint");
00057     goal.trajectory.joint_names.push_back("katana_motor3_lift_joint");
00058     goal.trajectory.joint_names.push_back("katana_motor4_lift_joint");
00059     goal.trajectory.joint_names.push_back("katana_motor5_wrist_roll_joint");
00060 
00061     goal.trajectory.points.resize(NUM_TRAJ_POINTS);
00062 
00063     // First trajectory point (should be equal to current position of joints)
00064     size_t seg = 0;
00065     goal.trajectory.points[seg].positions.resize(NUM_JOINTS);
00066     goal.trajectory.points[seg].velocities.resize(NUM_JOINTS);
00067     for (size_t j = 0; j < NUM_JOINTS; ++j)
00068     {
00069       goal.trajectory.points[seg].positions[j] = 0.0;
00070       goal.trajectory.points[seg].velocities[j] = 0.0;
00071     }
00072 
00073     // Remaining trajectory points
00074     for (size_t seg = 1; seg < NUM_TRAJ_POINTS; seg++)
00075     {
00076       goal.trajectory.points[seg].positions.resize(NUM_JOINTS);
00077       goal.trajectory.points[seg].velocities.resize(NUM_JOINTS);
00078       for (size_t j = 0; j < NUM_JOINTS; ++j)
00079       {
00080         goal.trajectory.points[seg].positions[j] = goal.trajectory.points[seg - 1].positions[j];
00081         goal.trajectory.points[seg].velocities[j] = 0.0;
00082       }
00083     }
00084 
00085     // overwrite with the positions for the moving joint
00086     // move to min, hold it there, move to max, hold it there, move to 0, hold it there
00087     assert(NUM_TRAJ_POINTS == 7);
00088     goal.trajectory.points[0].positions[moving_joint] = 0.0;
00089     goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
00090 
00091     goal.trajectory.points[1].positions[moving_joint] = MIN_POSITIONS[moving_joint];
00092     goal.trajectory.points[1].time_from_start = ros::Duration(2.5);
00093 
00094     goal.trajectory.points[2].positions[moving_joint] = MIN_POSITIONS[moving_joint];
00095     goal.trajectory.points[2].time_from_start = ros::Duration(5.5);
00096 
00097     goal.trajectory.points[3].positions[moving_joint] = MAX_POSITIONS[moving_joint];
00098     goal.trajectory.points[3].time_from_start = ros::Duration(10.5);
00099 
00100     goal.trajectory.points[4].positions[moving_joint] = MAX_POSITIONS[moving_joint];
00101     goal.trajectory.points[4].time_from_start = ros::Duration(13.5);
00102 
00103     goal.trajectory.points[5].positions[moving_joint] = 0.0;
00104     goal.trajectory.points[5].time_from_start = ros::Duration(16.0);
00105 
00106     goal.trajectory.points[6].positions[moving_joint] = 0.0;
00107     goal.trajectory.points[6].time_from_start = ros::Duration(19.0);
00108 
00109     //we are done; return the goal
00110     return goal;
00111   }
00112 
00114   actionlib::SimpleClientGoalState getState()
00115   {
00116     return traj_client_->getState();
00117   }
00118 
00119 };
00120 
00121 int main(int argc, char** argv)
00122 {
00123   // Init the ROS node
00124   ros::init(argc, argv, "min_max_trajectory");
00125 
00126   RobotArm arm;
00127   while (ros::ok())
00128   {
00129     for (size_t joint = 0; joint < NUM_JOINTS; joint++)
00130     {
00131       // Start the trajectory
00132       arm.startTrajectory(arm.armExtensionTrajectory(joint));
00133       // Wait for trajectory completion
00134       while (!arm.getState().isDone() && ros::ok())
00135       {
00136         usleep(50000);
00137       }
00138     }
00139   }
00140 
00141 }
00142 


katana_arm_gazebo
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:53