41#include <urdf_parser/urdf_parser.h>
42#include <gtest/gtest.h>
76 EXPECT_EQ(trajectory->getGroupName(),
arm_jmg_name_) <<
"Generated trajectory group name does not match";
77 EXPECT_TRUE(trajectory->empty()) <<
"Generated trajectory not empty";
79 double duration_from_previous = 0.1;
80 std::size_t waypoint_count = 5;
81 for (std::size_t ix = 0; ix < waypoint_count; ++ix)
83 trajectory->addSuffixWayPoint(*
robot_state_, duration_from_previous);
87 EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
88 <<
"Generated trajectory duration incorrect";
89 EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size())
90 <<
"Generated trajectory has the wrong number of waypoints";
91 EXPECT_EQ(waypoint_count, trajectory->size());
95 robot_trajectory::RobotTrajectoryPtr& trajectory_copy,
bool deepcopy)
98 trajectory_copy = std::make_shared<robot_trajectory::RobotTrajectory>(*trajectory, deepcopy);
100 EXPECT_EQ(trajectory_copy->getDuration(), trajectory->getDuration());
101 EXPECT_EQ(trajectory_copy->getWayPointDurations().size(), trajectory->getWayPointDurations().size());
110 moveit::core::RobotStatePtr trajectory_first_waypoint = trajectory->getWayPointPtr(0);
112 std::vector<double> trajectory_first_state;
113 trajectory_first_waypoint->copyJointGroupPositions(
arm_jmg_name_, trajectory_first_state);
116 trajectory_first_state[0] += 0.01;
117 trajectory_first_waypoint->setJointGroupPositions(
arm_jmg_name_, trajectory_first_state);
120 moveit::core::RobotStatePtr trajectory_first_waypoint_after_update = trajectory->getWayPointPtr(0);
121 std::vector<double> trajectory_first_state_after_update;
122 trajectory_first_waypoint_after_update->copyJointGroupPositions(
arm_jmg_name_, trajectory_first_state_after_update);
123 EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]);
126 double trajectory_first_duration_before_update = trajectory->getWayPointDurationFromPrevious(0);
127 double new_duration = trajectory_first_duration_before_update + 0.1;
128 trajectory->setWayPointDurationFromPrevious(0, new_duration);
131 EXPECT_EQ(trajectory->getWayPointDurationFromPrevious(0), new_duration);
142 std::vector<double> trajectory_first_state;
146 trajectory_first_state[0] += 0.01;
151 std::vector<double> trajectory_first_state_after_update;
153 EXPECT_NE(trajectory_first_state[0], trajectory_first_state_after_update[0]);
157class OneRobot :
public testing::Test
162 static const std::string MODEL2 =
163 "<?xml version=\"1.0\" ?>"
164 "<robot name=\"one_robot\">"
165 "<link name=\"base_link\">"
167 " <mass value=\"2.81\"/>"
168 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
169 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
171 " <collision name=\"my_collision\">"
172 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
174 " <box size=\"1 2 1\" />"
178 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
180 " <box size=\"1 2 1\" />"
184 "<joint name=\"panda_joint0\" type=\"continuous\">"
185 " <axis xyz=\"0 0 1\"/>"
186 " <parent link=\"base_link\"/>"
187 " <child link=\"link_a\"/>"
188 " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
190 "<link name=\"link_a\">"
192 " <mass value=\"1.0\"/>"
193 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
194 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
197 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
199 " <box size=\"1 2 1\" />"
203 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
205 " <box size=\"1 2 1\" />"
209 "<joint name=\"joint_b\" type=\"fixed\">"
210 " <parent link=\"link_a\"/>"
211 " <child link=\"link_b\"/>"
212 " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
214 "<link name=\"link_b\">"
216 " <mass value=\"1.0\"/>"
217 " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
218 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
221 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
223 " <box size=\"1 2 1\" />"
227 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
229 " <box size=\"1 2 1\" />"
233 " <joint name=\"panda_joint1\" type=\"prismatic\">"
234 " <axis xyz=\"1 0 0\"/>"
235 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
236 " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
237 "soft_upper_limit=\"0.089\"/>"
238 " <parent link=\"link_b\"/>"
239 " <child link=\"link_c\"/>"
240 " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
242 "<link name=\"link_c\">"
244 " <mass value=\"1.0\"/>"
245 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
246 " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
249 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
251 " <box size=\"1 2 1\" />"
255 " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
257 " <box size=\"1 2 1\" />"
261 " <joint name=\"mim_f\" type=\"prismatic\">"
262 " <axis xyz=\"1 0 0\"/>"
263 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
264 " <parent link=\"link_c\"/>"
265 " <child link=\"link_d\"/>"
266 " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
267 " <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
269 " <joint name=\"joint_f\" type=\"prismatic\">"
270 " <axis xyz=\"1 0 0\"/>"
271 " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
272 " <parent link=\"link_d\"/>"
273 " <child link=\"link_e\"/>"
274 " <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
276 "<link name=\"link_d\">"
278 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
280 " <box size=\"1 2 1\" />"
284 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
286 " <box size=\"1 2 1\" />"
290 "<link name=\"link_e\">"
292 " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
294 " <box size=\"1 2 1\" />"
298 " <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
300 " <box size=\"1 2 1\" />"
306 static const std::string SMODEL2 =
307 "<?xml version=\"1.0\" ?>"
308 "<robot name=\"one_robot\">"
309 "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
310 "<group name=\"panda_arm\">"
311 "<chain base_link=\"base_link\" tip_link=\"link_e\"/>"
312 "<joint name=\"base_joint\"/>"
316 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
317 srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
318 srdf_model->initString(*urdf_model, SMODEL2);
319 robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
322 robot_state_->setVariablePositions({
"panda_joint0" }, { -3.1416 });
340 EXPECT_EQ(trajectory->getGroupName(),
arm_jmg_name_) <<
"Generated trajectory group name does not match";
341 EXPECT_TRUE(trajectory->empty()) <<
"Generated trajectory not empty";
343 double duration_from_previous = 0.1;
344 std::size_t waypoint_count = 5;
345 for (std::size_t ix = 0; ix < waypoint_count; ++ix)
346 trajectory->addSuffixWayPoint(*
robot_state_, duration_from_previous);
348 EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
349 <<
"Generated trajectory duration incorrect";
350 EXPECT_EQ(waypoint_count, trajectory->getWayPointDurations().size())
351 <<
"Generated trajectory has the wrong number of waypoints";
352 EXPECT_EQ(waypoint_count, trajectory->size());
363 robot_trajectory::RobotTrajectoryPtr trajectory;
364 initTestTrajectory(trajectory);
365 modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
370 robot_trajectory::RobotTrajectoryPtr trajectory;
371 initTestTrajectory(trajectory);
372 modifyFirstWaypointAndCheckTrajectory(trajectory);
377 robot_trajectory::RobotTrajectoryPtr trajectory;
378 initTestTrajectory(trajectory);
379 moveit_msgs::msg::RobotTrajectory initial_trajectory_msg;
380 trajectory->getRobotTrajectoryMsg(initial_trajectory_msg);
382 trajectory->reverse().reverse();
384 moveit_msgs::msg::RobotTrajectory edited_trajectory_msg;
385 trajectory->getRobotTrajectoryMsg(edited_trajectory_msg);
387 EXPECT_EQ(initial_trajectory_msg, edited_trajectory_msg);
392 robot_trajectory::RobotTrajectoryPtr initial_trajectory;
393 initTestTrajectory(initial_trajectory);
394 moveit_msgs::msg::RobotTrajectory initial_trajectory_msg;
395 initial_trajectory->getRobotTrajectoryMsg(initial_trajectory_msg);
405 .
append(*initial_trajectory, 0.1);
408 EXPECT_EQ(trajectory.
getWayPointCount(), initial_trajectory->getWayPointCount() * 2 + 3);
413 robot_trajectory::RobotTrajectoryPtr initial_trajectory;
414 initTestTrajectory(initial_trajectory);
415 EXPECT_EQ(initial_trajectory->getWayPointCount(),
size_t(5));
418 robot_trajectory::RobotTrajectoryPtr traj2;
419 initTestTrajectory(traj2);
420 EXPECT_EQ(traj2->getWayPointCount(),
size_t(5));
423 const double expected_duration = 0.1;
424 initial_trajectory->append(*traj2, expected_duration, 0, 5);
425 EXPECT_EQ(initial_trajectory->getWayPointCount(),
size_t(10));
427 EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(4), expected_duration);
428 EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(5), expected_duration);
429 EXPECT_EQ(initial_trajectory->getWayPointDurationFromPrevious(6), expected_duration);
434 bool deepcopy =
false;
436 robot_trajectory::RobotTrajectoryPtr trajectory;
437 robot_trajectory::RobotTrajectoryPtr trajectory_copy;
439 initTestTrajectory(trajectory);
440 copyTrajectory(trajectory, trajectory_copy, deepcopy);
441 modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
445 std::vector<double> trajectory_first_state_after_update;
446 trajectory_first_waypoint_after_update.
copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
450 std::vector<double> trajectory_copy_first_state_after_update;
452 trajectory_copy_first_state_after_update);
455 EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
460 bool deepcopy =
true;
462 robot_trajectory::RobotTrajectoryPtr trajectory;
463 robot_trajectory::RobotTrajectoryPtr trajectory_copy;
465 initTestTrajectory(trajectory);
466 copyTrajectory(trajectory, trajectory_copy, deepcopy);
467 modifyFirstWaypointPtrAndCheckTrajectory(trajectory);
471 std::vector<double> trajectory_first_state_after_update;
472 trajectory_first_waypoint_after_update.
copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
476 std::vector<double> trajectory_copy_first_state_after_update;
478 trajectory_copy_first_state_after_update);
481 EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
483 EXPECT_NE(trajectory->getWayPointDurationFromPrevious(0), trajectory_copy->getWayPointDurationFromPrevious(0));
488 robot_trajectory::RobotTrajectoryPtr trajectory;
489 initTestTrajectory(trajectory);
491 ASSERT_EQ(5u, trajectory->size());
492 std::vector<double> positions;
494 double start_pos = 0.0;
496 for (
size_t i = 0; i < trajectory->size(); ++i)
498 auto waypoint = trajectory->getWayPointPtr(i);
500 waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
501 start_pos = positions[0];
502 positions[0] += 0.01 * i;
503 waypoint->setJointGroupPositions(arm_jmg_name_, positions);
506 unsigned int count = 0;
507 for (
const auto& waypoint_and_duration : *trajectory)
509 const auto& waypoint = waypoint_and_duration.first;
510 waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
511 EXPECT_EQ(start_pos + count * 0.01, positions[0]);
515 EXPECT_EQ(count, trajectory->size());
518 EXPECT_EQ(trajectory->begin(), trajectory->begin());
519 EXPECT_EQ(trajectory->end(), trajectory->end());
522 EXPECT_NE(trajectory->begin(), trajectory->end());
523 EXPECT_NE(++trajectory->begin(), trajectory->end());
524 EXPECT_NE(++(++trajectory->begin()), trajectory->end());
525 EXPECT_NE(++(++(++trajectory->begin())), trajectory->end());
526 EXPECT_NE(++(++(++(++trajectory->begin()))), trajectory->end());
527 EXPECT_EQ(++(++(++(++(++trajectory->begin())))), trajectory->end());
532 robot_trajectory::RobotTrajectoryPtr trajectory;
533 initTestTrajectory(trajectory);
537 std::vector<double> positions;
538 for (
size_t i = 0; i < trajectory->size(); ++i)
540 auto waypoint = trajectory->getWayPointPtr(i);
541 waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
542 positions[0] += 0.01 * i;
543 waypoint->setJointGroupPositions(arm_jmg_name_, positions);
550 robot_trajectory::RobotTrajectoryPtr trajectory;
551 initTestTrajectory(trajectory);
554 std::vector<double> positions;
555 for (
size_t i = 0; i < trajectory->size(); ++i)
557 auto waypoint = trajectory->getWayPointPtr(i);
558 waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
559 positions[0] += 0.01 * i;
560 waypoint->setJointGroupPositions(arm_jmg_name_, positions);
564 ASSERT_TRUE(smoothness.has_value());
565 EXPECT_GT(smoothness.value(), 0.0);
574 robot_trajectory::RobotTrajectoryPtr trajectory;
575 initTestTrajectory(trajectory);
579 ASSERT_FALSE(density.has_value());
582 std::vector<double> positions;
583 for (
size_t i = 0; i < trajectory->size(); ++i)
585 auto waypoint = trajectory->getWayPointPtr(i);
586 waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
587 positions[0] += 0.01 * i;
588 waypoint->setJointGroupPositions(arm_jmg_name_, positions);
592 ASSERT_TRUE(density.has_value());
593 EXPECT_GT(density.value(), 0.0);
598 EXPECT_FALSE(density.has_value());
603 const double epsilon = 1e-4;
607 robot_trajectory::RobotTrajectoryPtr trajectory;
608 initTestTrajectory(trajectory);
609 moveit::core::RobotStatePtr& first_waypoint = trajectory->getFirstWayPointPtr();
610 const double random_large_angle = 20.2;
611 first_waypoint->setVariablePosition(
"panda_joint0", random_large_angle);
612 first_waypoint->update();
613 trajectory->unwind();
614 EXPECT_NEAR(trajectory->getFirstWayPoint().getVariablePosition(
"panda_joint0"), 1.350444, epsilon);
620 const double epsilon = 1e-4;
624 robot_trajectory::RobotTrajectoryPtr trajectory;
625 initTestTrajectory(trajectory);
628 const double wrapped_angle = first_waypoint.
getVariablePosition(
"panda_joint0") + 12.566371;
632 trajectory->unwind(first_waypoint);
633 EXPECT_NEAR(trajectory->getFirstWayPoint().getVariablePosition(
"panda_joint0"), wrapped_angle, epsilon);
645 auto maybe_trajectory_msg = toJointTrajectory(trajectory,
true );
648 ASSERT_TRUE(maybe_trajectory_msg.has_value());
650 const auto traj = maybe_trajectory_msg.value();
651 const auto& joint_names = traj.joint_names;
653 size_t joint_variable_count = 0u;
654 for (
const auto& active_joint : robot_model_->getActiveJointModels())
656 joint_variable_count += active_joint->getVariableCount();
660 EXPECT_EQ(joint_names.size(), joint_variable_count);
661 EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(),
"base_joint/x") != joint_names.end());
663 ASSERT_EQ(traj.points.size(), 2u);
665 EXPECT_EQ(traj.points.at(0).positions.size(), joint_variable_count);
666 EXPECT_EQ(traj.points.at(1).positions.size(), joint_variable_count);
671 testing::InitGoogleTest(&argc, argv);
672 return RUN_ALL_TESTS();
const std::string arm_jmg_name_
void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
moveit::core::RobotStatePtr robot_state_
moveit::core::RobotModelConstPtr robot_model_
void modifyFirstWaypointPtrAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
const std::string robot_model_name_
void copyTrajectory(const robot_trajectory::RobotTrajectoryPtr &trajectory, robot_trajectory::RobotTrajectoryPtr &trajectory_copy, bool deepcopy)
moveit::core::RobotModelConstPtr robot_model_
moveit::core::RobotStatePtr robot_state_
void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
const std::string arm_jmg_name_
const std::string arm_state_name_
void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr &trajectory)
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePosition(const std::string &variable, double value)
Set the position of a single variable. An exception is thrown if the variable name is not known.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
void update(bool force=false)
Update all transforms.
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & reverse()
RobotTrajectory & addPrefixWayPoint(const moveit::core::RobotState &state, double dt)
const std::string & getGroupName() const
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
RobotTrajectory & clear()
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::msg::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
RobotTrajectory & append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...
std::size_t getWayPointCount() const
RobotTrajectory & setGroupName(const std::string &group_name)
RobotTrajectory & insertWayPoint(std::size_t index, const moveit::core::RobotState &state, double dt)
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
std::optional< double > waypointDensity(const RobotTrajectory &trajectory)
Calculate the waypoint density of a trajectory.
std::optional< double > smoothness(const RobotTrajectory &trajectory)
Calculate the smoothness of a given trajectory.
double pathLength(const RobotTrajectory &trajectory)
Calculate the path length of a given trajectory based on the accumulated robot state distances....
int main(int argc, char **argv)
TEST_F(RobotTrajectoryTestFixture, ModifyFirstWaypointByPtr)