Skip to content

Commit 398d024

Browse files
urfeexurfeex
committed
Correct setting target from derived classes
1 parent 16d7757 commit 398d024

2 files changed

Lines changed: 63 additions & 48 deletions

File tree

‎include/ur_client_library/control/motion_primitives.h‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -108,17 +108,15 @@ class MotionPrimitive
108108
class MotionPrimitiveWithTarget : public MotionPrimitive
109109
{
110110
public:
111-
MotionPrimitiveWithTarget(const MotionTarget& target, const double blend_radius = 0,
111+
MotionPrimitiveWithTarget(const double blend_radius = 0,
112112
const std::chrono::duration<double> duration = std::chrono::milliseconds(0),
113113
const double acceleration = 1.4, const double velocity = 1.04)
114114
: MotionPrimitive(blend_radius, duration, acceleration, velocity)
115115
{
116-
setTarget(target);
117-
}
118-
virtual void setTarget(const MotionTarget& target)
119-
{
120-
target_ = std::make_unique<MotionTarget>(target);
121116
}
117+
118+
virtual void setTarget(const MotionTarget& target) = 0;
119+
122120
MotionTarget getTarget() const
123121
{
124122
return *target_;

‎src/control/motion_primitives.cpp‎

Lines changed: 59 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -142,15 +142,17 @@ std::string motionTypeToString(const MotionType type)
142142
MoveJPrimitive::MoveJPrimitive(const MotionTarget& target, const double blend_radius,
143143
const std::chrono::duration<double> duration, const double acceleration,
144144
const double velocity)
145-
: MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity)
145+
: MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity)
146146
{
147+
setTarget(target);
147148
}
148149

149150
MoveJPrimitive::MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius,
150151
const std::chrono::duration<double> duration, const double acceleration,
151152
const double velocity)
152-
: MotionPrimitiveWithTarget(Q(target), blend_radius, duration, acceleration, velocity)
153+
: MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity)
153154
{
155+
setTarget(Q(target));
154156
}
155157

156158
void MoveJPrimitive::setTarget(const MotionTarget& target)
@@ -177,15 +179,17 @@ void MoveJPrimitive::setTarget(const MotionTarget& target)
177179
MoveLPrimitive::MoveLPrimitive(const urcl::Pose& target, const double blend_radius,
178180
const std::chrono::duration<double> duration, const double acceleration,
179181
const double velocity)
180-
: MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity)
182+
: MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity)
181183
{
184+
setTarget(target);
182185
}
183186

