Add TOTG test for a robot with a mimic joint

moveit · May 11, 2023 · 17cbe9b · 17cbe9b
1 parent f77e1c9
commit 17cbe9b
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 0 deletions.
diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -1064,6 +1064,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t
   const unsigned num_points = trajectory.getWayPointCount();
   const std::vector<int>& idx = group->getVariableIndexList();
   const unsigned num_joints = group->getVariableCount();
+  assert(group->getVariableCount() == max_velocity.size());
 
   // Have to convert into Eigen data structs and remove repeated points
   //  (https://github.com/tobiaskunz/trajectories/issues/3)

diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
@@ -39,6 +39,7 @@
 #include <gtest/gtest.h>
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 #include <moveit/utils/robot_model_test_utils.h>
+#include <urdf_parser/urdf_parser.h>
 
 using trajectory_processing::Path;
 using trajectory_processing::TimeOptimalTrajectoryGeneration;
@@ -410,6 +411,61 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
   }
 }
 
+TEST(time_optimal_trajectory_generation, testMimicJoint)
+{
+  // Request a trajectory. This will serve as the baseline.
+  // Then decrease the joint torque limits and re-parameterize. The trajectory duration should be shorter.
+
+  const std::string urdf = R"(<?xml version="1.0" ?>
+      <robot name="one_robot">
+      <link name="base_link"/>
+      <link name="link_a"/>
+      <link name="link_b"/>
+      <joint name="joint_a" type="continuous">
+        <axis xyz="0 1 0"/>
+        <parent link="base_link"/>
+        <child link="link_a"/>
+        <limit effort="3" velocity="3"/>
+      </joint>
+      <joint name="joint_b" type="continuous">
+        <axis xyz="1 0 0"/>
+        <parent link="link_a"/>
+        <child link="link_b"/>
+        <mimic joint="joint_a" multiplier="2" />
+        <limit effort="3" velocity="3"/>
+      </joint>
+      </robot>)";
+
+  const std::string srdf = R"(<?xml version="1.0" ?>
+      <robot name="one_robot">
+        <virtual_joint name="base_joint" child_link="base_link" parent_frame="odom_combined" type="planar"/>
+        <group name="group">
+          <joint name="joint_a"/>
+          <joint name="joint_b"/>
+        </group>
+      "</robot>)";
+
+  urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(urdf);
+  srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
+  srdf_model->initString(*urdf_model, srdf);
+  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
+  ASSERT_TRUE((bool)robot_model) << "Failed to load robot model";
+
+  auto group = robot_model->getJointModelGroup("group");
+  ASSERT_TRUE((bool)group) << "Failed to load joint model group ";
+  moveit::core::RobotState waypoint_state(robot_model);
+  waypoint_state.setToDefaultValues();
+
+  robot_trajectory::RobotTrajectory trajectory(robot_model, group);
+  waypoint_state.setJointGroupActivePositions(group, std::vector<double>{ -0.5 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+  waypoint_state.setJointGroupActivePositions(group, std::vector<double>{ 100.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+
+  TimeOptimalTrajectoryGeneration totg;
+  ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps";
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);