43 #include <control_msgs/FollowJointTrajectoryAction.h>
44 #include <trajectory_msgs/JointTrajectoryPoint.h>
69 template <
class Segment>
73 typedef typename Segment::Scalar
Scalar;
74 typedef typename Segment::Time
Time;
79 struct State :
public Segment::State
81 typedef typename Segment::State::Scalar
Scalar;
93 State(
const trajectory_msgs::JointTrajectoryPoint& point,
94 const std::vector<Scalar>& position_offset = std::vector<Scalar>())
96 init(point, position_offset);
99 void init(
const trajectory_msgs::JointTrajectoryPoint& point,
100 const std::vector<Scalar>& position_offset = std::vector<Scalar>())
102 using std::invalid_argument;
104 const unsigned int joint_dim = point.positions.size();
107 if (!
isValid(point, joint_dim))
109 throw(invalid_argument(
"Size mismatch in trajectory point position, velocity or acceleration data."));
111 if (!position_offset.empty() && joint_dim != position_offset.size())
113 throw(invalid_argument(
"Size mismatch between trajectory point "
114 "and vector specifying whether joints wrap around."));
118 if (!point.positions.empty()) {this->position.resize(joint_dim);}
119 if (!point.velocities.empty()) {this->velocity.resize(joint_dim);}
120 if (!point.accelerations.empty()) {this->acceleration.resize(joint_dim);}
122 for (
unsigned int i = 0; i < joint_dim; ++i)
125 const Scalar offset = position_offset.empty() ? 0.0 : position_offset[i];
127 if (!point.positions.empty()) {this->position[i] = point.positions[i] + offset;}
128 if (!point.velocities.empty()) {this->velocity[i] = point.velocities[i];}
129 if (!point.accelerations.empty()) {this->acceleration[i] = point.accelerations[i];}
132 this->time_from_start = point.time_from_start.toSec();
145 const State& start_state,
146 const Time& end_time,
147 const State& end_state)
151 Segment::init(start_time, start_state, end_time, end_state);
166 const trajectory_msgs::JointTrajectoryPoint& start_point,
167 const trajectory_msgs::JointTrajectoryPoint& end_point,
168 const std::vector<Scalar>& position_offset = std::vector<Scalar>())
172 if (start_point.positions.size() != end_point.positions.size())
174 throw(std::invalid_argument(
"Can't construct segment from ROS message: "
175 "Start/end points data size mismatch."));
178 const Time start_time = (traj_start_time + start_point.time_from_start).toSec();
179 const Time end_time = (traj_start_time + end_point.time_from_start).toSec();
183 const State start_state(start_point, position_offset);
184 const State end_state(end_point, position_offset);
186 this->
init(start_time, start_state,
187 end_time, end_state);
189 catch(
const std::invalid_argument& ex)
191 std::string msg =
"Can't construct segment from ROS message: " + std::string(ex.what());
192 throw(std::invalid_argument(msg));
222 template <
class Scalar>
224 const Scalar& next_position,
225 const bool& angle_wraparound)
228 Scalar pos_offset = 0.0;
230 if (angle_wraparound)
235 if (std::abs(std::abs(dist) - M_PI) < 1e-9)
237 dist = next_position > prev_position ? std::abs(dist) : -std::abs(dist);
239 pos_offset = (prev_position + dist) - next_position;