Skip to content

Commit 62fdb1a

Browse files
urfeexurfeex
committed
Add MoveC functionality
1 parent d129cf8 commit 62fdb1a

File tree

7 files changed

+120
-8
lines changed

7 files changed

+120
-8
lines changed

‎include/ur_client_library/control/motion_primitives.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -109,17 +109,19 @@ struct MovePPrimitive : public MotionPrimitive
109109
struct MoveCPrimitive : public MotionPrimitive
110110
{
111111
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)
112+
const double acceleration = 1.4, const double velocity = 1.04, const int32_t mode = 0)
113113
{
114114
type = MotionType::MOVEC;
115115
via_point_pose = via_point;
116116
target_pose = target;
117117
this->acceleration = acceleration;
118118
this->velocity = velocity;
119119
this->blend_radius = blend_radius;
120+
this->mode = mode;
120121
}
121122
urcl::Pose via_point_pose;
122123
urcl::Pose target_pose;
124+
int32_t mode = 0;
123125
};
124126
} // namespace control
125127
} // namespace urcl

‎include/ur_client_library/ur/instruction_executor.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,9 @@ class InstructionExecutor
113113
bool moveP(const urcl::Pose& target, const double acceleration = 1.4, const double velocity = 1.04,
114114
const double blend_radius = 0.0);
115115