184187
MoveLPrimitive::MoveLPrimitive(const MotionTarget& target, const double blend_radius,
185188
const std::chrono::duration<double> duration, const double acceleration,
186189
const double velocity)
187-
: MotionPrimitiveWithTarget(target, blend_radius, duration, acceleration, velocity)
190+
: MotionPrimitiveWithTarget(blend_radius, duration, acceleration, velocity)
188191
{
192+
setTarget(target);
189193
}
190194
void MoveLPrimitive::setTarget(const MotionTarget& target)
191195
{
@@ -209,14 +213,16 @@ void MoveLPrimitive::setTarget(const MotionTarget& target)
209213

210214
MovePPrimitive::MovePPrimitive(const urcl::Pose& target, const double blend_radius, const double acceleration,
211215
const double velocity)
212-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
216+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
213217
{
218+
setTarget(target);
214219
}
215220

216221
MovePPrimitive::MovePPrimitive(const MotionTarget& target, const double blend_radius, const double acceleration,
217222
const double velocity)
218-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
223+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
219224
{
225+
setTarget(target);
220226
}
221227

222228
void MovePPrimitive::setTarget(const MotionTarget& target)
@@ -241,7 +247,7 @@ void MovePPrimitive::setTarget(const MotionTarget& target)
241247

242248
MoveCPrimitive::MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& target, const double blend_radius,
243249
const double acceleration, const double velocity, const int32_t mode)
244-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
250+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
245251
, mode(mode)
246252
, via_point_(via_point)
247253
{
@@ -251,7 +257,7 @@ MoveCPrimitive::MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& ta
251257

252258
MoveCPrimitive::MoveCPrimitive(const MotionTarget& via_point, const MotionTarget& target, const double blend_radius,
253259
const double acceleration, const double velocity, const int32_t mode)
254-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
260+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration, velocity)
255261
, mode(mode)
256262
, via_point_(via_point)
257263
{
@@ -275,6 +281,11 @@ void MoveCPrimitive::setVia(const MotionTarget& via_point)
275281

276282
void MoveCPrimitive::recomputeType()
277283
{
284+
if (target_ == nullptr || via_point_.index() == std::variant_npos)
285+
{
286+
type = MotionType::UNKNOWN;
287+
return;
288+
}
278289
const bool target_is_pose = std::holds_alternative<urcl::Pose>(*target_);
279290
const bool via_is_pose = std::holds_alternative<urcl::Pose>(via_point_);
280291
if (target_is_pose && via_is_pose)
@@ -297,46 +308,52 @@ void MoveCPrimitive::recomputeType()
297308

298309
void MoveCPrimitive::refreshLegacyFields()
299310
{
300-
std::visit(
301-
[&](const auto& target_variant) {
302-
using T = std::decay_t<decltype(target_variant)>;
303-
if constexpr (std::is_same_v<T, urcl::Pose>)
304-
{
305-
target_pose = target_variant;
306-
}
307-
else if constexpr (std::is_same_v<T, Q>)
308-
{
309-
target_pose = urcl::Pose{};
310-
}
311-
},
312-
*target_);
313-
std::visit(
314-
[&](const auto& via_variant) {
315-
using T = std::decay_t<decltype(via_variant)>;
316-
if constexpr (std::is_same_v<T, urcl::Pose>)
317-
{
318-
via_point_pose = via_variant;
319-
}
320-
else if constexpr (std::is_same_v<T, Q>)
321-
{
322-
via_point_pose = urcl::Pose{};
323-
}
324-
},
325-
via_point_);
311+
if (target_ != nullptr)
312+
{
313+
std::visit(
314+
[&](const auto& target_variant) {
315+
using T = std::decay_t<decltype(target_variant)>;
316+
if constexpr (std::is_same_v<T, urcl::Pose>)
317+
{
318+
target_pose = target_variant;
319+
}
320+
else if constexpr (std::is_same_v<T, Q>)
321+
{
322+
target_pose = urcl::Pose{};
323+
}
324+
},
325+
*target_);
326+
}
327+
if (via_point_.index() != std::variant_npos)
328+
{
329+
std::visit(
330+
[&](const auto& via_variant) {
331+
using T = std::decay_t<decltype(via_variant)>;
332+
if constexpr (std::is_same_v<T, urcl::Pose>)
333+
{
334+
via_point_pose = via_variant;
335+
}
336+
else if constexpr (std::is_same_v<T, Q>)
337+
{
338+
via_point_pose = urcl::Pose{};
339+
}
340+
},
341+
via_point_);
342+
}
326343
}
327344

328345
OptimoveJPrimitive::OptimoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius,
329346
const double acceleration_fraction, const double velocity_fraction)
330-
: MotionPrimitiveWithTarget(Q(target), blend_radius, std::chrono::milliseconds(0), acceleration_fraction,
331-
velocity_fraction)
347+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction)
332348
{
349+
setTarget(Q(target));
333350
}
334351

335352
OptimoveJPrimitive::OptimoveJPrimitive(const MotionTarget& target, const double blend_radius,
336353
const double acceleration_fraction, const double velocity_fraction)
337-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction,
338-
velocity_fraction)
354+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction)
339355
{
356+
setTarget(target);
340357
}
341358

342359
void OptimoveJPrimitive::setTarget(const MotionTarget& target)
@@ -361,16 +378,16 @@ void OptimoveJPrimitive::setTarget(const MotionTarget& target)
361378

362379
OptimoveLPrimitive::OptimoveLPrimitive(const urcl::Pose& target, const double blend_radius,
363380
const double acceleration_fraction, const double velocity_fraction)
364-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction,
365-
velocity_fraction)
381+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction)
366382
{
383+
setTarget(target);
367384
}
368385

369386
OptimoveLPrimitive::OptimoveLPrimitive(const MotionTarget& target, const double blend_radius,
370387
const double acceleration_fraction, const double velocity_fraction)
371-
: MotionPrimitiveWithTarget(target, blend_radius, std::chrono::milliseconds(0), acceleration_fraction,
372-
velocity_fraction)
388+
: MotionPrimitiveWithTarget(blend_radius, std::chrono::milliseconds(0), acceleration_fraction, velocity_fraction)
373389
{
390+
setTarget(target);
374391
}
375392

376393
void OptimoveLPrimitive::setTarget(const MotionTarget& target)

0 commit comments

Comments
 (0)