Skip to content

Commit 7e047bf

Browse files
urfeexurfeex
committed
Update tests
1 parent 6c5e79f commit 7e047bf

1 file changed

Lines changed: 115 additions & 40 deletions

File tree

‎tests/test_trajectory_point_interface.cpp‎

Lines changed: 115 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
#include <ur_client_library/control/motion_primitives.h>
3636
#include <cmath>
3737
#include <limits>
38+
#include <variant>
3839
#include "ur_client_library/exceptions.h"
3940

4041
using namespace urcl;
@@ -231,12 +232,26 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
231232
toDouble(raw[3]), toDouble(raw[4]), toDouble(raw[5]) };
232233
}
233234

235+
/*! \brief Decode Cartesian pose from block 0; if block 2 index 2 is non-zero, attach ``q_near`` from block 1. */
236+
static urcl::Pose poseWithOptionalQNear(const vector6int32_t& pose_raw, const vector6int32_t& second_raw,
237+
const vector6int32_t& third_raw)
238+
{
239+
urcl::Pose pose = toPose(pose_raw);
240+
if (toDouble(third_raw[2]) != 0.0)
241+
{
242+
pose.q_near = urcl::Q{ toDouble(second_raw[0]), toDouble(second_raw[1]), toDouble(second_raw[2]),
243+
toDouble(second_raw[3]), toDouble(second_raw[4]), toDouble(second_raw[5]) };
244+
}
245+
return pose;
246+
}
247+
234248
std::shared_ptr<control::MotionPrimitive> getMotionPrimitive()
235249
{
236250
TrajData spl = getData();
237251
const double blend_radius = toDouble(spl.blend_radius_or_spline_type);
238-
const double acceleration = toDouble(spl.acc[0]);
239-
const double velocity = toDouble(spl.vel[0]);
252+
// Third block (decoded into ``spl.acc``) is [velocity, acceleration, …] for non-spline moves.
253+
const double velocity = toDouble(spl.acc[0]);
254+
const double acceleration = toDouble(spl.acc[1]);
240255
const auto duration = std::chrono::microseconds(spl.goal_time);
241256

242257
if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEJ))
@@ -246,14 +261,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
246261
}
247262
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEJ_POSE))
248263
{
249-
const auto target = spl.pos;
250-
return std::make_shared<control::MoveJPrimitive>(urcl::MotionTarget{ toPose(target) }, blend_radius, duration,
251-
acceleration, velocity);
264+
return std::make_shared<control::MoveJPrimitive>(
265+
urcl::MotionTarget{ poseWithOptionalQNear(spl.pos, spl.vel, spl.acc) }, blend_radius, duration,
266+
acceleration, velocity);
252267
}
253268
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEL))
254269
{
255-
return std::make_shared<control::MoveLPrimitive>(toPose(spl.pos), blend_radius, duration, acceleration,
256-
velocity);
270+
return std::make_shared<control::MoveLPrimitive>(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), blend_radius,
271+
duration, acceleration, velocity);
257272
}
258273
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEL_JOINT))
259274
{
@@ -264,7 +279,8 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
264279
}
265280
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEP))
266281
{
267-
return std::make_shared<control::MovePPrimitive>(toPose(spl.pos), blend_radius, acceleration, velocity);
282+
return std::make_shared<control::MovePPrimitive>(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc), blend_radius,
283+
acceleration, velocity);
268284
}
269285
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::MOVEP_JOINT))
270286
{
@@ -308,12 +324,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
308324
}
309325
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::OPTIMOVEJ_POSE))
310326
{
311-
return std::make_shared<control::OptimoveJPrimitive>(urcl::MotionTarget{ toPose(spl.pos) }, blend_radius,
312-
acceleration, velocity);
327+
return std::make_shared<control::OptimoveJPrimitive>(
328+
urcl::MotionTarget{ poseWithOptionalQNear(spl.pos, spl.vel, spl.acc) }, blend_radius, acceleration,
329+
velocity);
313330
}
314331
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::OPTIMOVEL))
315332
{
316-
return std::make_shared<control::OptimoveLPrimitive>(toPose(spl.pos), blend_radius, acceleration, velocity);
333+
return std::make_shared<control::OptimoveLPrimitive>(poseWithOptionalQNear(spl.pos, spl.vel, spl.acc),
334+
blend_radius, acceleration, velocity);
317335
}
318336
else if (spl.motion_type == static_cast<int32_t>(control::MotionType::OPTIMOVEL_JOINT))
319337
{
@@ -426,8 +444,8 @@ TEST_F(TrajectoryPointInterfaceTest, write_quintic_joint_spline)
426444
EXPECT_EQ(send_acc[4], ((double)received_data.acc[4]) / traj_point_interface_->MULT_JOINTSTATE);
427445
EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE);
428446

