Skip to content

Commit 7f5fffc

Browse files
urfeexurfeex
committed
Add MoveP to trajectory_point_interface
1 parent 2bf48be commit 7f5fffc

File tree

7 files changed

+242
-32
lines changed

7 files changed

+242
-32
lines changed

‎include/ur_client_library/control/motion_primitives.h‎

Lines changed: 33 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,10 @@ enum class MotionType : uint8_t
4848
SPLINE = 51,
4949
UNKNOWN = 255
5050
};
51+
5152
struct MotionPrimitive
5253
{
53-
MotionType type;
54+
MotionType type = MotionType::UNKNOWN;
5455
std::chrono::duration<double> duration;
5556
double acceleration;
5657
double velocity;
@@ -90,6 +91,36 @@ struct MoveLPrimitive : public MotionPrimitive
9091

9192
urcl::Pose target_pose;
9293
};
94+
95+
struct MovePPrimitive : public MotionPrimitive
96+
{
97+
MovePPrimitive(const urcl::Pose& target, const double blend_radius = 0, const double acceleration = 1.4,
98+
const double velocity = 1.04)
99+
{
100+
type = MotionType::MOVEP;
101+
target_pose = target;
102+
this->acceleration = acceleration;
103+
this->velocity = velocity;
104+
this->blend_radius = blend_radius;
105+
}
106+
urcl::Pose target_pose;
107+
};
108+
109+
struct MoveCPrimitive : public MotionPrimitive
110+
{
111+
MoveCPrimitive(const urcl::Pose& via_point, const urcl::Pose& target, const double blend_radius = 0,
112+
const double acceleration = 1.4, const double velocity = 1.04)
113+
{
114+
type = MotionType::MOVEC;
115+
via_point_pose = via_point;
116+
target_pose = target;
117+
this->acceleration = acceleration;
118+
this->velocity = velocity;
119+
this->blend_radius = blend_radius;
120+
}
121+
urcl::Pose via_point_pose;
122+
urcl::Pose target_pose;
123+
};
93124
} // namespace control
94125
} // namespace urcl
95-
#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
126+
#endif // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

‎include/ur_client_library/control/trajectory_point_interface.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131

3232
#include <optional>
3333

34+
#include "ur_client_library/control/motion_primitives.h"
3435
#include "ur_client_library/control/reverse_interface.h"
3536
#include "ur_client_library/comm/control_mode.h"
3637
#include "ur_client_library/types.h"
@@ -131,6 +132,8 @@ class TrajectoryPointInterface : public ReverseInterface
131132
bool writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
132133
const vector6d_t* accelerations, const float goal_time);
133134

135+
bool writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive);
136+
134137
void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
135138
{
136139
handle_trajectory_end_ = callback;

‎include/ur_client_library/exceptions.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -205,5 +205,13 @@ class MissingArgument : public UrException
205205
return text_.c_str();
206206
}
207207
};
208+
209+
class UnsupportedMotionType : public UrException
210+
{
211+
public:
212+
explicit UnsupportedMotionType() : std::runtime_error("Unsupported motion type")
213+
{
214+
}
215+
};
208216
} // namespace urcl
209217
#endif // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED

‎include/ur_client_library/types.h‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,11 @@ struct Pose
4646
double rx;
4747
double ry;
4848
double rz;
49+
50+
bool operator==(const Pose& other) const
51+
{
52+
return x == other.x && y == other.y && z == other.z && rx == other.rx && ry == other.ry && rz == other.rz;
53+
}
4954
};
5055

5156
template <class T, std::size_t N>

‎resources/external_control.urscript‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,8 @@ TRAJECTORY_MODE_CANCEL = -1
3333

3434
MOTION_TYPE_MOVEJ = 0
3535
MOTION_TYPE_MOVEL = 1
36+
MOTION_TYPE_MOVEP = 2
37+
MOTION_TYPE_MOVEC = 3
3638
MOTION_TYPE_SPLINE = 51
3739
TRAJECTORY_DATA_DIMENSION = 3 * 6 + 1
3840

