← Back to branch summary

~guihome/sr-ros-interface/compatibility_manipstack2

« back to all changes in this revision

Viewing changes to sr_mechanism_controllers/example/simple_spline_trajectory.cpp

  • Committer: Ugo Cupcic
  • Date: 2012-07-20 13:18:06 UTC
  • mfrom: (366.1.1 shadow_robot)
  • Revision ID: ugo@shadowrobot.com-20120720131806-t3cxkv09p7cpgdyy
Extracted and adapted modifications needed to shadow stack from the manip-electric version to be compatible with sr-manipulation electric and to run in real. Works without manipulation stack also, but requires some testing of all the launch files. New dae meshes are currently with wrong colors but are working.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_mechanism_controllers/example/simple_spline_trajectory.cpp
 
1
// http://www.ros.org/wiki/pr2_controllers/Tutorials/Moving%20the%20arm%20using%20the%20Joint%20Trajectory%20Action
 
2
 
 
3
#include <ros/ros.h>
 
4
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
 
5
#include <actionlib/client/simple_action_client.h>
 
6
 
 
7
typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;
 
8
 
 
9
class ShadowTrajectory
 
10
{
 
11
private:
 
12
  // Action client for the joint trajectory action
 
13
  // used to trigger the arm movement action
 
14
  TrajClient* traj_client_;
 
15
 
 
16
public:
 
17
  //! Initialize the action client and wait for action server to come up
 
18
  ShadowTrajectory()
 
19
  {
 
20
    // tell the action client that we want to spin a thread by default
 
21
    traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
 
22
 
 
23
    // wait for action server to come up
 
24
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
 
25
      ROS_INFO("Waiting for the joint_trajectory_action server");
 
26
    }
 
27
  }
 
28
 
 
29
  //! Clean up the action client
 
30
  ~ShadowTrajectory()
 
31
  {
 
32
    delete traj_client_;
 
33
  }
 
34
 
 
35
  //! Sends the command to start a given trajectory
 
36
  void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
 
37
  {
 
38
    // When to start the trajectory: 1s from now
 
39
    goal.trajectory.header.stamp = ros::Time::now(); //+ ros::Duration(1.0);
 
40
    traj_client_->sendGoal(goal);
 
41
  }
 
42
 
 
43
  //! Wait for currently running trajectory to finish
 
44
  void waitTrajectory() {
 
45
    while(!getState().isDone() && ros::ok()) { usleep(50000); }
 
46
  }
 
47
 
 
48
  //! Generates a simple trajectory to move the arm.
 
49
  /*! Note that this trajectory contains three waypoints, joined together
 
50
      as a single trajectory. Alternatively, each of these waypoints could
 
51
      be in its own trajectory - a trajectory can have one or more waypoints
 
52
      depending on the desired application.
 
53
  */
 
54
  pr2_controllers_msgs::JointTrajectoryGoal arm_movement()
 