116+
bool moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration = 1.4,
117+
const double velocity = 1.04, const double blend_radius = 0.0, const int32_t mode = 0);
118+
116119
/**
117120
* \brief Cancel the current motion.
118121
*

‎resources/external_control.urscript‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -517,7 +517,16 @@ thread trajectoryThread():
517517
spline_qdd = [0, 0, 0, 0, 0, 0]
518518
spline_qd = [0, 0, 0, 0, 0, 0]
519519

520-
# Joint spline point
520+
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEC:
521+
local v = [raw_point[7], raw_point[8], raw_point[9], raw_point[10], raw_point[11], raw_point[12]] / MULT_jointstate
522+
via = p[v[0], v[1], v[2], v[3], v[4], v[5]]
523+
target = p[q[0], q[1], q[2], q[3], q[4], q[5]]
524+
acceleration = raw_point[13] / MULT_jointstate
525+
velocity = raw_point[14] / MULT_jointstate
526+
mode = raw_point[15]
527+
movec(via, target, acceleration, velocity, blend_radius, mode)
528+
529+
# Joint spline point
521530
elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_SPLINE:
522531

523532
# Cubic spline

‎src/control/trajectory_point_interface.cpp‎

Lines changed: 45 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include <ur_client_library/control/trajectory_point_interface.h>
3030
#include <ur_client_library/exceptions.h>
3131
#include <math.h>
32+
#include <cstdint>
3233
#include <stdexcept>
3334
#include "ur_client_library/comm/socket_t.h"
3435
#include "ur_client_library/control/motion_primitives.h"
@@ -91,6 +92,13 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
9192
movep_primitive->target_pose.rx, movep_primitive->target_pose.ry, movep_primitive->target_pose.rz };
9293
break;
9394
}
95+
case MotionType::MOVEC:
96+
{
97+
auto movec_primitive = std::static_pointer_cast<control::MoveCPrimitive>(primitive);
98+
positions = { movec_primitive->target_pose.x, movec_primitive->target_pose.y, movec_primitive->target_pose.z,
99+
movec_primitive->target_pose.rx, movec_primitive->target_pose.ry, movec_primitive->target_pose.rz };
100+
break;
101+
}
94102
default:
95103
throw UnsupportedMotionType();
96104
}
@@ -102,17 +110,49 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
102110
buffer[index] = htobe32(val);
103111
index++;
104112
}
105-
for (size_t i = 0; i < positions.size(); ++i)
113+
114+
if (primitive->type == MotionType::MOVEC)
106115
{
116+
auto movec_primitive = std::static_pointer_cast<control::MoveCPrimitive>(primitive);
117+
auto via = { movec_primitive->via_point_pose.x, movec_primitive->via_point_pose.y,
118+
movec_primitive->via_point_pose.z, movec_primitive->via_point_pose.rx,
119+
movec_primitive->via_point_pose.ry, movec_primitive->via_point_pose.rz };
120+
for (auto const& pos : via)
121+
{
122+
int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
123+
buffer[index] = htobe32(val);
124+
index++;
125+
}
107126
int32_t val = static_cast<int32_t>(round(primitive->velocity * MULT_JOINTSTATE));
108127
buffer[index] = htobe32(val);
109128
index++;
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));
129+
val = static_cast<int32_t>(round(primitive->acceleration * MULT_JOINTSTATE));
130+
buffer[index] = htobe32(val);
131+
index++;
132+
val = static_cast<int32_t>(round(movec_primitive->mode));
114133
buffer[index] = htobe32(val);
115134
index++;
135+
for (size_t i = 0; i < positions.size() - 3; ++i)
136+
{
137+
val = 0;
138+
buffer[index] = htobe32(val);
139+
index++;
140+
}
141+
}
142+
else
143+
{
144+
for (size_t i = 0; i < positions.size(); ++i)
145+
{
146+
int32_t val = static_cast<int32_t>(round(primitive->velocity * MULT_JOINTSTATE));
147+
buffer[index] = htobe32(val);
148+
index++;
149+
}
150+
for (size_t i = 0; i < positions.size(); ++i)
151+
{
152+
int32_t val = static_cast<int32_t>(round(primitive->acceleration * MULT_JOINTSTATE));
153+
buffer[index] = htobe32(val);
154+
index++;
155+
}
116156
}
117157

118158
int32_t val = static_cast<int32_t>(round(primitive->duration.count() * MULT_TIME));

‎src/ur/instruction_executor.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,13 @@ bool urcl::InstructionExecutor::moveP(const urcl::Pose& target, const double acc
112112
return executeMotion({ std::make_shared<control::MovePPrimitive>(target, blend_radius, acceleration, velocity) });
113113
}
114114

115+
bool urcl::InstructionExecutor::moveC(const urcl::Pose& via, const urcl::Pose& target, const double acceleration,
116+
const double velocity, const double blend_radius, const int32_t mode)
117+
{
118+
return executeMotion(
119+
{ std::make_shared<control::MoveCPrimitive>(via, target, blend_radius, acceleration, velocity, mode) });
120+
}
121+
115122
bool urcl::InstructionExecutor::cancelMotion()
116123
{
117124
cancel_requested_ = true;

‎tests/test_instruction_executor.cpp‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -371,6 +371,16 @@ TEST_F(InstructionExecutorTest, movep_succeeds)
371371
ASSERT_TRUE(executor_->moveP({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 }, 2.0, 2.0));
372372
}
373373

374+
TEST_F(InstructionExecutorTest, movec_succeeds)
375+
{
376+
// move to a feasible starting pose
377+
ASSERT_TRUE(executor_->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }));
378+
379+
ASSERT_TRUE(executor_->moveP({ -0.209, 0.492, 0.5522, 0.928, -1.134, -2.168 }, 1.2, 0.25, 0.025));
380+
ASSERT_TRUE(executor_->moveC({ -0.209, 0.487, 0.671, 1.026, -0.891, -2.337 },
381+
{ -0.209, 0.425, 0.841, 1.16, -0.477, -2.553 }, 0.1, 0.25, 0.025, 0));
382+
}
383+
374384
int main(int argc, char* argv[])
375385
{
376386
::testing::InitGoogleTest(&argc, argv);
@@ -391,4 +401,4 @@ int main(int argc, char* argv[])
391401
}
392402

393403
return RUN_ALL_TESTS();
394-
}
404+
}

‎tests/test_trajectory_point_interface.cpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -227,6 +227,25 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
227227
(double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
228228
(double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
229229
}
230+
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEC))
231+
{
232+
return std::make_shared<control::MoveCPrimitive>(
233+
urcl::Pose{ ((double)spl.vel[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
234+
((double)spl.vel[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
235+
((double)spl.vel[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
236+
((double)spl.vel[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
237+
((double)spl.vel[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
238+
((double)spl.vel[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
239+
urcl::Pose{ ((double)spl.pos[0]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
240+
((double)spl.pos[1]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
241+
((double)spl.pos[2]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
242+
((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
243+
((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
244+
((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
245+
(double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
246+
(double)spl.acc[1] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
247+
(double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE, (double)spl.acc[2]);
248+
}
230249
else
231250
{
232251
throw std::runtime_error("Unknown motion type");
@@ -550,6 +569,28 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep)
550569
EXPECT_EQ(received_primitive->acceleration, acceleration);
551570
}
552571

572+
TEST_F(TrajectoryPointInterfaceTest, send_movec)
573+
{
574+
urcl::Pose send_target = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
575+
urcl::Pose send_via = { 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 };
576+
double blend_radius = 0.5;
577+
double acceleration = 0.7;
578+
double velocity = 0.7;
579+
double mode = 1;
580+
auto primitive =
581+
std::make_shared<control::MoveCPrimitive>(send_via, send_target, blend_radius, acceleration, velocity, mode);
582+
583+
traj_point_interface_->writeMotionPrimitive(primitive);
584+
auto received_primitive = client_->getMotionPrimitive();
585+
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEC);
586+
EXPECT_EQ(std::static_pointer_cast<control::MoveCPrimitive>(received_primitive)->target_pose, send_target);
587+
EXPECT_EQ(std::static_pointer_cast<control::MoveCPrimitive>(received_primitive)->via_point_pose, send_via);
588+
EXPECT_EQ(received_primitive->blend_radius, blend_radius);
589+
EXPECT_EQ(received_primitive->acceleration, acceleration);
590+
EXPECT_EQ(received_primitive->acceleration, velocity);
591+
EXPECT_EQ(std::static_pointer_cast<control::MoveCPrimitive>(received_primitive)->mode, mode);
592+
}
593+
553594
TEST_F(TrajectoryPointInterfaceTest, unsupported_motion_type_throws)
554595
{
555596
auto primitive = std::make_shared<control::MotionPrimitive>();

0 commit comments

Comments
 (0)