@@ -504,6 +506,16 @@ thread trajectoryThread():
504506
# reset old acceleration
505507
spline_qdd = [0, 0, 0, 0, 0, 0]
506508
spline_qd = [0, 0, 0, 0, 0, 0]
509+
510+
# MoveP point
511+
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP:
512+
acceleration = raw_point[13] / MULT_jointstate
513+
velocity = raw_point[7] / MULT_jointstate
514+
movep(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius)
515+
516+
# reset old acceleration
517+
spline_qdd = [0, 0, 0, 0, 0, 0]
518+
spline_qd = [0, 0, 0, 0, 0, 0]
507519

508520
# Joint spline point
509521
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_SPLINE:

‎src/control/trajectory_point_interface.cpp‎

Lines changed: 63 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -59,9 +59,7 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter
5959
{
6060
}
6161

62-
bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration,
63-
const float velocity, const float goal_time,
64-
const float blend_radius, const bool cartesian)
62+
bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<control::MotionPrimitive> primitive)
6563
{
6664
if (client_fd_ == -1)
6765
{
@@ -70,48 +68,62 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
7068
uint8_t buffer[sizeof(int32_t) * MESSAGE_LENGTH];
7169
uint8_t* b_pos = buffer;
7270

73-
if (positions != nullptr)
71+
vector6d_t positions;
72+
73+
switch (primitive->type)
7474
{
75-
for (auto const& pos : *positions)
75+
case MotionType::MOVEJ:
7676
{
77-
int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
78-
val = htobe32(val);
79-
b_pos += append(b_pos, val);
77+
auto movej_primitive = std::static_pointer_cast<control::MoveJPrimitive>(primitive);
78+
positions = movej_primitive->target_joint_configuration;
79+
break;
8080
}
81-
for (size_t i = 0; i < positions->size(); ++i)
81+
case MotionType::MOVEL:
8282
{
83-
int32_t val = static_cast<int32_t>(round(velocity * MULT_JOINTSTATE));
84-
val = htobe32(val);
85-
b_pos += append(b_pos, val);
83+
auto movel_primitive = std::static_pointer_cast<control::MoveLPrimitive>(primitive);
84+
positions = { movel_primitive->target_pose.x, movel_primitive->target_pose.y, movel_primitive->target_pose.z,
85+
movel_primitive->target_pose.rx, movel_primitive->target_pose.ry, movel_primitive->target_pose.rz };
86+
break;
8687
}
87-
for (size_t i = 0; i < positions->size(); ++i)
88+
case urcl::control::MotionType::MOVEP:
8889
{
89-
int32_t val = static_cast<int32_t>(round(acceleration * MULT_JOINTSTATE));
90-
val = htobe32(val);
91-
b_pos += append(b_pos, val);
90+
auto movep_primitive = std::static_pointer_cast<control::MovePPrimitive>(primitive);
91+
positions = { movep_primitive->target_pose.x, movep_primitive->target_pose.y, movep_primitive->target_pose.z,
92+
movep_primitive->target_pose.rx, movep_primitive->target_pose.ry, movep_primitive->target_pose.rz };
93+
break;
9294
}
95+
default:
96+
throw UnsupportedMotionType();
9397
}
94-
else
98+
99+
for (auto const& pos : positions)
95100
{
96-
b_pos += 6 * sizeof(int32_t);
101+
int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
102+
val = htobe32(val);
103+
b_pos += append(b_pos, val);
104+
}
105+
for (size_t i = 0; i < positions.size(); ++i)
106+
{
107+
int32_t val = static_cast<int32_t>(round(primitive->velocity * MULT_JOINTSTATE));
108+
val = htobe32(val);
109+
b_pos += append(b_pos, val);
110+
}
111+
for (size_t i = 0; i < positions.size(); ++i)
112+
{
113+
int32_t val = static_cast<int32_t>(round(primitive->acceleration * MULT_JOINTSTATE));
114+
val = htobe32(val);
115+
b_pos += append(b_pos, val);
97116
}
98117

99-
int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
118+
int32_t val = static_cast<int32_t>(round(primitive->duration.count() * MULT_TIME));
100119
val = htobe32(val);
101120
b_pos += append(b_pos, val);
102121

103-
val = static_cast<int32_t>(round(blend_radius * MULT_TIME));
122+
val = static_cast<int32_t>(round(primitive->blend_radius * MULT_TIME));
104123
val = htobe32(val);
105124
b_pos += append(b_pos, val);
106125

107-
if (cartesian)
108-
{
109-
val = static_cast<int32_t>(control::MotionType::MOVEL);
110-
}
111-
else
112-
{
113-
val = static_cast<int32_t>(control::MotionType::MOVEJ);
114-
}
126+
val = static_cast<int32_t>(primitive->type);
115127

116128
val = htobe32(val);
117129
b_pos += append(b_pos, val);
@@ -121,6 +133,28 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
121133
return server_.write(client_fd_, buffer, sizeof(buffer), written);
122134
}
123135

136+
bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration,
137+
const float velocity, const float goal_time,
138+
const float blend_radius, const bool cartesian)
139+
{
140+
std::shared_ptr<MotionPrimitive> primitive;
141+
if (cartesian)
142+
{
143+
primitive = std::make_shared<MoveLPrimitive>(
144+
urcl::Pose{ (*positions)[0], (*positions)[1], (*positions)[2], (*positions)[3], (*positions)[4],
145+
(*positions)[5] },
146+
blend_radius, std::chrono::milliseconds(static_cast<int>(goal_time * 1000)), acceleration, velocity);
147+
}
148+
else
149+
{
150+
primitive = std::make_shared<MoveJPrimitive>(*positions, blend_radius,
151+
std::chrono::milliseconds(static_cast<int>(goal_time * 1000)),
152+
acceleration, velocity);
153+
}
154+
155+
return writeMotionPrimitive(primitive);
156+
}
157+
124158
bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time,
125159
const float blend_radius, const bool cartesian)
126160
{
@@ -249,4 +283,4 @@ void TrajectoryPointInterface::messageCallback(const socket_t filedescriptor, ch
249283
}
250284
}
251285
} // namespace control
252-
} // namespace urcl
286+
} // namespace urcl