55
  {
 
56
    //our goal variable
 
57
    pr2_controllers_msgs::JointTrajectoryGoal goal;
 
58
 
 
59
    // First, the joint names, which apply to all waypoints
 
60
    goal.trajectory.joint_names.push_back("ShoulderJRotate");
 
61
    goal.trajectory.joint_names.push_back("ShoulderJSwing");
 
62
    goal.trajectory.joint_names.push_back("ElbowJSwing");
 
63
    goal.trajectory.joint_names.push_back("ElbowJRotate");
 
64
    goal.trajectory.joint_names.push_back("WRJ2");
 
65
    goal.trajectory.joint_names.push_back("WRJ1");
 
66
 
 
67
    // Set number of waypoints in this goal trajectory
 
68
    goal.trajectory.points.resize(3);
 
69
 
 
70
    // First trajectory point
 
71
    // Positions
 
72
    int ind = 0;
 
73
    goal.trajectory.points[ind].positions.resize(6);
 
74
    goal.trajectory.points[ind].positions[0] = 0.0;
 
75
    goal.trajectory.points[ind].positions[1] = 0.0;
 
76
    goal.trajectory.points[ind].positions[2] = 1.57;
 
77
    goal.trajectory.points[ind].positions[3] = -0.78;
 
78
    goal.trajectory.points[ind].positions[4] = 0.0;
 
79
    goal.trajectory.points[ind].positions[5] = 0.0;
 
80
    
 
81
    // Points also have velocities
 
82
    goal.trajectory.points[ind].velocities.resize(6);
 
83
    goal.trajectory.points[ind].velocities[0] = 0.0;
 
84
    goal.trajectory.points[ind].velocities[1] = 0.0;
 
85
    goal.trajectory.points[ind].velocities[2] = 0.0;
 
86
    goal.trajectory.points[ind].velocities[3] = 0.0;
 
87
    goal.trajectory.points[ind].velocities[4] = 0.0;
 
88
    goal.trajectory.points[ind].velocities[5] = 0.0;
 
89
 
 
90
    // To be reached 4.0 second after starting along the trajectory
 
91
    goal.trajectory.points[ind].time_from_start = ros::Duration(10.0);
 
92
 
 
93
    // 2nd trajectory point
 
94
    ind += 1;
 
95
    goal.trajectory.points[ind].positions.resize(6);
 
96
    goal.trajectory.points[ind].positions[0] = 0.4;
 
97
    goal.trajectory.points[ind].positions[1] = 0.78;
 
98
    goal.trajectory.points[ind].positions[2] = 0.78;
 
99
    goal.trajectory.points[ind].positions[3] = 0.78;
 
100
    goal.trajectory.points[ind].positions[4] = 0.1;
 
101
    goal.trajectory.points[ind].positions[5] = 0.1;
 
102
 
 
103
    // Points also have velocities
 
104
    goal.trajectory.points[ind].velocities.resize(6);
 
105
    goal.trajectory.points[ind].velocities[0] = 0.3;
 
106
    goal.trajectory.points[ind].velocities[1] = 0.0;
 
107
    goal.trajectory.points[ind].velocities[2] = 0.0;
 
108
    goal.trajectory.points[ind].velocities[3] = 0.0;
 
109
    goal.trajectory.points[ind].velocities[4] = 0.0;
 
110
    goal.trajectory.points[ind].velocities[5] = 0.0;
 
111
 
 
112
    // To be reached 4.0 second after starting along the trajectory
 
113
    goal.trajectory.points[ind].time_from_start = ros::Duration(16.0);
 
114
    // 3rd trajectory point
 
115
    ind += 1;
 
116
    goal.trajectory.points[ind].positions.resize(6);
 
117
    goal.trajectory.points[ind].positions[0] = 0.6;
 
118
    goal.trajectory.points[ind].positions[1] = 0.48;
 
119
    goal.trajectory.points[ind].positions[2] = 0.78;
 
120
    goal.trajectory.points[ind].positions[3] = 0.78;
 
121
    goal.trajectory.points[ind].positions[4] = 0.1;
 
122
    goal.trajectory.points[ind].positions[5] = 0.1;
 
123
 
 
124
    // Points also have velocities
 
125
    goal.trajectory.points[ind].velocities.resize(6);
 
126
    goal.trajectory.points[ind].velocities[0] = 0.0;
 
127
    goal.trajectory.points[ind].velocities[1] = 0.0;
 
128
    goal.trajectory.points[ind].velocities[2] = 0.0;
 
129
    goal.trajectory.points[ind].velocities[3] = 0.0;
 
130
    goal.trajectory.points[ind].velocities[4] = 0.0;
 
131
    goal.trajectory.points[ind].velocities[5] = 0.0;
 
132
 
 
133
    // To be reached 4.0 second after starting along the trajectory
 
134
    goal.trajectory.points[ind].time_from_start = ros::Duration(20.0);
 
135
 
 
136
    return goal;
 
137
  }
 
138
 
 
139
  //! Returns the current state of the action
 
140
  actionlib::SimpleClientGoalState getState()
 
141
  {
 
142
    return traj_client_->getState();
 
143
  }
 
144
 
 
145
};
 
146
 
 
147
int main(int argc, char** argv)
 
148
{
 
149
  // Init the ROS node
 
150
  ros::init(argc, argv, "shadow_trajectory_driver");
 
151
 
 
152
  ShadowTrajectory sj;
 
153
  sj.startTrajectory(sj.arm_movement());
 
154
  sj.waitTrajectory();
 
155
}
 
156
 
 
157
// vim: sw=2:ts=2