429-
// Goal time
430-
EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_JOINTSTATE));
447+
// Goal time (segment duration, ``MULT_TIME`` on the wire)
448+
EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_TIME));
431449

432450
// Spline type
433451
EXPECT_EQ(static_cast<int32_t>(control::TrajectorySplineType::SPLINE_QUINTIC),
@@ -471,7 +489,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_cubic_joint_spline)
471489
EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE);
472490

473491
// Goal time
474-
EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_JOINTSTATE);
492+
EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_TIME);
475493

476494
// Spline type
477495
EXPECT_EQ(static_cast<int32_t>(control::TrajectorySplineType::SPLINE_CUBIC),
@@ -522,7 +540,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_goal_time)
522540
traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, false);
523541
int32_t received_goal_time = client_->getGoalTime();
524542

525-
EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_JOINTSTATE);
543+
EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME);
526544
}
527545

528546
// Wire format: int32 microseconds (MULT_TIME). Duration must reach writeMotionPrimitive as seconds
@@ -588,17 +606,11 @@ TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity)
588606
float send_goal_time = 0.5;
589607
traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
590608
send_goal_time, 0, 0);
591-
int32_t received_move_acceleration = client_->getAcceleration()[0];
592-
traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
593-
send_goal_time, 0, 0);
594-
int32_t received_move_velocity = client_->getVelocity()[0];
595-
traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
596-
send_goal_time, 0, 0);
597-
int32_t received_goal_time = client_->getGoalTime();
609+
Client::TrajData d = client_->getData();
598610

599-
EXPECT_EQ(send_move_acceleration, ((float)received_move_acceleration) / traj_point_interface_->MULT_JOINTSTATE);
600-
EXPECT_EQ(send_move_velocity, ((float)received_move_velocity) / traj_point_interface_->MULT_JOINTSTATE);
601-
EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_JOINTSTATE);
611+
EXPECT_EQ(send_move_velocity, ((float)d.acc[0]) / traj_point_interface_->MULT_JOINTSTATE);
612+
EXPECT_EQ(send_move_acceleration, ((float)d.acc[1]) / traj_point_interface_->MULT_JOINTSTATE);
613+
EXPECT_EQ(send_goal_time, ((float)d.goal_time) / traj_point_interface_->MULT_TIME);
602614
}
603615

604616
TEST_F(TrajectoryPointInterfaceTest, write_blend_radius)
@@ -608,7 +620,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_blend_radius)
608620
traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, false);
609621
int32_t received_blend_radius = client_->getBlendRadius();
610622

611-
EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_JOINTSTATE);
623+
EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_TIME);
612624
}
613625

614626
TEST_F(TrajectoryPointInterfaceTest, write_cartesian)
@@ -668,7 +680,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movej)
668680

669681
TEST_F(TrajectoryPointInterfaceTest, send_movel)
670682
{
671-
urcl::Pose send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
683+
urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2);
672684
double blend_radius = 0.5;
673685
double velocity = 0.6;
674686
double acceleration = 0.7;
@@ -690,7 +702,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movel)
690702

691703
TEST_F(TrajectoryPointInterfaceTest, send_movep)
692704
{
693-
urcl::Pose send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
705+
urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2);
694706
double blend_radius = 0.5;
695707
double velocity = 0.6;
696708
double acceleration = 0.7;
@@ -709,8 +721,8 @@ TEST_F(TrajectoryPointInterfaceTest, send_movep)
709721

