38 #include <angles/angles.h>
40 #include <geometric_shapes/check_isometry.h>
50 , angular_distance_weight_(1.0)
51 , motion_model_(HOLONOMIC)
52 , min_translational_distance_(1e-5)
59 for (
int i = 0; i < 3; ++i)
70 variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
72 variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
87 double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
88 double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
89 return sqrt(dx * dx + dy * dy) + M_PI * angular_distance_weight_;
94 for (
unsigned int i = 0; i < 2; ++i)
97 if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
100 values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
106 const Bounds& bounds)
const
108 if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
109 bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
112 values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
113 if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
114 bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
117 values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
118 values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);
122 const Bounds& bounds,
const double* near,
125 if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
126 bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
129 values[0] = rng.uniformReal(std::max(bounds[0].min_position_, near[0] -
distance),
130 std::min(bounds[0].max_position_, near[0] +
distance));
131 if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
132 bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
135 values[1] = rng.uniformReal(std::max(bounds[1].min_position_, near[1] -
distance),
136 std::min(bounds[1].max_position_, near[1] +
distance));
138 double da = angular_distance_weight_ *
distance;
142 values[2] = rng.uniformReal(near[2] - da, near[2] + da);
147 double& dx,
double& dy,
double& initial_turn,
double& drive_angle,
double& final_turn)
149 dx = to[0] - from[0];
150 dy = to[1] - from[1];
161 const double angle_straight_diff = std::hypot(dx, dy) > min_translational_distance ?
162 angles::shortest_angular_distance(from[2], std::atan2(dy, dx)) :
164 const double angle_backward_diff = angles::normalize_angle(angle_straight_diff - M_PI);
165 const double move_straight_cost =
166 std::abs(angle_straight_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_straight_diff, to[2]));
167 const double move_backward_cost =
168 std::abs(angle_backward_diff) + std::abs(angles::shortest_angular_distance(from[2] + angle_backward_diff, to[2]));
169 if (move_straight_cost <= move_backward_cost)
171 initial_turn = angle_straight_diff;
175 initial_turn = angle_backward_diff;
177 drive_angle = from[2] + initial_turn;
178 final_turn = angles::shortest_angular_distance(drive_angle, to[2]);
186 state[0] = from[0] + (to[0] - from[0]) * t;
187 state[1] = from[1] + (to[1] - from[1]) * t;
190 double diff = to[2] - from[2];
191 if (fabs(diff) <= M_PI)
192 state[2] = from[2] + diff * t;
196 diff = 2.0 * M_PI - diff;
198 diff = -2.0 * M_PI - diff;
199 state[2] = from[2] - diff * t;
202 state[2] -= 2.0 * M_PI;
203 else if (state[2] < -M_PI)
204 state[2] += 2.0 * M_PI;
209 double dx, dy, initial_turn, drive_angle, final_turn;
212 double initial_d = fabs(initial_turn) * angular_distance_weight_;
213 double drive_d = hypot(dx, dy);
214 double final_d = fabs(final_turn) * angular_distance_weight_;
216 double total_d = initial_d + drive_d + final_d;
218 double initial_frac = initial_d / total_d;
219 double drive_frac = drive_d / total_d;
220 double final_frac = final_d / total_d;
223 if (t <= initial_frac)
225 percent = t / initial_frac;
228 state[2] = from[2] + initial_turn * percent;
230 else if (t <= initial_frac + drive_frac)
232 percent = (t - initial_frac) / drive_frac;
233 state[0] = from[0] + dx * percent;
234 state[1] = from[1] + dy * percent;
235 state[2] = drive_angle;
239 percent = (t - initial_frac - drive_frac) / final_frac;
242 state[2] = drive_angle + final_turn * percent;
251 double dx = values1[0] - values2[0];
252 double dy = values1[1] - values2[1];
254 double d = fabs(values1[2] - values2[2]);
255 d = (
d > M_PI) ? 2.0 * M_PI -
d :
d;
256 return sqrt(dx * dx + dy * dy) + angular_distance_weight_ *
d;
260 double dx, dy, initial_turn, drive_angle, final_turn;
263 return hypot(dx, dy) + angular_distance_weight_ * (fabs(initial_turn) + fabs(final_turn));
271 for (
unsigned int i = 0; i < 3; ++i)
272 if (values[i] < bounds[i].min_position_ - margin || values[i] > bounds[i].max_position_ + margin)
279 double& v = values[2];
280 if (v >= -M_PI && v <= M_PI)
282 v = fmod(v, 2.0 * M_PI);
293 for (
unsigned int i = 0; i < 2; ++i)
295 if (values[i] < bounds[i].min_position_)
297 values[i] = bounds[i].min_position_;
300 else if (values[i] > bounds[i].max_position_)
302 values[i] = bounds[i].max_position_;
311 transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], 0.0) *
312 Eigen::AngleAxisd(joint_values[2], Eigen::Vector3d::UnitZ()));
317 joint_values[0] = transf.translation().x();
318 joint_values[1] = transf.translation().y();
320 ASSERT_ISOMETRY(transf)
321 Eigen::Quaterniond q(transf.linear());
323 double s_squared = 1.0 - (q.w() * q.w());
324 if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())
325 joint_values[2] = 0.0;
328 double s = 1.0 / sqrt(s_squared);
329 joint_values[2] = (acos(q.w()) * 2.0f) * (q.z() * s);
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
VariableIndexMap variable_index_map_
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within th...
JointType type_
The type of joint.
Bounds variable_bounds_
The bounds for each variable (low, high) in the same order as variable_names_.
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
const std::string & getName() const
Get the name of the joint.
void computeVariableBoundsMsg()
std::vector< std::string > local_variable_names_
The local names to use for the variables that make up this joint.
double getMaximumExtent() const
std::vector< std::string > variable_names_
The full names to use for the variables that make up this joint.
bool normalizeRotation(double *values) const
void interpolate(const double *from, const double *to, const double t, double *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
void getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds, const double *near, const double distance) const override
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const override
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to ...
PlanarJointModel(const std::string &name, size_t joint_index, size_t first_variable_index)
void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const override
Given the joint values for a joint, compute the corresponding transform. The computed transform is gu...
bool satisfiesPositionBounds(const double *values, const Bounds &other_bounds, double margin) const override
Check if the set of position values for the variables of this joint are within bounds,...
void computeVariablePositions(const Eigen::Isometry3d &transf, double *joint_values) const override
Given the transform generated by joint, compute the corresponding joint values. Make sure the passed ...
unsigned int getStateSpaceDimension() const override
Get the dimension of the state space that corresponds to this joint.
bool enforcePositionBounds(double *values, const Bounds &other_bounds) const override
Force the specified values to be inside bounds and normalized. Quaternions are normalized,...
void getVariableDefaultPositions(double *values, const Bounds &other_bounds) const override
Provide a default value for the joint given the joint variable bounds. Most joints will use the defau...
double distance(const double *values1, const double *values2) const override
Compute the distance between two joint states of the same model (represented by the variable values)
void computeTurnDriveTurnGeometry(const double *from, const double *to, const double min_translational_distance, double &dx, double &dy, double &initial_turn, double &drive_angle, double &final_turn)
Compute the geometry to turn toward the target point, drive straight and then turn to target orientat...
Main namespace for MoveIt.
double distance(const urdf::Pose &transform)