moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
robot_state_test.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan */
36#include <moveit/robot_model/robot_model.h>
37#include <moveit/robot_state/robot_state.h>
38#include <moveit/utils/robot_model_test_utils.h>
39#include <urdf_parser/urdf_parser.h>
40#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
41#include <gtest/gtest.h>
42#include <sstream>
43#include <algorithm>
44#include <ctype.h>
45
46namespace
47{
48constexpr double EPSILON{ 1.e-9 };
49
50// Helper to create an Eigen::VectorXd from an initializer list, e.g.:
51// auto vector = makeVector({1.0, 2.0, 3.0});
52Eigen::VectorXd makeVector(const std::vector<double>& values)
53{
54 Eigen::VectorXd vector = Eigen::VectorXd::Zero(values.size());
55 for (std::size_t i = 0; i < values.size(); i++)
56 {
57 vector[i] = values[i];
58 }
59 return vector;
60}
61
62// Checks the validity of state.getJacobian() at the given 'joint_values' and 'joint_velocities'.
63void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointModelGroup& joint_model_group,
64 const Eigen::VectorXd& joint_values, const Eigen::VectorXd& joint_velocities,
65 const moveit::core::LinkModel* reference_link = nullptr)
66{
67 // Using the Jacobian, compute the Cartesian velocity vector at which the end-effector would move, with the given
68 // joint velocities.
69 state.setToDefaultValues();
70 state.setJointGroupPositions(&joint_model_group, joint_values);
71 state.updateLinkTransforms();
72
73 const moveit::core::JointModel* root_joint_model = joint_model_group.getJointModels().front();
74 const moveit::core::LinkModel* root_link_model = root_joint_model->getParentLinkModel();
75 const Eigen::Isometry3d root_pose_world = state.getGlobalLinkTransform(root_link_model).inverse();
76
77 if (!reference_link)
78 {
79 reference_link = joint_model_group.getLinkModels().back();
80 }
81 const Eigen::Isometry3d tip_pose_initial = root_pose_world * state.getGlobalLinkTransform(reference_link);
82 Eigen::MatrixXd jacobian;
83 state.getJacobian(&joint_model_group, reference_link, Eigen::Vector3d::Zero(), jacobian);
84
85 // Verify that only elements of the Jacobian contain values that correspond to joints that are being used based on the reference link.
86 const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group.getJointModels();
87 auto it = std::find_if(joint_models.begin(), joint_models.end(), [&](const moveit::core::JointModel* jm) {
88 return jm->getParentLinkModel() == reference_link;
89 });
90 if (it != joint_models.end())
91 {
92 std::size_t index = 0;
93 for (auto jt = joint_models.begin(); jt != it; ++jt)
94 {
95 index += (*jt)->getVariableCount();
96 }
97
98 EXPECT_TRUE(jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index).isZero())
99 << "Jacobian contains non-zero values for joints that are not used based on the reference link "
100 << reference_link->getName() << ". This is the faulty Jacobian: " << '\n'
101 << jacobian << '\n'
102 << "The columns " << index << " to " << jacobian.cols() << " should be zero. Instead the values are: " << '\n'
103 << jacobian.block(0, index, jacobian.rows(), jacobian.cols() - index);
104 }
105
106 // Compute the Cartesian velocity vector using the Jacobian.
107 const Eigen::VectorXd cartesian_velocity = jacobian * joint_velocities;
108
109 // Compute the instantaneous displacement that the end-effector would achieve if the given joint
110 // velocities were applied for a small amount of time.
111 constexpr double time_step = 1e-5;
112 const Eigen::VectorXd delta_joint_angles = time_step * joint_velocities;
113 state.setJointGroupPositions(&joint_model_group, joint_values + delta_joint_angles);
114 state.updateLinkTransforms();
115 const Eigen::Isometry3d tip_pose_after_delta = root_pose_world * state.getGlobalLinkTransform(reference_link);
116 const Eigen::Vector3d displacement = tip_pose_after_delta.translation() - tip_pose_initial.translation();
117
118 // The Cartesian velocity vector obtained via the Jacobian should be aligned with the instantaneous robot motion, i.e.
119 // the angle between the vectors should be close to zero.
120 double angle = std::acos(displacement.dot(cartesian_velocity.head<3>()) /
121 (displacement.norm() * cartesian_velocity.head<3>().norm()));
122 EXPECT_NEAR(angle, 0.0, 1e-05) << "Angle between Cartesian velocity and Cartesian displacement larger than expected. "
123 "Angle: "
124 << angle << ". displacement: " << displacement.transpose()
125 << ". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() << '\n';
126}
127} // namespace
128
129static void expect_near(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
130 double eps = std::numeric_limits<double>::epsilon())
131{
132 ASSERT_EQ(x.rows(), y.rows());
133 ASSERT_EQ(x.cols(), y.cols());
134 for (int r = 0; r < x.rows(); ++r)
135 {
136 for (int c = 0; c < x.cols(); ++c)
137 EXPECT_NEAR(x(r, c), y(r, c), eps) << "(r,c) = (" << r << ',' << c << ')';
138 }
139}
140
141// clang-format off
142#define EXPECT_NEAR_TRACED(...) { \
143 SCOPED_TRACE("expect_near(" #__VA_ARGS__ ")"); \
144 expect_near(__VA_ARGS__); \
145}
146// clang-format on
147
148TEST(Loading, SimpleRobot)
149{
150 moveit::core::RobotModelBuilder builder("myrobot", "base_link");
151 builder.addVirtualJoint("odom_combined", "base_link", "floating", "base_joint");
152 ASSERT_TRUE(builder.isValid());
153 moveit::core::RobotModelPtr model = builder.build();
154 moveit::core::RobotState state(model);
155
156 state.setToDefaultValues();
157
158 // make sure that this copy constructor works
159 moveit::core::RobotState new_state(state);
160
161 EXPECT_EQ(new_state.getVariablePosition("base_joint/rot_w"), 1.0);
162
163 EXPECT_EQ(std::string("myrobot"), model->getName());
164 EXPECT_EQ(7u, new_state.getVariableCount());
165
166 const std::vector<moveit::core::LinkModel*>& links = model->getLinkModels();
167 EXPECT_EQ(1u, links.size());
168
169 const std::vector<moveit::core::JointModel*>& joints = model->getJointModels();
170 EXPECT_EQ(1u, joints.size());
171
172 const std::vector<std::string>& pgroups = model->getJointModelGroupNames();
173 EXPECT_EQ(0u, pgroups.size());
174}
175
176TEST(LoadingAndFK, SimpleRobot)
177{
178 moveit::core::RobotModelBuilder builder("myrobot", "base_link");
179 geometry_msgs::msg::Pose pose;
180 pose.position.x = -0.1;
181 pose.position.y = 0;
182 pose.position.z = 0;
183
184 tf2::Quaternion q;
185 q.setRPY(0, 0, -1);
186 pose.orientation = tf2::toMsg(q);
187 builder.addCollisionBox("base_link", { 1, 2, 1 }, pose);
188 pose.position.x = 0;
189 pose.position.y = 0;
190 pose.position.z = 0;
191 q.setRPY(0, 0, 0);
192 pose.orientation = tf2::toMsg(q);
193 builder.addVisualBox("base_link", { 1, 2, 1 }, pose);
194 pose.position.x = 0;
195 pose.position.y = 0.099;
196 pose.position.z = 0;
197 q.setRPY(0, 0, 0);
198 pose.orientation = tf2::toMsg(q);
199 builder.addInertial("base_link", 2.81, pose, 0.1, -0.2, 0.5, -0.09, 1, 0.101);
200 builder.addVirtualJoint("odom_combined", "base_link", "planar", "base_joint");
201 builder.addGroup({}, { "base_joint" }, "base");
202
203 ASSERT_TRUE(builder.isValid());
204 moveit::core::RobotModelPtr model = builder.build();
205 moveit::core::RobotState state(model);
206
207 EXPECT_EQ(3u, state.getVariableCount());
208
209 state.setToDefaultValues();
210
211 EXPECT_EQ(1u, static_cast<unsigned int>(model->getJointModelCount()));
212 EXPECT_EQ(3u, static_cast<unsigned int>(model->getJointModels()[0]->getLocalVariableNames().size()));
213
214 std::map<std::string, double> joint_values;
215 joint_values["base_joint/x"] = 10.0;
216 joint_values["base_joint/y"] = 8.0;
217
218 // testing incomplete state
219 std::vector<std::string> missing_states;
220 state.setVariablePositions(joint_values, missing_states);
221 ASSERT_EQ(missing_states.size(), 1u);
222 EXPECT_EQ(missing_states[0], std::string("base_joint/theta"));
223 joint_values["base_joint/theta"] = 0.1;
224
225 state.setVariablePositions(joint_values, missing_states);
226 ASSERT_EQ(missing_states.size(), 0u);
227
228 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
229
230 state.setVariableAcceleration("base_joint/x", 0.0);
231
232 const auto new_state = std::make_unique<moveit::core::RobotState>(state); // making sure that values get copied
233 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
234
235 std::vector<double> jv(state.getVariableCount(), 0.0);
236 jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 5.0;
237 jv[state.getRobotModel()->getVariableIndex("base_joint/y")] = 4.0;
238 jv[state.getRobotModel()->getVariableIndex("base_joint/theta")] = 0.0;
239
240 state.setVariablePositions(jv);
241 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(5, 4, 0));
242}
243
244class OneRobot : public testing::Test
245{
246protected:
247 void SetUp() override
248 {
249 static const std::string MODEL2 = R"(
250 <?xml version="1.0" ?>
251 <robot name="one_robot">
252 <link name="base_link">
253 <inertial>
254 <mass value="2.81"/>
255 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
256 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
257 </inertial>
258 <collision name="my_collision">
259 <origin rpy="0 0 0" xyz="0 0 0"/>
260 <geometry>
261 <box size="1 2 1" />
262 </geometry>
263 </collision>
264 <visual>
265 <origin rpy="0 0 0" xyz="0.0 0 0"/>
266 <geometry>
267 <box size="1 2 1" />
268 </geometry>
269 </visual>
270 </link>
271 <joint name="joint_a" type="continuous">
272 <axis xyz="0 0 1"/>
273 <parent link="base_link"/>
274 <child link="link_a"/>
275 <origin rpy=" 0.0 0 0 " xyz="0.0 0 0 "/>
276 </joint>
277 <link name="link_a">
278 <inertial>
279 <mass value="1.0"/>
280 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
281 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
282 </inertial>
283 <collision>
284 <origin rpy="0 0 0" xyz="0 0 0"/>
285 <geometry>
286 <box size="1 2 1" />
287 </geometry>
288 </collision>
289 <visual>
290 <origin rpy="0 0 0" xyz="0.0 0 0"/>
291 <geometry>
292 <box size="1 2 1" />
293 </geometry>
294 </visual>
295 </link>
296 <joint name="joint_b" type="fixed">
297 <parent link="link_a"/>
298 <child link="link_b"/>
299 <origin rpy=" 0.0 -0.42 0 " xyz="0.0 0.5 0 "/>
300 </joint>
301 <link name="link_b">
302 <inertial>
303 <mass value="1.0"/>
304 <origin rpy="0 0 0" xyz="0.0 0.0 .0"/>
305 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
306 </inertial>
307 <collision>
308 <origin rpy="0 0 0" xyz="0 0 0"/>
309 <geometry>
310 <box size="1 2 1" />
311 </geometry>
312 </collision>
313 <visual>
314 <origin rpy="0 0 0" xyz="0.0 0 0"/>
315 <geometry>
316 <box size="1 2 1" />
317 </geometry>
318 </visual>
319 </link>
320 <joint name="joint_c" type="prismatic">
321 <axis xyz="1 0 0"/>
322 <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
323 <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="0.0"
324 soft_upper_limit="0.089"/>
325 <parent link="link_b"/>
326 <child link="link_c"/>
327 <origin rpy=" 0.0 0.42 0.0 " xyz="0.0 -0.1 0 "/>
328 </joint>
329 <link name="link_c">
330 <inertial>
331 <mass value="1.0"/>
332 <origin rpy="0 0 0" xyz="0.0 0 .0"/>
333 <inertia ixx="0.1" ixy="-0.2" ixz="0.5" iyy="-.09" iyz="1" izz="0.101"/>
334 </inertial>
335 <collision>
336 <origin rpy="0 0 0" xyz="0 0 0"/>
337 <geometry>
338 <box size="1 2 1" />
339 </geometry>
340 </collision>
341 <visual>
342 <origin rpy="0 0 0" xyz="0.0 0 0"/>
343 <geometry>
344 <box size="1 2 1" />
345 </geometry>
346 </visual>
347 </link>
348 <joint name="mim_f" type="prismatic">
349 <axis xyz="1 0 0"/>
350 <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
351 <parent link="link_c"/>
352 <child link="link_d"/>
353 <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
354 <mimic joint="joint_f" multiplier="1.5" offset="0.1"/>
355 </joint>
356 <joint name="joint_f" type="prismatic">
357 <axis xyz="1 0 0"/>
358 <limit effort="100.0" lower="0.0" upper="0.19" velocity="0.2"/>
359 <parent link="link_d"/>
360 <child link="link_e"/>
361 <origin rpy=" 0.0 0.0 0.0 " xyz="0.1 0.1 0 "/>
362 </joint>
363 <link name="link_d">
364 <collision>
365 <origin rpy="0 0 0" xyz="0 0 0"/>
366 <geometry>
367 <box size="1 2 1" />
368 </geometry>
369 </collision>
370 <visual>
371 <origin rpy="0 1 0" xyz="0 0.1 0"/>
372 <geometry>
373 <box size="1 2 1" />
374 </geometry>
375 </visual>
376 </link>
377 <link name="link_e">
378 <collision>
379 <origin rpy="0 0 0" xyz="0 0 0"/>
380 <geometry>
381 <box size="1 2 1" />
382 </geometry>
383 </collision>
384 <visual>
385 <origin rpy="0 1 0" xyz="0 0.1 0"/>
386 <geometry>
387 <box size="1 2 1" />
388 </geometry>
389 </visual>
390 </link>
391 </robot>
392 )";
393
394 static const std::string SMODEL2 = R"xml(
395 <?xml version="1.0" ?>
396 <robot name="one_robot">
397 <virtual_joint name="base_joint" child_link="base_link" parent_frame="odom_combined" type="planar"/>
398 <group name="base_from_joints">
399 <joint name="base_joint"/>
400 <joint name="joint_a"/>
401 <joint name="joint_c"/>
402 </group>
403 <group name="mim_joints">
404 <joint name="joint_f"/>
405 <joint name="mim_f"/>
406 </group>
407 <group name="base_with_subgroups">
408 <group name="base_from_base_to_tip"/>
409 <joint name="joint_c"/>
410 </group>
411 <group name="base_from_base_to_tip">
412 <chain base_link="base_link" tip_link="link_b"/>
413 <joint name="base_joint"/>
414 </group>
415 <group name="base_from_base_to_e">
416 <chain base_link="base_link" tip_link="link_e"/>
417 <joint name="base_joint"/>
418 </group>
419 <group name="base_with_bad_subgroups">
420 <group name="error"/>
421 </group>
422 </robot>
423 )xml";
424
425 urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
426 auto srdf_model = std::make_shared<srdf::Model>();
427 srdf_model->initString(*urdf_model, SMODEL2);
428 robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
429 }
430
431 void TearDown() override
432 {
433 }
434
435protected:
436 moveit::core::RobotModelConstPtr robot_model_;
437};
438
439TEST_F(OneRobot, setToDefaultValues)
440{
441 moveit::core::RobotState state(robot_model_);
442 state.setToDefaultValues();
443
444 // Get default values for joint_a.
445 double joint_a_default_position = state.getVariablePosition("joint_a");
446 double joint_a_default_velocity = state.getVariableVelocity("joint_a");
447 double joint_a_default_acceleration = state.getVariableAcceleration("joint_a");
448
449 // Set joint_a to some values.
450 state.setVariablePosition("joint_a", 1.0);
451 state.setVariableVelocity("joint_a", 2.0);
452 state.setVariableAcceleration("joint_a", 3.0);
453
454 // Check that joint_a has the right values.
455 EXPECT_EQ(state.getVariablePosition("joint_a"), 1.0);
456 EXPECT_EQ(state.getVariableVelocity("joint_a"), 2.0);
457 EXPECT_EQ(state.getVariableAcceleration("joint_a"), 3.0);
458
459 // Set to default values.
460 // Check that joint_a is back to the default values.
461 state.setToDefaultValues();
462 EXPECT_EQ(state.getVariablePosition("joint_a"), joint_a_default_position);
463 EXPECT_EQ(state.getVariableVelocity("joint_a"), joint_a_default_velocity);
464 EXPECT_EQ(state.getVariableAcceleration("joint_a"), joint_a_default_acceleration);
465}
466
467TEST_F(OneRobot, FK)
468{
469 moveit::core::RobotModelConstPtr model = robot_model_;
470
471 // testing that the two planning groups are the same
472 const moveit::core::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints");
473 const moveit::core::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip");
474 const moveit::core::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups");
475 const moveit::core::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups");
476 const moveit::core::JointModelGroup* g_mim = model->getJointModelGroup("mim_joints");
477
478 ASSERT_TRUE(g_one != nullptr);
479 ASSERT_TRUE(g_two != nullptr);
480 ASSERT_TRUE(g_three != nullptr);
481 ASSERT_TRUE(g_four == nullptr);
482
483 // joint_b is a fixed joint, so no one should have it
484 ASSERT_EQ(g_one->getJointModelNames().size(), 3u);
485 ASSERT_EQ(g_two->getJointModelNames().size(), 3u);
486 ASSERT_EQ(g_three->getJointModelNames().size(), 4u);
487 ASSERT_EQ(g_mim->getJointModelNames().size(), 2u);
488
489 // only the links in between the joints, and the children of the leafs
490 ASSERT_EQ(g_one->getLinkModelNames().size(), 3u);
491 // g_two only has three links
492 ASSERT_EQ(g_two->getLinkModelNames().size(), 3u);
493 ASSERT_EQ(g_three->getLinkModelNames().size(), 4u);
494
495 std::vector<std::string> jmn = g_one->getJointModelNames();
496 std::sort(jmn.begin(), jmn.end());
497 EXPECT_EQ(jmn[0], "base_joint");
498 EXPECT_EQ(jmn[1], "joint_a");
499 EXPECT_EQ(jmn[2], "joint_c");
500 jmn = g_two->getJointModelNames();
501 std::sort(jmn.begin(), jmn.end());
502 EXPECT_EQ(jmn[0], "base_joint");
503 EXPECT_EQ(jmn[1], "joint_a");
504 EXPECT_EQ(jmn[2], "joint_b");
505 jmn = g_three->getJointModelNames();
506 std::sort(jmn.begin(), jmn.end());
507 EXPECT_EQ(jmn[0], "base_joint");
508 EXPECT_EQ(jmn[1], "joint_a");
509 EXPECT_EQ(jmn[2], "joint_b");
510 EXPECT_EQ(jmn[3], "joint_c");
511
512 // but they should have the same links to be updated
513 ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 6u);
514 ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 6u);
515 ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 6u);
516
517 EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(), "base_link");
518 EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(), "link_a");
519 EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(), "link_b");
520 EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(), "link_c");
521
522 EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(), "base_link");
523 EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(), "link_a");
524 EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(), "link_b");
525 EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(), "link_c");
526
527 EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(), "base_link");
528 EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(), "link_a");
529 EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(), "link_b");
530 EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(), "link_c");
531
532 // bracketing so the state gets destroyed before we bring down the model
533
534 moveit::core::RobotState state(model);
535
536 EXPECT_EQ(7u, state.getVariableCount());
537
538 state.setToDefaultValues();
539
540 std::map<std::string, double> joint_values;
541 joint_values["base_joint/x"] = 1.0;
542 joint_values["base_joint/y"] = 1.0;
543 joint_values["base_joint/theta"] = 0.5;
544 joint_values["joint_a"] = -0.5;
545 joint_values["joint_c"] = 0.08;
546 state.setVariablePositions(joint_values);
547
548 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(1, 1, 0));
549 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).x(), 1e-5);
550 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).y(), 1e-5);
551 EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).z(), 1e-5);
552 EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getGlobalLinkTransform("base_link").linear()).w(), 1e-5);
553
554 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_a").translation(), Eigen::Vector3d(1, 1, 0));
555 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).x(), 1e-5);
556 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).y(), 1e-5);
557 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).z(), 1e-5);
558 EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_a").linear()).w(), 1e-5);
559
560 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_b").translation(), Eigen::Vector3d(1, 1.5, 0));
561 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).x(), 1e-5);
562 EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).y(), 1e-5);
563 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).z(), 1e-5);
564 EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getGlobalLinkTransform("link_b").linear()).w(), 1e-5);
565
566 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(1.08, 1.4, 0));
567 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).x(), 1e-5);
568 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).y(), 1e-5);
569 EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).z(), 1e-5);
570 EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getGlobalLinkTransform("link_c").linear()).w(), 1e-5);
571
572 EXPECT_TRUE(state.satisfiesBounds());
573
574 std::map<std::string, double> upd_a;
575 upd_a["joint_a"] = 0.2;
576 state.setVariablePositions(upd_a);
577 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
578 EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
579 state.enforceBounds();
580 EXPECT_NEAR(state.getVariablePosition("joint_a"), 0.2, 1e-3);
581
582 upd_a["joint_a"] = 3.2;
583 state.setVariablePositions(upd_a);
584 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
585 EXPECT_NEAR(state.getVariablePosition("joint_a"), 3.2, 1e-3);
586 state.enforceBounds();
587 EXPECT_NEAR(state.getVariablePosition("joint_a"), -3.083185, 1e-3);
588 EXPECT_TRUE(state.satisfiesBounds(model->getJointModel("joint_a")));
589
590 // mimic joints
591 state.setToDefaultValues();
592 EXPECT_TRUE(state.dirtyLinkTransforms());
593 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
594 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.2, 0.5, 0));
595 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(0.3, 0.6, 0));
596
597 // setVariablePosition should update corresponding mimic joints too
598 state.setVariablePosition("joint_f", 1.0);
599 EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
600 EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
601 EXPECT_TRUE(state.dirtyLinkTransforms());
602 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
603 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
604 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
605
606 ASSERT_EQ(g_mim->getVariableCount(), 2u);
607 double gstate[2];
608 state.copyJointGroupPositions(g_mim, gstate);
609
610 // setToRandomPositions() uses a different mechanism to update mimic joints
611 state.setToRandomPositions(g_mim);
612 double joint_f = state.getVariablePosition("joint_f");
613 double mim_f = state.getVariablePosition("mim_f");
614 EXPECT_NEAR(mim_f, 1.5 * joint_f + 0.1, 1e-8);
615 EXPECT_TRUE(state.dirtyLinkTransforms());
616 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
617 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(0.1 + mim_f, 0.5, 0));
618 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(),
619 Eigen::Vector3d(0.1 + mim_f + joint_f + 0.1, 0.6, 0));
620
621 // setJointGroupPositions() uses a different mechanism to update mimic joints
622 state.setJointGroupPositions(g_mim, gstate);
623 EXPECT_EQ(state.getVariablePosition("joint_f"), 1.0);
624 EXPECT_EQ(state.getVariablePosition("mim_f"), 1.6);
625 EXPECT_TRUE(state.dirtyLinkTransforms());
626 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_c").translation(), Eigen::Vector3d(0.0, 0.4, 0));
627 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_d").translation(), Eigen::Vector3d(1.7, 0.5, 0));
628 EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("link_e").translation(), Eigen::Vector3d(2.8, 0.6, 0));
629}
630
631TEST_F(OneRobot, testPrintCurrentPositionWithJointLimits)
632{
633 moveit::core::RobotState state(robot_model_);
634 const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("base_from_base_to_e");
635 ASSERT_TRUE(joint_model_group);
636
637 state.setToDefaultValues();
638
639 std::cout << "\nVisual inspection should show NO joints out of bounds:\n";
640 state.printStatePositionsWithJointLimits(joint_model_group);
641
642 std::cout << "\nVisual inspection should show ONE joint out of bounds:\n";
643 std::vector<double> single_joint(1);
644 single_joint[0] = -1.0;
645 state.setJointPositions("joint_c", single_joint);
646 state.printStatePositionsWithJointLimits(joint_model_group);
647
648 std::cout << "\nVisual inspection should show TWO joint out of bounds:\n";
649 single_joint[0] = 1.0;
650 state.setJointPositions("joint_f", single_joint);
651 state.printStatePositionsWithJointLimits(joint_model_group);
652
653 std::cout << "\nVisual inspection should show ONE joint out of bounds:\n";
654 single_joint[0] = 0.19;
655 state.setJointPositions("joint_f", single_joint);
656 state.printStatePositionsWithJointLimits(joint_model_group);
657}
658
659TEST_F(OneRobot, testInterpolation)
660{
661 moveit::core::RobotState state_a(robot_model_);
662
663 // Interpolate with itself
664 state_a.setToDefaultValues();
665 moveit::core::RobotState state_b(state_a);
666 moveit::core::RobotState interpolated_state(state_a);
667 for (size_t i = 0; i <= 10; ++i)
668 {
669 state_a.interpolate(state_b, static_cast<double>(i) / 10., interpolated_state,
670 robot_model_->getJointModelGroup("base_from_base_to_e"));
671 EXPECT_NEAR(state_a.distance(state_b), 0, EPSILON) << "Interpolation between identical states yielded a different "
672 "state.";
673
674 for (const auto& link_name : robot_model_->getLinkModelNames())
675 {
676 EXPECT_FALSE(interpolated_state.getCollisionBodyTransform(link_name, 0).matrix().hasNaN()) << "Interpolation "
677 "between identical "
678 "states yielded "
679 "NaN value.";
680 }
681 }
682
683 // Some simple interpolation
684 std::map<std::string, double> joint_values;
685 joint_values["base_joint/x"] = 1.0;
686 joint_values["base_joint/y"] = 1.0;
687 state_a.setVariablePositions(joint_values);
688 joint_values["base_joint/x"] = 0.0;
689 joint_values["base_joint/y"] = 2.0;
690 state_b.setVariablePositions(joint_values);
691 EXPECT_NEAR(3 * std::sqrt(2), state_a.distance(state_b), EPSILON) << "Simple interpolation of base_joint failed.";
692
693 state_a.interpolate(state_b, 0.5, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
694 EXPECT_NEAR(0., state_a.distance(interpolated_state) - state_b.distance(interpolated_state), EPSILON) << "Simple "
695 "interpolati"
696 "on of "
697 "base_joint "
698 "failed.";
699 EXPECT_NEAR(0.5, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) << "Simple interpolation of "
700 "base_joint failed.";
701 EXPECT_NEAR(1.5, interpolated_state.getVariablePosition("base_joint/y"), EPSILON) << "Simple interpolation of "
702 "base_joint failed.";
703 state_a.interpolate(state_b, 0.1, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
704 EXPECT_NEAR(0.9, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) << "Simple interpolation of "
705 "base_joint failed.";
706 EXPECT_NEAR(1.1, interpolated_state.getVariablePosition("base_joint/y"), EPSILON) << "Simple interpolation of "
707 "base_joint failed.";
708
709 // Interpolate all the joints
710 joint_values["base_joint/x"] = 0.0;
711 joint_values["base_joint/y"] = 20.0;
712 joint_values["base_joint/theta"] = 3 * M_PI / 4;
713 joint_values["joint_a"] = -4 * M_PI / 5;
714 joint_values["joint_c"] = 0.0;
715 joint_values["joint_f"] = 1.0;
716 state_a.setVariablePositions(joint_values);
717
718 joint_values["base_joint/x"] = 10.0;
719 joint_values["base_joint/y"] = 0.0;
720 joint_values["base_joint/theta"] = -3 * M_PI / 4;
721 joint_values["joint_a"] = 4 * M_PI / 5;
722 joint_values["joint_c"] = 0.07;
723 joint_values["joint_f"] = 0.0;
724 state_b.setVariablePositions(joint_values);
725
726 for (size_t i = 0; i <= 5; ++i)
727 {
728 double t = static_cast<double>(i) / 5.;
729 state_a.interpolate(state_b, t, interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
730 EXPECT_NEAR(10.0 * t, interpolated_state.getVariablePosition("base_joint/x"), EPSILON) << "Base joint "
731 "interpolation failed.";
732 EXPECT_NEAR(20.0 * (1 - t), interpolated_state.getVariablePosition("base_joint/y"), EPSILON) << "Base joint "
733 "interpolation "
734 "failed.";
735 if (t < 0.5)
736 {
737 EXPECT_NEAR(3 * M_PI / 4 + (M_PI / 2) * t, interpolated_state.getVariablePosition("base_joint/theta"), EPSILON)
738 << "Base joint theta interpolation failed.";
739 EXPECT_NEAR(-4 * M_PI / 5 - (2 * M_PI / 5) * t, interpolated_state.getVariablePosition("joint_a"), EPSILON)
740 << "Continuous joint interpolation failed.";
741 }
742 else
743 {
744 EXPECT_NEAR(-3 * M_PI / 4 - (M_PI / 2) * (1 - t), interpolated_state.getVariablePosition("base_joint/theta"),
745 EPSILON)
746 << "Base joint theta interpolation failed.";
747 EXPECT_NEAR(4 * M_PI / 5 + (2 * M_PI / 5) * (1 - t), interpolated_state.getVariablePosition("joint_a"), EPSILON)
748 << "Continuous joint interpolation failed.";
749 }
750 EXPECT_NEAR(0.07 * t, interpolated_state.getVariablePosition("joint_c"), EPSILON) << "Interpolation of joint_c "
751 "failed.";
752 EXPECT_NEAR(1 - t, interpolated_state.getVariablePosition("joint_f"), EPSILON) << "Interpolation of joint_f "
753 "failed.";
754 EXPECT_NEAR(1.5 * (1 - t) + 0.1, interpolated_state.getVariablePosition("mim_f"), EPSILON) << "Interpolation of "
755 "mimic joint mim_f "
756 "failed.";
757 }
758
759 bool nan_exception = false;
760 try
761 {
762 state_a.interpolate(state_b, 1. / 0., interpolated_state, robot_model_->getJointModelGroup("base_from_base_to_e"));
763 }
764 catch (std::exception& e)
765 {
766 std::cout << "Caught expected exception: " << e.what() << '\n';
767 nan_exception = true;
768 }
769 EXPECT_TRUE(nan_exception) << "NaN interpolation parameter did not create expected exception.";
770}
771
772TEST_F(OneRobot, rigidlyConnectedParent)
773{
774 // link_e is its own rigidly-connected parent
775 const moveit::core::LinkModel* link_e{ robot_model_->getLinkModel("link_e") };
776 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
777
778 // link_b is rigidly connected to its parent link_a
779 const moveit::core::LinkModel* link_a{ robot_model_->getLinkModel("link_a") };
780 const moveit::core::LinkModel* link_b{ robot_model_->getLinkModel("link_b") };
781 EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
782
783 moveit::core::RobotState state(robot_model_);
784 state.setToDefaultValues();
785 state.updateLinkTransforms();
786
787 EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);
788
789 // attach "object" with "subframe" to link_b
790 state.attachBody(std::make_unique<moveit::core::AttachedBody>(
791 link_b, "object", Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{},
792 EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{},
793 moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } }));
794
795 // RobotState's version should resolve these too
796 EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object"));
797 EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe"));
798
799 // test failure cases
800 EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object"));
801 EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/no_subframe"));
802 EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel(""));
803 EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/"));
804 EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/"));
805}
806
807TEST(getJacobian, RevoluteJoints)
808{
809 // Robot URDF with four revolute joints.
810 constexpr char robot_urdf[] = R"(
811 <?xml version="1.0" ?>
812 <robot name="one_robot">
813 <link name="base_link"/>
814 <joint name="joint_a_revolute" type="revolute">
815 <axis xyz="0 0 1"/>
816 <parent link="base_link"/>
817 <child link="link_a"/>
818 <origin rpy="0 0 0" xyz="0 0 0"/>
819 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
820 </joint>
821 <link name="link_a"/>
822 <joint name="joint_b_revolute" type="revolute">
823 <axis xyz="0 0 1"/>
824 <parent link="link_a"/>
825 <child link="link_b"/>
826 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
827 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
828 </joint>
829 <link name="link_b"/>
830 <joint name="joint_c_revolute" type="revolute">
831 <axis xyz="0 1 0"/>
832 <parent link="link_b"/>
833 <child link="link_c"/>
834 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
835 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
836 </joint>
837 <link name="link_c"/>
838 <joint name="joint_d_revolute" type="revolute">
839 <axis xyz="1 0 0"/>
840 <parent link="link_c"/>
841 <child link="link_d"/>
842 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
843 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
844 </joint>
845 <link name="link_d"/>
846 </robot>
847 )";
848
849 constexpr char robot_srdf[] = R"xml(
850 <?xml version="1.0" ?>
851 <robot name="one_robot">
852 <group name="base_to_tip">
853 <joint name="joint_a_revolute"/>
854 <joint name="joint_b_revolute"/>
855 <joint name="joint_c_revolute"/>
856 <joint name="joint_d_revolute"/>
857 </group>
858 </robot>
859 )xml";
860
861 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
862 ASSERT_TRUE(urdf_model);
863 const auto srdf_model = std::make_shared<srdf::Model>();
864 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
865 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
866
867 moveit::core::RobotState state(robot_model);
868 const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
869
870 // Some made-up numbers, at zero and non-zero robot configurations.
871 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }));
872 checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }));
873}
874
875TEST(getJacobian, RevoluteJointsButDifferentLink)
876{
877 // Robot URDF with four revolute joints.
878 constexpr char robot_urdf[] = R"(
879 <?xml version="1.0" ?>
880 <robot name="one_robot">
881 <link name="base_link"/>
882 <joint name="joint_a_revolute" type="revolute">
883 <axis xyz="0 0 1"/>
884 <parent link="base_link"/>
885 <child link="link_a"/>
886 <origin rpy="0 0 0" xyz="0 0 0"/>
887 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
888 </joint>
889 <link name="link_a"/>
890 <joint name="joint_b_revolute" type="revolute">
891 <axis xyz="0 0 1"/>
892 <parent link="link_a"/>
893 <child link="link_b"/>
894 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
895 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
896 </joint>
897 <link name="link_b"/>
898 <joint name="joint_c_revolute" type="revolute">
899 <axis xyz="0 1 0"/>
900 <parent link="link_b"/>
901 <child link="link_c"/>
902 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
903 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
904 </joint>
905 <link name="link_c"/>
906 <joint name="joint_d_revolute" type="revolute">
907 <axis xyz="1 0 0"/>
908 <parent link="link_c"/>
909 <child link="link_d"/>
910 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
911 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
912 </joint>
913 <link name="link_d"/>
914 </robot>
915 )";
916
917 constexpr char robot_srdf[] = R"xml(
918 <?xml version="1.0" ?>
919 <robot name="one_robot">
920 <group name="base_to_tip">
921 <joint name="joint_a_revolute"/>
922 <joint name="joint_b_revolute"/>
923 <joint name="joint_c_revolute"/>
924 <joint name="joint_d_revolute"/>
925 </group>
926 </robot>
927 )xml";
928
929 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
930 ASSERT_TRUE(urdf_model);
931 const auto srdf_model = std::make_shared<srdf::Model>();
932 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
933 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
934
935 moveit::core::RobotState state(robot_model);
936 const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
937
938 // Some made-up numbers, at zero and non-zero robot configurations.
939 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }),
940 state.getLinkModel("link_c"));
941 checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }),
942 state.getLinkModel("link_c"));
943}
944
945TEST(getJacobian, RevoluteAndPrismaticJoints)
946{
947 // Robot URDF with three revolute joints and one prismatic joint.
948 constexpr char robot_urdf[] = R"(
949 <?xml version="1.0" ?>
950 <robot name="one_robot">
951 <link name="base_link"/>
952 <joint name="joint_a_revolute" type="revolute">
953 <axis xyz="0 0 1"/>
954 <parent link="base_link"/>
955 <child link="link_a"/>
956 <origin rpy="0 0 0" xyz="0 0 0"/>
957 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
958 </joint>
959 <link name="link_a"/>
960 <joint name="joint_b_revolute" type="revolute">
961 <axis xyz="0 0 1"/>
962 <parent link="link_a"/>
963 <child link="link_b"/>
964 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
965 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
966 </joint>
967 <link name="link_b"/>
968 <joint name="joint_c_prismatic" type="prismatic">
969 <axis xyz="0 1 0"/>
970 <parent link="link_b"/>
971 <child link="link_c"/>
972 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
973 <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.2"/>
974 </joint>
975 <link name="link_c"/>
976 <joint name="joint_d_revolute" type="revolute">
977 <axis xyz="1 0 0"/>
978 <parent link="link_c"/>
979 <child link="link_d"/>
980 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
981 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
982 </joint>
983 <link name="link_d"/>
984 </robot>
985 )";
986
987 constexpr char robot_srdf[] = R"xml(
988 <?xml version="1.0" ?>
989 <robot name="one_robot">
990 <group name="base_to_tip">
991 <joint name="joint_a_revolute"/>
992 <joint name="joint_b_revolute"/>
993 <joint name="joint_c_prismatic"/>
994 <joint name="joint_d_revolute"/>
995 </group>
996 </robot>
997 )xml";
998
999 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1000 ASSERT_TRUE(urdf_model);
1001 const auto srdf_model = std::make_shared<srdf::Model>();
1002 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1003 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1004
1005 moveit::core::RobotState state(robot_model);
1006 const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
1007
1008 // Some made-up numbers, at zero and non-zero robot configurations.
1009 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.2, 0.3, 0.4 }));
1010 checkJacobian(state, *jmg, makeVector({ 0.1, 0.2, 0.3, 0.4 }), makeVector({ 0.5, 0.3, 0.2, 0.1 }));
1011}
1012
1013TEST(getJacobian, RevoluteAndFixedJoints)
1014{
1015 // Robot URDF with two revolute joints and two fixed joints.
1016 constexpr char robot_urdf[] = R"(
1017 <?xml version="1.0" ?>
1018 <robot name="one_robot">
1019 <link name="base_link"/>
1020 <joint name="joint_a_revolute" type="revolute">
1021 <axis xyz="0 0 1"/>
1022 <parent link="base_link"/>
1023 <child link="link_a"/>
1024 <origin rpy="0 0 0" xyz="0 0 0"/>
1025 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1026 </joint>
1027 <link name="link_a"/>
1028 <joint name="joint_b_fixed" type="fixed">
1029 <parent link="link_a"/>
1030 <child link="link_b"/>
1031 </joint>
1032 <link name="link_b"/>
1033 <joint name="joint_c_fixed" type="fixed">
1034 <parent link="link_b"/>
1035 <child link="link_c"/>
1036 </joint>
1037 <link name="link_c"/>
1038 <joint name="joint_d_revolute" type="revolute">
1039 <axis xyz="1 0 0"/>
1040 <parent link="link_c"/>
1041 <child link="link_d"/>
1042 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1043 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1044 </joint>
1045 <link name="link_d"/>
1046 </robot>
1047 )";
1048
1049 constexpr char robot_srdf[] = R"xml(
1050 <?xml version="1.0" ?>
1051 <robot name="one_robot">
1052 <group name="base_to_tip">
1053 <joint name="joint_a_revolute"/>
1054 <joint name="joint_b_fixed"/>
1055 <joint name="joint_c_fixed"/>
1056 <joint name="joint_d_revolute"/>
1057 </group>
1058 </robot>
1059 )xml";
1060
1061 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1062 ASSERT_TRUE(urdf_model);
1063 const auto srdf_model = std::make_shared<srdf::Model>();
1064 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1065 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1066
1067 moveit::core::RobotState state(robot_model);
1068 const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
1069
1070 // Some made-up numbers, at zero and non-zero robot configurations.
1071 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0 }), makeVector({ 0.1, 0.4 }));
1072 checkJacobian(state, *jmg, makeVector({ 0.1, 0.4 }), makeVector({ 0.5, 0.1 }));
1073}
1074
1075TEST(getJacobian, RevolutePlanarAndPrismaticJoints)
1076{
1077 // Robot URDF with two revolute joints, one planar joint and one prismatic.
1078 constexpr char robot_urdf[] = R"(
1079 <?xml version="1.0" ?>
1080 <robot name="one_robot">
1081 <link name="base_link"/>
1082 <joint name="joint_a_planar" type="planar">
1083 <axis xyz="0 0 1"/>
1084 <parent link="base_link"/>
1085 <child link="link_a"/>
1086 <origin rpy="0 0 0" xyz="0 0 0"/>
1087 <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.2"/>
1088 </joint>
1089 <link name="link_a"/>
1090 <joint name="joint_b_revolute" type="revolute">
1091 <axis xyz="0 0 1"/>
1092 <parent link="link_a"/>
1093 <child link="link_b"/>
1094 <origin rpy="0 0 0" xyz="0.0 0.5 0"/>
1095 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1096 </joint>
1097 <link name="link_b"/>
1098 <joint name="joint_c_prismatic" type="prismatic">
1099 <axis xyz="0 1 0"/>
1100 <parent link="link_b"/>
1101 <child link="link_c"/>
1102 <origin rpy="0 0 0" xyz="0.2 0.2 0"/>
1103 <limit effort="100.0" lower="-1.0" upper="1.0" velocity="0.2"/>
1104 </joint>
1105 <link name="link_c"/>
1106 <joint name="joint_d_revolute" type="revolute">
1107 <axis xyz="1 0 0"/>
1108 <parent link="link_c"/>
1109 <child link="link_d"/>
1110 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1111 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1112 </joint>
1113 <link name="link_d"/>
1114 </robot>
1115 )";
1116
1117 constexpr char robot_srdf[] = R"xml(
1118 <?xml version="1.0" ?>
1119 <robot name="one_robot">
1120 <group name="base_to_tip">
1121 <joint name="joint_a_planar"/>
1122 <joint name="joint_b_revolute"/>
1123 <joint name="joint_c_prismatic"/>
1124 <joint name="joint_d_revolute"/>
1125 </group>
1126 </robot>
1127 )xml";
1128
1129 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1130 ASSERT_TRUE(urdf_model);
1131 const auto srdf_model = std::make_shared<srdf::Model>();
1132 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1133 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1134
1135 moveit::core::RobotState state(robot_model);
1136 const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
1137
1138 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }),
1139 makeVector({ 0.2, 0.05, 0.1, 0.2, 0.3, 0.4 }));
1140}
1141
1142TEST(getJacobian, GroupNotAtOrigin)
1143{
1144 // URDF with a 3 DOF robot not at the URDF origin.
1145 constexpr char robot_urdf[] = R"(
1146 <?xml version="1.0" ?>
1147 <robot name="one_robot">
1148 <link name="origin"/>
1149 <joint name="fixed_offset" type="fixed">
1150 <parent link="origin"/>
1151 <child link="link_a"/>
1152 <origin rpy="0 0 1.5707" xyz="0.0 0.0 0.0"/>
1153 </joint>
1154 <link name="link_a"/>
1155 <joint name="joint_a_revolute" type="revolute">
1156 <axis xyz="1 0 0"/>
1157 <parent link="link_a"/>
1158 <child link="link_b"/>
1159 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1160 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1161 </joint>
1162 <link name="link_b"/>
1163 <joint name="joint_b_revolute" type="revolute">
1164 <axis xyz="1 0 0"/>
1165 <parent link="link_b"/>
1166 <child link="link_c"/>
1167 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1168 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1169 </joint>
1170 <link name="link_c"/>
1171 <joint name="joint_c_revolute" type="revolute">
1172 <axis xyz="1 0 0"/>
1173 <parent link="link_c"/>
1174 <child link="link_d"/>
1175 <origin rpy="0 0 0" xyz="0.0 0.2 0.4"/>
1176 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1177 </joint>
1178 <link name="link_d"/>
1179 </robot>
1180 )";
1181
1182 constexpr char robot_srdf[] = R"xml(
1183 <?xml version="1.0" ?>
1184 <robot name="one_robot">
1185 <group name="base_to_tip">
1186 <joint name="joint_a_revolute"/>
1187 <joint name="joint_b_revolute"/>
1188 <joint name="joint_c_revolute"/>
1189 </group>
1190 </robot>
1191 )xml";
1192
1193 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1194 ASSERT_TRUE(urdf_model);
1195 const auto srdf_model = std::make_shared<srdf::Model>();
1196 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1197 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1198
1199 moveit::core::RobotState state(robot_model);
1200 const moveit::core::JointModelGroup* jmg = state.getJointModelGroup("base_to_tip");
1201
1202 // Some made-up numbers, at zero and non-zero robot configurations.
1203 checkJacobian(state, *jmg, makeVector({ 0.0, 0.0, 0.0 }), makeVector({ 0.1, 0.4, 0.2 }));
1204 checkJacobian(state, *jmg, makeVector({ 0.1, 0.4, 0.3 }), makeVector({ 0.5, 0.1, 0.2 }));
1205}
1206
1207TEST(getJointPositions, getFixedJointValue)
1208{
1209 // Robot URDF with two revolute joints and a final fixed joint.
1210 // Trying to get the value of the fixed joint should work and return nullptr.
1211 constexpr char robot_urdf[] = R"(
1212 <?xml version="1.0" ?>
1213 <robot name="one_robot">
1214 <link name="base_link"/>
1215 <joint name="joint_a_revolute" type="revolute">
1216 <axis xyz="0 0 1"/>
1217 <parent link="base_link"/>
1218 <child link="link_a"/>
1219 <origin rpy="0 0 0" xyz="0 0 0"/>
1220 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1221 </joint>
1222 <link name="link_a"/>
1223 <joint name="joint_b_revolute" type="revolute">
1224 <axis xyz="0 0 1"/>
1225 <parent link="link_a"/>
1226 <child link="link_b"/>
1227 <origin rpy="0 0 0" xyz="0 0 0"/>
1228 <limit effort="100.0" lower="-3.14" upper="3.14" velocity="0.2"/>
1229 </joint>
1230 <link name="link_b"/>
1231 <joint name="joint_c_fixed" type="fixed">
1232 <parent link="link_b"/>
1233 <child link="link_c"/>
1234 </joint>
1235 <link name="link_c"/>
1236 </robot>
1237 )";
1238
1239 constexpr char robot_srdf[] = R"xml(
1240 <?xml version="1.0" ?>
1241 <robot name="one_robot">
1242 <group name="base_to_tip">
1243 <joint name="joint_a_revolute"/>
1244 <joint name="joint_b_revolute"/>
1245 <joint name="joint_c_fixed"/>
1246 </group>
1247 </robot>
1248 )xml";
1249
1250 const urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(robot_urdf);
1251 ASSERT_TRUE(urdf_model);
1252 const auto srdf_model = std::make_shared<srdf::Model>();
1253 ASSERT_TRUE(srdf_model->initString(*urdf_model, robot_srdf));
1254 const auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
1255
1256 // Getting the value of the last fixed joint should return nullptr, since it doesn't have an active variable.
1257 moveit::core::RobotState state(robot_model);
1258 state.setToDefaultValues();
1259 const double* joint_value = state.getJointPositions("joint_c_fixed");
1260 ASSERT_EQ(joint_value, nullptr);
1261}
1262
1263int main(int argc, char** argv)
1264{
1265 testing::InitGoogleTest(&argc, argv);
1266 return RUN_ALL_TESTS();
1267}
OneRobot::TearDown
void TearDown() override
Definition robot_state_test.cpp:431
OneRobot::SetUp
void SetUp() override
Definition robot_state_test.cpp:247
OneRobot::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition robot_state_test.cpp:436
moveit::core::JointModelGroup::getJointModelNames
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...
Definition joint_model_group.h:151
moveit::core::JointModelGroup::getLinkModels
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
Definition joint_model_group.h:213
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition joint_model_group.h:410
moveit::core::JointModelGroup::getLinkModelNames
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
Definition joint_model_group.h:219
moveit::core::JointModelGroup::getUpdatedLinkModels
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....
Definition joint_model_group.h:234
moveit::core::JointModelGroup::getJointModels
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Definition joint_model_group.h:144
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition joint_model.h:117
moveit::core::JointModel::getParentLinkModel
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint,...
Definition joint_model.h:162
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition link_model.h:72
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition robot_model_test_utils.h:112
moveit::core::RobotModelBuilder::addInertial
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)
Definition robot_model_test_utils.cpp:276
moveit::core::RobotModelBuilder::addCollisionBox
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.
Definition robot_model_test_utils.cpp:313
moveit::core::RobotModelBuilder::isValid
bool isValid()
Returns true if the building process so far has been valid.
Definition robot_model_test_utils.cpp:448
moveit::core::RobotModelBuilder::addGroup
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.
Definition robot_model_test_utils.cpp:421
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition robot_model_test_utils.cpp:453
moveit::core::RobotModelBuilder::addVirtualJoint
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.
Definition robot_model_test_utils.cpp:388
moveit::core::RobotModelBuilder::addVisualBox
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.
Definition robot_model_test_utils.cpp:303
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
Definition robot_state.cpp:349
moveit::core::RobotState::setVariableAcceleration
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.
Definition robot_state.h:381
moveit::core::RobotState::getVariableAcceleration
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
Definition robot_state.h:395
moveit::core::RobotState::attachBody
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
Definition robot_state.cpp:1167
moveit::core::RobotState::getJacobian
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.
Definition robot_state.cpp:1461
moveit::core::RobotState::getJointPositions
const double * getJointPositions(const std::string &joint_name) const
Definition robot_state.h:543
moveit::core::RobotState::setVariableVelocity
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.
Definition robot_state.h:282
moveit::core::RobotState::getVariableVelocity
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
Definition robot_state.h:296
moveit::core::RobotState::setVariablePosition
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.
Definition robot_state.h:188
moveit::core::RobotState::setJointGroupPositions
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...
Definition robot_state.h:583
moveit::core::RobotState::copyJointGroupPositions
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...
Definition robot_state.h:669
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition robot_state.h:134
moveit::core::RobotState::printStatePositionsWithJointLimits
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.
Definition robot_state.cpp:2255
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition robot_state.h:122
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition robot_state.h:104
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
Definition robot_state.cpp:775
moveit::core::RobotState::interpolate
void interpolate(const RobotState &to, double t, RobotState &state) const
Definition robot_state.cpp:1111
moveit::core::RobotState::getCollisionBodyTransform
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...
Definition robot_state.h:1286
moveit::core::RobotState::distance
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints.
Definition robot_state.h:1354
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition robot_state.cpp:262
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
Definition robot_state.cpp:937
moveit::core::RobotState::dirtyLinkTransforms
bool dirtyLinkTransforms() const
Definition robot_state.h:1331
moveit::core::RobotState::getVariablePosition
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition robot_state.h:207
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition robot_state.cpp:339
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition robot_state.h:110
moveit::core::RobotState::getGlobalLinkTransform
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...
Definition robot_state.h:1246
moveit::core::RobotState::getRigidlyConnectedParentLinkModel
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.
Definition robot_state.cpp:910
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition robot_state.h:515
robot_model.h
robot_state.h
moveit::core::FixedTransformsMap
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...
Definition transforms.h:53
robot_model_test_utils.h
main
int main(int argc, char **argv)
Definition robot_state_test.cpp:1263
TEST
TEST(Loading, SimpleRobot)
Definition robot_state_test.cpp:148
EXPECT_NEAR_TRACED
#define EXPECT_NEAR_TRACED(...)
Definition robot_state_test.cpp:142
TEST_F
TEST_F(OneRobot, setToDefaultValues)
Definition robot_state_test.cpp:439
EPSILON
#define EPSILON
Definition unittest_velocity_profile_atrap.cpp:40
Generated by doxygen 1.9.8