710722
TEST_F(TrajectoryPointInterfaceTest, send_movec)
711723
{
712-
urcl::Pose send_target = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
713-
urcl::Pose send_via = { 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 };
724+
urcl::Pose send_target(1.2, 3.1, 2.2, -3.4, -1.1, -1.2);
725+
urcl::Pose send_via(0.5, 0.6, 0.7, 0.8, 0.9, 1.0);
714726
double blend_radius = 0.5;
715727
double acceleration = 0.4;
716728
double velocity = 0.7;
@@ -732,26 +744,88 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec)
732744

733745
TEST_F(TrajectoryPointInterfaceTest, send_movej_pose)
734746
{
735-
urcl::Pose send_pose = { 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 };
747+
const urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6);
736748
double blend_radius = 0.25;
737749
double velocity = 0.6;
738750
double acceleration = 0.7;
739751
auto duration = std::chrono::milliseconds(500);
740-
auto primitive = std::make_shared<control::MoveJPrimitive>(urcl::MotionTarget{ send_pose }, blend_radius, duration,
741-
acceleration, velocity);
752+
auto primitive = std::make_shared<control::MoveJPrimitive>(
753+
urcl::MotionTarget{ std::in_place_type<urcl::Pose>, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }, blend_radius, duration,
754+
acceleration, velocity);
742755

743756
traj_point_interface_->writeMotionPrimitive(primitive);
744757
auto received_primitive = client_->getMotionPrimitive();
745758
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE);
746759
EXPECT_EQ(std::get<urcl::Pose>(
747760
std::static_pointer_cast<control::MotionPrimitiveWithTarget>(received_primitive)->getTarget().value()),
748-
send_pose);
761+
expected_pose);
749762
EXPECT_EQ(received_primitive->blend_radius, blend_radius);
750763
EXPECT_EQ(received_primitive->velocity, velocity);
751764
EXPECT_EQ(received_primitive->acceleration, acceleration);
752765
EXPECT_EQ(received_primitive->duration, duration);
753766
}
754767