‎tests/test_trajectory_point_interface.cpp‎

Lines changed: 118 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
#include <ur_client_library/control/trajectory_point_interface.h>
3333
#include <ur_client_library/comm/tcp_socket.h>
3434
#include <ur_client_library/control/motion_primitives.h>
35+
#include "ur_client_library/exceptions.h"
3536

3637
using namespace urcl;
3738

@@ -179,6 +180,58 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
179180
readMessage(spl.pos, spl.vel, spl.acc, spl.goal_time, spl.blend_radius_or_spline_type, spl.motion_type);
180181
return spl;
181182
}
183+
184+
std::shared_ptr<control::MotionPrimitive> getMotionPrimitive()
185+
{
186+
TrajData spl = getData();
187+
if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEJ))
188+
{
189+
return std::make_shared<control::MoveJPrimitive>(
190+
vector6d_t{
191+
(double)spl.pos[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
192+
(double)spl.pos[1] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
193+
(double)spl.pos[2] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
194+
(double)spl.pos[3] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
195+
(double)spl.pos[4] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
196+
(double)spl.pos[5] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
197+
},
198+
(double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
199+
std::chrono::milliseconds(spl.goal_time),
200+
(double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
201+
(double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
202+
}
203+
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEL))
204+
{
205+
return std::make_shared<control::MoveLPrimitive>(
206+
urcl::Pose{ ((double)spl.pos[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
207+
((double)spl.pos[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
208+
((double)spl.pos[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
209+
((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
210+
((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
211+
((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
212+
(double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
213+
std::chrono::milliseconds(spl.goal_time),
214+
(double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
215+
(double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
216+
}
217+
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEP))
218+
{
219+
return std::make_shared<control::MovePPrimitive>(
220+
urcl::Pose{ ((double)spl.pos[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
221+
((double)spl.pos[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
222+
((double)spl.pos[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
223+
((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
224+
((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
225+
((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
226+
(double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
227+
(double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
228+
(double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
229+
}
230+
else
231+
{
232+
throw std::runtime_error("Unknown motion type");
233+
}
234+
}
182235
};
183236

184237
void SetUp()
@@ -439,9 +492,73 @@ TEST_F(TrajectoryPointInterfaceTest, trajectory_result)
439492
EXPECT_TRUE(waitTrajectoryEnd(1000, control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS));
440493
}
441494

495+
TEST_F(TrajectoryPointInterfaceTest, send_movej)
496+
{
497+
urcl::vector6d_t send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
498+
double blend_radius = 0.5;
499+
double velocity = 0.6;
500+
double acceleration = 0.7;
501+
auto duration = std::chrono::milliseconds(434);
502+
auto primitive =
503+
std::make_shared<control::MoveJPrimitive>(send_positions, blend_radius, duration, acceleration, velocity);
504+
505+
traj_point_interface_->writeMotionPrimitive(primitive);
506+
auto received_primitive = client_->getMotionPrimitive();
507+
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ);
508+
EXPECT_EQ(std::static_pointer_cast<control::MoveJPrimitive>(received_primitive)->target_joint_configuration,
509+
send_positions);
510+
EXPECT_EQ(received_primitive->blend_radius, blend_radius);
511+
EXPECT_EQ(received_primitive->velocity, velocity);
512+
EXPECT_EQ(received_primitive->acceleration, acceleration);
513+
EXPECT_EQ(received_primitive->duration, duration);
514+
}
515+
516+
TEST_F(TrajectoryPointInterfaceTest, send_movel)
517+
{
518+
urcl::Pose send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
519+
double blend_radius = 0.5;
520+
double velocity = 0.6;
521+
double acceleration = 0.7;
522+
auto duration = std::chrono::milliseconds(434);
523+
auto primitive =
524+
std::make_shared<control::MoveLPrimitive>(send_positions, blend_radius, duration, acceleration, velocity);
525+
526+
traj_point_interface_->writeMotionPrimitive(primitive);
527+
auto received_primitive = client_->getMotionPrimitive();
528+
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL);
529+
EXPECT_EQ(std::static_pointer_cast<control::MoveLPrimitive>(received_primitive)->target_pose, send_positions);
530+
EXPECT_EQ(received_primitive->blend_radius, blend_radius);
531+
EXPECT_EQ(received_primitive->velocity, velocity);
532+
EXPECT_EQ(received_primitive->acceleration, acceleration);
533+
EXPECT_EQ(received_primitive->duration, duration);
534+
}
535+
536+
TEST_F(TrajectoryPointInterfaceTest, send_movep)
537+
{
538+
urcl::Pose send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
539+
double blend_radius = 0.5;
540+
double velocity = 0.6;
541+
double acceleration = 0.7;
542+
auto primitive = std::make_shared<control::MovePPrimitive>(send_positions, blend_radius, acceleration, velocity);
543+
544+
traj_point_interface_->writeMotionPrimitive(primitive);
545+
auto received_primitive = client_->getMotionPrimitive();
546+
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEP);
547+
EXPECT_EQ(std::static_pointer_cast<control::MovePPrimitive>(received_primitive)->target_pose, send_positions);
548+
EXPECT_EQ(received_primitive->blend_radius, blend_radius);
549+
EXPECT_EQ(received_primitive->velocity, velocity);
550+
EXPECT_EQ(received_primitive->acceleration, acceleration);
551+
}
552+
553+
TEST_F(TrajectoryPointInterfaceTest, unsupported_motion_type_throws)
554+
{
555+
auto primitive = std::make_shared<control::MotionPrimitive>();
556+
EXPECT_THROW(traj_point_interface_->writeMotionPrimitive(primitive), urcl::UnsupportedMotionType);
557+
}
558+
442559
int main(int argc, char* argv[])
443560
{
444561
::testing::InitGoogleTest(&argc, argv);
445562

446563
return RUN_ALL_TESTS();
447-
}
564+
}

0 commit comments

Comments
 (0)