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