768+
TEST_F(TrajectoryPointInterfaceTest, send_movej_pose_with_q_near_roundtrip)
769+
{
770+
urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6);
771+
expected_pose.q_near = urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 };
772+
double blend_radius = 0.25;
773+
double velocity = 0.6;
774+
double acceleration = 0.7;
775+
auto duration = std::chrono::milliseconds(500);
776+
urcl::MotionTarget target{ std::in_place_type<urcl::Pose>, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 };
777+
std::get<urcl::Pose>(target).q_near = urcl::Q{ -1.0, -2.0, 1.0, 0.5, 0.25, 0.0 };
778+
auto primitive =
779+
std::make_shared<control::MoveJPrimitive>(std::move(target), blend_radius, duration, acceleration, velocity);
780+
781+
traj_point_interface_->writeMotionPrimitive(primitive);
782+
auto received_primitive = client_->getMotionPrimitive();
783+
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEJ_POSE);
784+
EXPECT_EQ(std::get<urcl::Pose>(
785+
std::static_pointer_cast<control::MotionPrimitiveWithTarget>(received_primitive)->getTarget().value()),
786+
expected_pose);
787+
}
788+
789+
TEST_F(TrajectoryPointInterfaceTest, send_movel_with_q_near_roundtrip)
790+
{
791+
urcl::Pose send_positions(1.2, 3.1, 2.2, -3.4, -1.1, -1.2);
792+
send_positions.q_near = urcl::Q{ 0.11, -0.22, 0.33, 0.44, -0.55, 0.66 };
793+
double blend_radius = 0.5;
794+
double velocity = 0.6;
795+
double acceleration = 0.7;
796+
auto duration = std::chrono::milliseconds(434);
797+
auto primitive =
798+
std::make_shared<control::MoveLPrimitive>(send_positions, blend_radius, duration, acceleration, velocity);
799+
800+
traj_point_interface_->writeMotionPrimitive(primitive);
801+
auto received_primitive = client_->getMotionPrimitive();
802+
EXPECT_EQ(received_primitive->type, control::MotionType::MOVEL);
803+
EXPECT_EQ(std::get<urcl::Pose>(
804+
std::static_pointer_cast<control::MotionPrimitiveWithTarget>(received_primitive)->getTarget().value()),
805+
send_positions);
806+
}
807+
808+
TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose_with_q_near_roundtrip)
809+
{
810+
const urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6);
811+
double blend_radius = 0.1;
812+
double acceleration_fraction = 0.4;
813+
double velocity_fraction = 0.6;
814+
urcl::MotionTarget target{ std::in_place_type<urcl::Pose>, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 };
815+
std::get<urcl::Pose>(target).q_near = urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 };
816+
auto primitive = std::make_shared<control::OptimoveJPrimitive>(std::move(target), blend_radius, acceleration_fraction,
817+
velocity_fraction);
818+
819+
traj_point_interface_->writeMotionPrimitive(primitive);
820+
auto received_primitive = client_->getMotionPrimitive();
821+
EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE);
822+
urcl::Pose expected_with_q = expected_pose;
823+
expected_with_q.q_near = urcl::Q{ 0.01, 0.02, -0.03, 0.04, -0.05, 0.06 };
824+
EXPECT_EQ(std::get<urcl::Pose>(
825+
std::static_pointer_cast<control::MotionPrimitiveWithTarget>(received_primitive)->getTarget().value()),
826+
expected_with_q);
827+
}
828+
755829
TEST_F(TrajectoryPointInterfaceTest, send_movel_joint)
756830
{
757831
urcl::Q send_joints{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
@@ -822,7 +896,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_joint)
822896
TEST_F(TrajectoryPointInterfaceTest, send_movec_pose_joint)
823897
{
824898
// via is a Pose, target is a Q
825-
urcl::Pose send_via{ 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 };
899+
urcl::Pose send_via(0.1, -0.2, 0.3, 0.4, -0.5, 0.6);
826900
urcl::Q send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
827901
double blend_radius = 0.25;
828902
double acceleration = 0.7;
@@ -848,7 +922,7 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose)
848922
{
849923
// via is a Q, target is a Pose
850924
urcl::Q send_via{ 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 };
851-
urcl::Pose send_target{ 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
925+
urcl::Pose send_target(1.2, 3.1, 2.2, -3.4, -1.1, -1.2);
852926
double blend_radius = 0.25;
853927
double acceleration = 0.7;
854928
double velocity = 0.5;
@@ -871,19 +945,20 @@ TEST_F(TrajectoryPointInterfaceTest, send_movec_joint_pose)
871945

872946
TEST_F(TrajectoryPointInterfaceTest, send_optimovej_pose)
873947
{
874-
urcl::Pose send_pose{ 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 };
948+
const urcl::Pose expected_pose(0.1, -0.2, 0.3, 0.4, -0.5, 0.6);
875949
double blend_radius = 0.1;
876950
double acceleration_fraction = 0.4;
877951
double velocity_fraction = 0.6;
878-
auto primitive = std::make_shared<control::OptimoveJPrimitive>(urcl::MotionTarget{ send_pose }, blend_radius,
879-
acceleration_fraction, velocity_fraction);
952+
auto primitive = std::make_shared<control::OptimoveJPrimitive>(
953+
urcl::MotionTarget{ std::in_place_type<urcl::Pose>, 0.1, -0.2, 0.3, 0.4, -0.5, 0.6 }, blend_radius,
954+
acceleration_fraction, velocity_fraction);
880955

881956
traj_point_interface_->writeMotionPrimitive(primitive);
882957
auto received_primitive = client_->getMotionPrimitive();
883958
EXPECT_EQ(received_primitive->type, control::MotionType::OPTIMOVEJ_POSE);
884959
EXPECT_EQ(std::get<urcl::Pose>(
885960
std::static_pointer_cast<control::MotionPrimitiveWithTarget>(received_primitive)->getTarget().value()),
886-
send_pose);
961+
expected_pose);
887962
EXPECT_EQ(received_primitive->blend_radius, blend_radius);
888963
EXPECT_EQ(received_primitive->velocity, velocity_fraction);
889964
EXPECT_EQ(received_primitive->acceleration, acceleration_fraction);

0 commit comments

Comments
 (0)