30 #ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_SEGMENT_H 31 #define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_SEGMENT_H 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];}
143 const State& start_state,
144 const Time& end_time,
145 const State& end_state)
149 Segment::init(start_time, start_state, end_time, end_state);
164 const trajectory_msgs::JointTrajectoryPoint& start_point,
165 const trajectory_msgs::JointTrajectoryPoint& end_point,
166 const std::vector<Scalar>& position_offset = std::vector<Scalar>())
170 if (start_point.positions.size() != end_point.positions.size())
172 throw(std::invalid_argument(
"Can't construct segment from ROS message: " 173 "Start/end points data size mismatch."));
176 const Time start_time = (traj_start_time + start_point.time_from_start).toSec();
177 const Time end_time = (traj_start_time + end_point.time_from_start).toSec();
181 const State start_state(start_point, position_offset);
182 const State end_state(end_point, position_offset);
184 this->
init(start_time, start_state,
185 end_time, end_state);
187 catch(
const std::invalid_argument& ex)
189 std::string msg =
"Can't construct segment from ROS message: " + std::string(ex.what());
190 throw(std::invalid_argument(msg));
220 template <
class Scalar>
222 const Scalar& next_position,
223 const bool& angle_wraparound)
226 Scalar pos_offset = 0.0;
228 if (angle_wraparound)
233 if (std::abs(std::abs(dist) - M_PI) < 1e-9)
235 dist = next_position > prev_position ? std::abs(dist) : -std::abs(dist);
237 pos_offset = (prev_position + dist) - next_position;
245 #endif // header guard
boost::shared_ptr< RealtimeGoalHandle > RealtimeGoalHandlePtr
RealtimeGoalHandlePtr rt_goal_handle_
Segment::State::Scalar Scalar
RealtimeGoalHandlePtr getGoalHandle() const
void init(const trajectory_msgs::JointTrajectoryPoint &point, const std::vector< Scalar > &position_offset=std::vector< Scalar >())
void setGoalHandle(RealtimeGoalHandlePtr rt_goal_handle)
Set the (realtime) goal handle associated to this segment.
const SegmentTolerancesPerJoint< Scalar > & getTolerances() const
realtime_tools::RealtimeServerGoalHandle< control_msgs::FollowJointTrajectoryAction > RealtimeGoalHandle
Scalar wraparoundJointOffset(const Scalar &prev_position, const Scalar &next_position, const bool &angle_wraparound)
State(const trajectory_msgs::JointTrajectoryPoint &point, const std::vector< Scalar > &position_offset=std::vector< Scalar >())
Class representing a multi-dimensional quintic spline segment with a start and end time...
Trajectory segment tolerances per Joint.
SegmentTolerancesPerJoint< Scalar > tolerances_
JointTrajectorySegment(const ros::Time &traj_start_time, const trajectory_msgs::JointTrajectoryPoint &start_point, const trajectory_msgs::JointTrajectoryPoint &end_point, const std::vector< Scalar > &position_offset=std::vector< Scalar >())
Construct a segment from start and end points (boundary conditions) specified in ROS message format...
JointTrajectorySegment(const Time &start_time, const State &start_state, const Time &end_time, const State &end_state)
Construct segment from start and end states (boundary conditions).
void setTolerances(const SegmentTolerancesPerJoint< Scalar > &tolerances)
Set the tolerances this segment is associated to.
bool isValid(const trajectory_msgs::JointTrajectoryPoint &point, const unsigned int joint_dim)
def shortest_angular_distance(from_angle, to_angle)