39#include <urdf_parser/urdf_parser.h>
40#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41#include <gtest/gtest.h>
48constexpr double EPSILON{ 1.e-9 };
52Eigen::VectorXd makeVector(
const std::vector<double>& values)
54 Eigen::VectorXd vector = Eigen::VectorXd::Zero(values.size());
55 for (std::size_t i = 0; i < values.size(); i++)
57 vector[i] = values[i];
64 const Eigen::VectorXd& joint_values,
const Eigen::VectorXd& joint_velocities)
76 const Eigen::Isometry3d tip_pose_initial =
78 const Eigen::MatrixXd jacobian = state.
getJacobian(&joint_model_group);
79 const Eigen::VectorXd cartesian_velocity = jacobian * joint_velocities;
83 constexpr double time_step = 1e-5;
84 const Eigen::VectorXd delta_joint_angles = time_step * joint_velocities;
87 const Eigen::Isometry3d tip_pose_after_delta =
89 const Eigen::Vector3d displacement = tip_pose_after_delta.translation() - tip_pose_initial.translation();
93 double angle = std::acos(displacement.dot(cartesian_velocity.head<3>()) /
94 (displacement.norm() * cartesian_velocity.head<3>().norm()));
95 EXPECT_NEAR(angle, 0.0, 1e-05) <<
"Angle between Cartesian velocity and Cartesian displacement larger than expected. "
97 << angle <<
". displacement: " << displacement.transpose()
98 <<
". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() <<
'\n';
102static void expect_near(
const Eigen::MatrixXd& x,
const Eigen::MatrixXd& y,
103 double eps = std::numeric_limits<double>::epsilon())
105 ASSERT_EQ(x.rows(), y.rows());
106 ASSERT_EQ(x.cols(), y.cols());
107 for (
int r = 0; r < x.rows(); ++r)
109 for (
int c = 0; c < x.cols(); ++c)
110 EXPECT_NEAR(x(r, c), y(r, c), eps) <<
"(r,c) = (" << r <<
',' << c <<
')';
115#define EXPECT_NEAR_TRACED(...) { \
116 SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
117 expect_near(__VA_ARGS__); \
124 builder.
addVirtualJoint(
"odom_combined",
"base_link",
"floating",
"base_joint");
125 ASSERT_TRUE(builder.
isValid());
126 moveit::core::RobotModelPtr model = builder.
build();
136 EXPECT_EQ(std::string(
"myrobot"), model->getName());
139 const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
140 EXPECT_EQ(1u, links.size());
142 const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
143 EXPECT_EQ(1u, joints.size());
145 const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
146 EXPECT_EQ(0u, pgroups.size());
152 geometry_msgs::msg::Pose pose;
153 pose.position.x = -0.1;
159 pose.orientation = tf2::toMsg(q);
165 pose.orientation = tf2::toMsg(q);
168 pose.position.y = 0.099;
171 pose.orientation = tf2::toMsg(q);
172 builder.
addInertial(
"base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101);
173 builder.
addVirtualJoint(
"odom_combined",
"base_link",
"planar",
"base_joint");
174 builder.
addGroup({}, {
"base_joint" },
"base");
176 ASSERT_TRUE(builder.
isValid());
177 moveit::core::RobotModelPtr model = builder.
build();
184 EXPECT_EQ(1u,
static_cast<unsigned int>(model->getJointModelCount()));
185 EXPECT_EQ(3u,
static_cast<unsigned int>(model->getJointModels()[0]->getLocalVariableNames().size()));
187 std::map<std::string, double> joint_values;
188 joint_values[
"base_joint/x"] = 10.0;
189 joint_values[
"base_joint/y"] = 8.0;
192 std::vector<std::string> missing_states;
194 ASSERT_EQ(missing_states.size(), 1u);
195 EXPECT_EQ(missing_states[0], std::string(
"base_joint/theta"));
196 joint_values[
"base_joint/theta"] = 0.1;
199 ASSERT_EQ(missing_states.size(), 0u);
205 const auto new_state = std::make_unique<moveit::core::RobotState>(state);
209 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/x")] = 5.0;
210 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/y")] = 4.0;
211 jv[state.
getRobotModel()->getVariableIndex(
"base_joint/theta")] = 0.0;
222 static const std::string MODEL2 = R
"(
223 <?xml version="1.0" ?>
224 <robot name="one_robot">
225 <link name="base_link">
228 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
229 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
231 <collision name="my_collision">
232 <origin rpy="0 0 0" xyz="0 0 0"/>
238 <origin rpy="0 0 0" xyz="0.0 0 0"/>
244 <joint name="joint_a" type="continuous">
246 <parent link="base_link"/>
247 <child link="link_a"/>
248 <origin rpy=" 0.0 0 0 " xyz="0.0 0 0 "/>
253 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
254 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
257 <origin rpy="0 0 0" xyz="0 0 0"/>
263 <origin rpy="0 0 0" xyz="0.0 0 0"/>
269 <joint name="joint_b" type="fixed">
270 <parent link="link_a"/>
271 <child link="link_b"/>
272 <origin rpy=" 0.0 -0.42 0 " xyz="0.0 0.5 0 "/>
277 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
278 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
281 <origin rpy="0 0 0" xyz="0 0 0"/>
287 <origin rpy="0 0 0" xyz="0.0 0 0"/>
293 <joint name="joint_c" type="prismatic">
295 <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
296 <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="0.0"
297 soft_upper_limit="0.089"/>
298 <parent link="link_b"/>
299 <child link="link_c"/>
300 <origin rpy=" 0.0 0.42 0.0 " xyz="0.0 -0.1 0 "/>
305 <origin rpy="0 0 0" xyz="0.0 0 .0"/>
306 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
309 <origin rpy="0 0 0" xyz="0 0 0"/>
315 <origin rpy="0 0 0" xyz="0.0 0 0"/>
321 <joint name="mim_f" type="prismatic">
323 <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
324 <parent link="link_c"/>
325 <child link="link_d"/>
326 <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
327 <mimic joint="joint_f" multiplier="1.5" offset="0.1"/>
329 <joint name="joint_f" type="prismatic">
331 <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
332 <parent link="link_d"/>
333 <child link="link_e"/>
334 <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
338 <origin rpy="0 0 0" xyz="0 0 0"/>
344 <origin rpy="0 1 0" xyz="0 0.1 0"/>
352 <origin rpy="0 0 0" xyz="0 0 0"/>
358 <origin rpy="0 1 0" xyz="0 0.1 0"/>
367 static const std::string SMODEL2 = R
"xml(
368 <?xml version="1.0" ?>
369 <robot name="one_robot">
370 <virtual_joint name="base_joint" child_link="base_link" parent_frame="odom_combined" type="planar"/>
371 <group name="base_from_joints">
372 <joint name="base_joint"/>
373 <joint name="joint_a"/>
374 <joint name="joint_c"/>
376 <group name="mim_joints">
377 <joint name="joint_f"/>
378 <joint name="mim_f"/>
380 <group name="base_with_subgroups">
381 <group name="base_from_base_to_tip"/>
382 <joint name="joint_c"/>
384 <group name="base_from_base_to_tip">
385 <chain base_link="base_link" tip_link="link_b"/>
386 <joint name="base_joint"/>
388 <group name="base_from_base_to_e">
389 <chain base_link="base_link" tip_link="link_e"/>
390 <joint name="base_joint"/>
392 <group name="base_with_bad_subgroups">
393 <group name="error"/>
398 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
399 auto srdf_model = std::make_shared<srdf::Model>();
400 srdf_model->initString(*urdf_model, SMODEL2);
401 robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
442 moveit::core::RobotModelConstPtr model = robot_model_;
451 ASSERT_TRUE(g_one !=
nullptr);
452 ASSERT_TRUE(g_two !=
nullptr);
453 ASSERT_TRUE(g_three !=
nullptr);
454 ASSERT_TRUE(g_four ==
nullptr);
469 std::sort(jmn.begin(), jmn.end());
470 EXPECT_EQ(jmn[0],
"base_joint");
471 EXPECT_EQ(jmn[1],
"joint_a");
472 EXPECT_EQ(jmn[2],
"joint_c");
474 std::sort(jmn.begin(), jmn.end());
475 EXPECT_EQ(jmn[0],
"base_joint");
476 EXPECT_EQ(jmn[1],
"joint_a");
477 EXPECT_EQ(jmn[2],
"joint_b");
479 std::sort(jmn.begin(), jmn.end());
480 EXPECT_EQ(jmn[0],
"base_joint");
481 EXPECT_EQ(jmn[1],
"joint_a");
482 EXPECT_EQ(jmn[2],
"joint_b");
483 EXPECT_EQ(jmn[3],
"joint_c");
513 std::map<std::string, double> joint_values;
514 joint_values[
"base_joint/x"] = 1.0;
515 joint_values[
"base_joint/y"] = 1.0;
516 joint_values[
"base_joint/theta"] = 0.5;
517 joint_values[
"joint_a"] = -0.5;
518 joint_values[
"joint_c"] = 0.08;
547 std::map<std::string, double> upd_a;
548 upd_a[
"joint_a"] = 0.2;
555 upd_a[
"joint_a"] = 3.2;
587 EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
592 Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
608 ASSERT_TRUE(joint_model_group);
612 std::cout <<
"\nVisual inspection should show NO joints out of bounds:\n";
615 std::cout <<
"\nVisual inspection should show ONE joint out of bounds:\n";
616 std::vector<double> single_joint(1);
617 single_joint[0] = -1.0;
621 std::cout <<
"\nVisual inspection should show TWO joint out of bounds:\n";
622 single_joint[0] = 1.0;
626 std::cout <<
"\nVisual inspection should show ONE joint out of bounds:\n";
627 single_joint[0] = 0.19;
640 for (
size_t i = 0; i <= 10; ++i)
642 state_a.
interpolate(state_b,
static_cast<double>(i) / 10., interpolated_state,
644 EXPECT_NEAR(state_a.
distance(state_b), 0,
EPSILON) <<
"Interpolation between identical states yielded a different "
647 for (
const auto& link_name : robot_model_->getLinkModelNames())
657 std::map<std::string, double> joint_values;
658 joint_values[
"base_joint/x"] = 1.0;
659 joint_values[
"base_joint/y"] = 1.0;
661 joint_values[
"base_joint/x"] = 0.0;
662 joint_values[
"base_joint/y"] = 2.0;
664 EXPECT_NEAR(3 * std::sqrt(2), state_a.
distance(state_b),
EPSILON) <<
"Simple interpolation of base_joint failed.";
667 EXPECT_NEAR(0., state_a.
distance(interpolated_state) - state_b.
distance(interpolated_state),
EPSILON) <<
"Simple "
673 "base_joint failed.";
675 "base_joint failed.";
678 "base_joint failed.";
680 "base_joint failed.";
683 joint_values[
"base_joint/x"] = 0.0;
684 joint_values[
"base_joint/y"] = 20.0;
685 joint_values[
"base_joint/theta"] = 3 * M_PI / 4;
686 joint_values[
"joint_a"] = -4 * M_PI / 5;
687 joint_values[
"joint_c"] = 0.0;
688 joint_values[
"joint_f"] = 1.0;
691 joint_values[
"base_joint/x"] = 10.0;
692 joint_values[
"base_joint/y"] = 0.0;
693 joint_values[
"base_joint/theta"] = -3 * M_PI / 4;
694 joint_values[
"joint_a"] = 4 * M_PI / 5;
695 joint_values[
"joint_c"] = 0.07;
696 joint_values[
"joint_f"] = 0.0;
699 for (
size_t i = 0; i <= 5; ++i)
701 double t =
static_cast<double>(i) / 5.;
704 "interpolation failed.";
711 <<
"Base joint theta interpolation failed.";
713 <<
"Continuous joint interpolation failed.";
717 EXPECT_NEAR(-3 * M_PI / 4 - (M_PI / 2) * (1 - t), interpolated_state.
getVariablePosition(
"base_joint/theta"),
719 <<
"Base joint theta interpolation failed.";
721 <<
"Continuous joint interpolation failed.";
732 bool nan_exception =
false;
737 catch (std::exception& e)
739 std::cout <<
"Caught expected exception: " << e.what() <<
'\n';
740 nan_exception =
true;
742 EXPECT_TRUE(nan_exception) <<
"NaN interpolation parameter did not create expected exception.";
749 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
754 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
761 state.
attachBody(std::make_unique<moveit::core::AttachedBody>(
762 link_b,
"object", Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{},
763 EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{},
778TEST(getJacobian, RevoluteJoints)
781 constexpr char robot_urdf[] = R
"(
782 <?xml version="1.0" ?>
783 <robot name="one_robot">
784 <link name="base_link"/>
785 <joint name="joint_a_revolute" type="revolute">
787 <parent link="base_link"/>
788 <child link="link_a"/>
789 <origin rpy="0 0 0" xyz="0 0 0"/>
790 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
792 <link name="link_a"/>
793 <joint name="joint_b_revolute" type="revolute">
795 <parent link="link_a"/>
796 <child link="link_b"/>
797 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
798 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
800 <link name="link_b"/>
801 <joint name="joint_c_revolute" type="revolute">
803 <parent link="link_b"/>
804 <child link="link_c"/>
805 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
806 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
808 <link name="link_c"/>
809 <joint name="joint_d_revolute" type="revolute">
811 <parent link="link_c"/>
812 <child link="link_d"/>
813 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
814 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
816 <link name="link_d"/>
820 constexpr char robot_srdf[] = R
"xml(
821 <?xml version="1.0" ?>
822 <robot name="one_robot">
823 <group name="base_to_tip">
824 <joint name="joint_a_revolute"/>
825 <joint name="joint_b_revolute"/>
826 <joint name="joint_c_revolute"/>
827 <joint name="joint_d_revolute"/>
832 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
833 ASSERT_TRUE(urdf_model);
834 const auto srdf_model = std::make_shared<srdf::Model>();
835 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
836 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
842 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }));
843 checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }));
846TEST(getJacobian, RevoluteAndPrismaticJoints)
849 constexpr char robot_urdf[] = R
"(
850 <?xml version="1.0" ?>
851 <robot name="one_robot">
852 <link name="base_link"/>
853 <joint name="joint_a_revolute" type="revolute">
855 <parent link="base_link"/>
856 <child link="link_a"/>
857 <origin rpy="0 0 0" xyz="0 0 0"/>
858 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
860 <link name="link_a"/>
861 <joint name="joint_b_revolute" type="revolute">
863 <parent link="link_a"/>
864 <child link="link_b"/>
865 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
866 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
868 <link name="link_b"/>
869 <joint name="joint_c_prismatic" type="prismatic">
871 <parent link="link_b"/>
872 <child link="link_c"/>
873 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
874 <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.2"/>
876 <link name="link_c"/>
877 <joint name="joint_d_revolute" type="revolute">
879 <parent link="link_c"/>
880 <child link="link_d"/>
881 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
882 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
884 <link name="link_d"/>
888 constexpr char robot_srdf[] = R
"xml(
889 <?xml version="1.0" ?>
890 <robot name="one_robot">
891 <group name="base_to_tip">
892 <joint name="joint_a_revolute"/>
893 <joint name="joint_b_revolute"/>
894 <joint name="joint_c_prismatic"/>
895 <joint name="joint_d_revolute"/>
900 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
901 ASSERT_TRUE(urdf_model);
902 const auto srdf_model = std::make_shared<srdf::Model>();
903 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
904 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
910 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }));
911 checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }));
914TEST(getJacobian, RevoluteAndFixedJoints)
917 constexpr char robot_urdf[] = R
"(
918 <?xml version="1.0" ?>
919 <robot name="one_robot">
920 <link name="base_link"/>
921 <joint name="joint_a_revolute" type="revolute">
923 <parent link="base_link"/>
924 <child link="link_a"/>
925 <origin rpy="0 0 0" xyz="0 0 0"/>
926 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
928 <link name="link_a"/>
929 <joint name="joint_b_fixed" type="fixed">
930 <parent link="link_a"/>
931 <child link="link_b"/>
933 <link name="link_b"/>
934 <joint name="joint_c_fixed" type="fixed">
935 <parent link="link_b"/>
936 <child link="link_c"/>
938 <link name="link_c"/>
939 <joint name="joint_d_revolute" type="revolute">
941 <parent link="link_c"/>
942 <child link="link_d"/>
943 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
944 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
946 <link name="link_d"/>
950 constexpr char robot_srdf[] = R
"xml(
951 <?xml version="1.0" ?>
952 <robot name="one_robot">
953 <group name="base_to_tip">
954 <joint name="joint_a_revolute"/>
955 <joint name="joint_b_fixed"/>
956 <joint name="joint_c_fixed"/>
957 <joint name="joint_d_revolute"/>
962 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
963 ASSERT_TRUE(urdf_model);
964 const auto srdf_model = std::make_shared<srdf::Model>();
965 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
966 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
972 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0 }), makeVector({ 0.1, 0.4 }));
973 checkJacobian(state, *jmg, makeVector({ 0.1, 0.4 }), makeVector({ 0.5, 0.1 }));
976TEST(getJacobian, RevolutePlanarAndPrismaticJoints)
979 constexpr char robot_urdf[] = R
"(
980 <?xml version="1.0" ?>
981 <robot name="one_robot">
982 <link name="base_link"/>
983 <joint name="joint_a_planar" type="planar">
985 <parent link="base_link"/>
986 <child link="link_a"/>
987 <origin rpy="0 0 0" xyz="0 0 0"/>
988 <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.2"/>
990 <link name="link_a"/>
991 <joint name="joint_b_revolute" type="revolute">
993 <parent link="link_a"/>
994 <child link="link_b"/>
995 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
996 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
998 <link name="link_b"/>
999 <joint name="joint_c_prismatic" type="prismatic">
1001 <parent link="link_b"/>
1002 <child link="link_c"/>
1003 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
1004 <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.2"/>
1006 <link name="link_c"/>
1007 <joint name="joint_d_revolute" type="revolute">
1009 <parent link="link_c"/>
1010 <child link="link_d"/>
1011 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1012 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1014 <link name="link_d"/>
1018 constexpr char robot_srdf[] = R
"xml(
1019 <?xml version="1.0" ?>
1020 <robot name="one_robot">
1021 <group name="base_to_tip">
1022 <joint name="joint_a_planar"/>
1023 <joint name="joint_b_revolute"/>
1024 <joint name="joint_c_prismatic"/>
1025 <joint name="joint_d_revolute"/>
1030 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1031 ASSERT_TRUE(urdf_model);
1032 const auto srdf_model = std::make_shared<srdf::Model>();
1033 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1034 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1039 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }),
1040 makeVector({ 0.2, 0.05, 0.1, 0.2, 0.3, 0.4 }));
1046 constexpr char robot_urdf[] = R
"(
1047 <?xml version="1.0" ?>
1048 <robot name="one_robot">
1049 <link name="origin"/>
1050 <joint name="fixed_offset" type="fixed">
1051 <parent link="origin"/>
1052 <child link="link_a"/>
1053 <origin rpy="0 0 1.5707" xyz="0.0 0.0 0.0"/>
1055 <link name="link_a"/>
1056 <joint name="joint_a_revolute" type="revolute">
1058 <parent link="link_a"/>
1059 <child link="link_b"/>
1060 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1061 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1063 <link name="link_b"/>
1064 <joint name="joint_b_revolute" type="revolute">
1066 <parent link="link_b"/>
1067 <child link="link_c"/>
1068 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1069 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1071 <link name="link_c"/>
1072 <joint name="joint_c_revolute" type="revolute">
1074 <parent link="link_c"/>
1075 <child link="link_d"/>
1076 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1077 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1079 <link name="link_d"/>
1083 constexpr char robot_srdf[] = R
"xml(
1084 <?xml version="1.0" ?>
1085 <robot name="one_robot">
1086 <group name="base_to_tip">
1087 <joint name="joint_a_revolute"/>
1088 <joint name="joint_b_revolute"/>
1089 <joint name="joint_c_revolute"/>
1094 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1095 ASSERT_TRUE(urdf_model);
1096 const auto srdf_model = std::make_shared<srdf::Model>();
1097 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1098 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1104 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.4, 0.2 }));
1105 checkJacobian(state, *jmg, makeVector({ 0.1, 0.4, 0.3 }), makeVector({ 0.5, 0.1, 0.2 }));
1108TEST(getJointPositions, getFixedJointValue)
1112 constexpr char robot_urdf[] = R
"(
1113 <?xml version="1.0" ?>
1114 <robot name="one_robot">
1115 <link name="base_link"/>
1116 <joint name="joint_a_revolute" type="revolute">
1118 <parent link="base_link"/>
1119 <child link="link_a"/>
1120 <origin rpy="0 0 0" xyz="0 0 0"/>
1121 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1123 <link name="link_a"/>
1124 <joint name="joint_b_revolute" type="revolute">
1126 <parent link="link_a"/>
1127 <child link="link_b"/>
1128 <origin rpy="0 0 0" xyz="0 0 0"/>
1129 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1131 <link name="link_b"/>
1132 <joint name="joint_c_fixed" type="fixed">
1133 <parent link="link_b"/>
1134 <child link="link_c"/>
1136 <link name="link_c"/>
1140 constexpr char robot_srdf[] = R
"xml(
1141 <?xml version="1.0" ?>
1142 <robot name="one_robot">
1143 <group name="base_to_tip">
1144 <joint name="joint_a_revolute"/>
1145 <joint name="joint_b_revolute"/>
1146 <joint name="joint_c_fixed"/>
1151 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1152 ASSERT_TRUE(urdf_model);
1153 const auto srdf_model = std::make_shared<srdf::Model>();
1154 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1155 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1161 ASSERT_EQ(joint_value,
nullptr);
1166 testing::InitGoogleTest(&argc, argv);
1167 return RUN_ALL_TESTS();
moveit::core::RobotModelConstPtr robot_model_
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
const std::vector< const LinkModel * > & getUpdatedLinkModels() const
Get the names of the links that are to be updated when the state of this group changes....
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint,...
A link from the robot. Contains the constant transform applied to the link and its geometry.
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
void addInertial(const std::string &link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::msg::Pose origin)
Adds a collision box to a specific link.
bool isValid()
Returns true if the building process so far has been valid.
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
void addVisualBox(const std::string &link_name, const std::vector< double > &size, geometry_msgs::msg::Pose origin)
Adds a visual box to a specific link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void setVariableAcceleration(const std::string &variable, double value)
Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown.
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
const double * getJointPositions(const std::string &joint_name) const
void setVariableVelocity(const std::string &variable, double value)
Set the velocity of a variable. If an unknown variable name is specified, an exception is thrown.
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
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...
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot's joint limits.
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
void interpolate(const RobotState &to, double t, RobotState &state) const
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints.
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool satisfiesBounds(double margin=0.0) const
bool dirtyLinkTransforms() const
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 setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
std::size_t getVariableCount() const
Get the number of variables that make up this state.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
void setJointPositions(const std::string &joint_name, const double *position)
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
int main(int argc, char **argv)
TEST(Loading, SimpleRobot)
#define EXPECT_NEAR_TRACED(...)
TEST_F(OneRobot, setToDefaultValues)