moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
robot_state.hpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Ioan A. Sucan
5 * Copyright (c) 2013, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Ioan Sucan */
37
38#pragma once
39
43#include <sensor_msgs/msg/joint_state.hpp>
44#include <visualization_msgs/msg/marker_array.hpp>
45#include <std_msgs/msg/color_rgba.hpp>
46#include <geometry_msgs/msg/twist.hpp>
47#include <cassert>
48
49#include <rclcpp/duration.hpp>
50
51/* Terminology
52 * Model Frame: RobotModel's root frame == PlanningScene's planning frame
53 If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint.
54 Otherwise, it is the root link of the URDF model.
55 * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics
56*/
57
58namespace moveit
59{
60namespace core
61{
62MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc
63
68typedef std::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
69 const double* joint_group_variable_values)>
71
90{
91public:
94 RobotState(const RobotModelConstPtr& robot_model);
96
98 RobotState(const RobotState& other);
99
101 RobotState& operator=(const RobotState& other);
102
104 const RobotModelConstPtr& getRobotModel() const
105 {
106 return robot_model_;
107 }
108
110 std::size_t getVariableCount() const
111 {
112 return robot_model_->getVariableCount();
113 }
114
116 const std::vector<std::string>& getVariableNames() const
117 {
118 return robot_model_->getVariableNames();
119 }
120
122 const LinkModel* getLinkModel(const std::string& link) const
123 {
124 return robot_model_->getLinkModel(link);
125 }
126
128 const JointModel* getJointModel(const std::string& joint) const
129 {
130 return robot_model_->getJointModel(joint);
131 }
132
134 const JointModelGroup* getJointModelGroup(const std::string& group) const
135 {
136 return robot_model_->getJointModelGroup(group);
137 }
138
148 {
149 return position_.data();
150 }
151
154 const double* getVariablePositions() const
155 {
156 return position_.data();
157 }
158
162 void setVariablePositions(const double* position);
163
167 void setVariablePositions(const std::vector<double>& position)
168 {
169 assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
170 setVariablePositions(&position[0]);
171 }
172
175 void setVariablePositions(const std::map<std::string, double>& variable_map);
176
179 void setVariablePositions(const std::map<std::string, double>& variable_map,
180 std::vector<std::string>& missing_variables);
181
184 void setVariablePositions(const std::vector<std::string>& variable_names,
185 const std::vector<double>& variable_position);
186
188 void setVariablePosition(const std::string& variable, double value)
189 {
190 setVariablePosition(robot_model_->getVariableIndex(variable), value);
191 }
192
195 void setVariablePosition(int index, double value)
196 {
197 position_[index] = value;
198 const JointModel* jm = robot_model_->getJointOfVariable(index);
199 if (jm)
200 {
201 markDirtyJointTransforms(jm);
202 updateMimicJoint(jm);
203 }
204 }
205
207 double getVariablePosition(const std::string& variable) const
208 {
209 return position_[robot_model_->getVariableIndex(variable)];
210 }
211
215 double getVariablePosition(int index) const
216 {
217 return position_[index];
218 }
219
229 bool hasVelocities() const
230 {
231 return has_velocity_;
232 }
233
237 {
238 markVelocity();
239 return velocity_.data();
240 }
241
244 const double* getVariableVelocities() const
245 {
246 return velocity_.data();
247 }
248
250 void zeroVelocities();
251
253 void setVariableVelocities(const double* velocity)
254 {
255 has_velocity_ = true;
256 // assume everything is in order in terms of array lengths (for efficiency reasons)
257 memcpy(velocity_.data(), velocity, robot_model_->getVariableCount() * sizeof(double));
258 }
259
261 void setVariableVelocities(const std::vector<double>& velocity)
262 {
263 assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
264 setVariableVelocities(&velocity[0]);
265 }
266
269 void setVariableVelocities(const std::map<std::string, double>& variable_map);
270
273 void setVariableVelocities(const std::map<std::string, double>& variable_map,
274 std::vector<std::string>& missing_variables);
275
278 void setVariableVelocities(const std::vector<std::string>& variable_names,
279 const std::vector<double>& variable_velocity);
280
282 void setVariableVelocity(const std::string& variable, double value)
283 {
284 setVariableVelocity(robot_model_->getVariableIndex(variable), value);
285 }
286
289 void setVariableVelocity(int index, double value)
290 {
291 markVelocity();
292 velocity_[index] = value;
293 }
294
296 double getVariableVelocity(const std::string& variable) const
297 {
298 return velocity_[robot_model_->getVariableIndex(variable)];
299 }
300
304 double getVariableVelocity(int index) const
305 {
306 return velocity_[index];
307 }
308
310 void dropVelocities();
311
322 bool hasAccelerations() const
323 {
324 return has_acceleration_;
325 }
326
331 {
332 markAcceleration();
333 return effort_or_acceleration_.data();
334 }
335
338 const double* getVariableAccelerations() const
339 {
340 return effort_or_acceleration_.data();
341 }
342
344 void zeroAccelerations();
345
348 void setVariableAccelerations(const double* acceleration)
349 {
350 has_acceleration_ = true;
351 has_effort_ = false;
352
353 // assume everything is in order in terms of array lengths (for efficiency reasons)
354 memcpy(effort_or_acceleration_.data(), acceleration, robot_model_->getVariableCount() * sizeof(double));
355 }
356
359 void setVariableAccelerations(const std::vector<double>& acceleration)
360 {
361 assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
362 setVariableAccelerations(&acceleration[0]);
363 }
364
367 void setVariableAccelerations(const std::map<std::string, double>& variable_map);
368
372 void setVariableAccelerations(const std::map<std::string, double>& variable_map,
373 std::vector<std::string>& missing_variables);
374
377 void setVariableAccelerations(const std::vector<std::string>& variable_names,
378 const std::vector<double>& variable_acceleration);
379
381 void setVariableAcceleration(const std::string& variable, double value)
382 {
383 setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
384 }
385
388 void setVariableAcceleration(int index, double value)
389 {
390 markAcceleration();
391 effort_or_acceleration_[index] = value;
392 }
393
395 double getVariableAcceleration(const std::string& variable) const
396 {
397 return effort_or_acceleration_[robot_model_->getVariableIndex(variable)];
398 }
399
403 double getVariableAcceleration(int index) const
404 {
405 return effort_or_acceleration_[index];
406 }
407
409 void dropAccelerations();
410
420 bool hasEffort() const
421 {
422 return has_effort_;
423 }
424
429 {
430 markEffort();
431 return effort_or_acceleration_.data();
432 }
433
436 const double* getVariableEffort() const
437 {
438 return effort_or_acceleration_.data();
439 }
440
442 void zeroEffort();
443
445 void setVariableEffort(const double* effort)
446 {
447 has_effort_ = true;
448 has_acceleration_ = false;
449 // assume everything is in order in terms of array lengths (for efficiency reasons)
450 memcpy(effort_or_acceleration_.data(), effort, robot_model_->getVariableCount() * sizeof(double));
451 }
452
454 void setVariableEffort(const std::vector<double>& effort)
455 {
456 assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
457 setVariableEffort(&effort[0]);
458 }
459
461 void setVariableEffort(const std::map<std::string, double>& variable_map);
462
465 void setVariableEffort(const std::map<std::string, double>& variable_map, std::vector<std::string>& missing_variables);
466
468 void setVariableEffort(const std::vector<std::string>& variable_names,
469 const std::vector<double>& variable_acceleration);
470
472 void setVariableEffort(const std::string& variable, double value)
473 {
474 setVariableEffort(robot_model_->getVariableIndex(variable), value);
475 }
476
479 void setVariableEffort(int index, double value)
480 {
481 markEffort();
482 effort_or_acceleration_[index] = value;
483 }
484
486 double getVariableEffort(const std::string& variable) const
487 {
488 return effort_or_acceleration_[robot_model_->getVariableIndex(variable)];
489 }
490
494 double getVariableEffort(int index) const
495 {
496 return effort_or_acceleration_[index];
497 }
498
500 void dropEffort();
501
503 void dropDynamics();
504
506 void invertVelocity();
507
515 void setJointPositions(const std::string& joint_name, const double* position)
516 {
517 setJointPositions(robot_model_->getJointModel(joint_name), position);
518 }
519
520 void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
521 {
522 setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
523 }
524
525 void setJointPositions(const JointModel* joint, const std::vector<double>& position)
526 {
527 setJointPositions(joint, &position[0]);
528 }
529
530 void setJointPositions(const JointModel* joint, const double* position);
531
532 void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
533 {
534 setJointPositions(robot_model_->getJointModel(joint_name), transform);
535 }
536
537 void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform);
538
539 void setJointVelocities(const JointModel* joint, const double* velocity);
540
541 void setJointEfforts(const JointModel* joint, const double* effort);
542
543 const double* getJointPositions(const std::string& joint_name) const
544 {
545 return getJointPositions(robot_model_->getJointModel(joint_name));
546 }
547
548 // Returns nullptr if `joint` doesn't have any active variables.
549 const double* getJointPositions(const JointModel* joint) const;
550
551 const double* getJointVelocities(const std::string& joint_name) const
552 {
553 return getJointVelocities(robot_model_->getJointModel(joint_name));
554 }
555
556 // Returns nullptr if `joint` doesn't have any active variables.
557 const double* getJointVelocities(const JointModel* joint) const;
558
559 const double* getJointAccelerations(const std::string& joint_name) const
560 {
561 return getJointAccelerations(robot_model_->getJointModel(joint_name));
562 }
563
564 // Returns nullptr if `joint` doesn't have any active variables.
565 const double* getJointAccelerations(const JointModel* joint) const;
566
567 const double* getJointEffort(const std::string& joint_name) const
568 {
569 return getJointEffort(robot_model_->getJointModel(joint_name));
570 }
571
572 // Returns nullptr if `joint` doesn't have any active variables.
573 const double* getJointEffort(const JointModel* joint) const;
574
583 void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
584 {
585 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
586 if (jmg)
587 setJointGroupPositions(jmg, gstate);
588 }
589
592 void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
593 {
594 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
595 if (jmg)
596 {
597 assert(gstate.size() == jmg->getVariableCount());
598 setJointGroupPositions(jmg, &gstate[0]);
599 }
600 }
601
604 void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
605 {
606 assert(gstate.size() == group->getVariableCount());
607 setJointGroupPositions(group, &gstate[0]);
608 }
609
612 void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
613
616 void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
617 {
618 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
619 if (jmg)
620 {
621 assert(values.size() == jmg->getVariableCount());
622 setJointGroupPositions(jmg, values);
623 }
624 }
625
628 void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
629
633 void setJointGroupActivePositions(const JointModelGroup* group, const std::vector<double>& gstate);
634
638 void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector<double>& gstate)
639 {
640 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
641 if (jmg)
642 {
643 assert(gstate.size() == jmg->getActiveVariableCount());
644 setJointGroupActivePositions(jmg, gstate);
645 }
646 }
647
651 void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values);
652
656 void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
657 {
658 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
659 if (jmg)
660 {
661 assert(values.size() == jmg->getActiveVariableCount());
662 setJointGroupActivePositions(jmg, values);
663 }
664 }
665
669 void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
670 {
671 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
672 if (jmg)
673 {
674 gstate.resize(jmg->getVariableCount());
675 copyJointGroupPositions(jmg, &gstate[0]);
676 }
677 }
678
682 void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
683 {
684 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
685 if (jmg)
686 copyJointGroupPositions(jmg, gstate);
687 }
688
692 void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
693 {
694 gstate.resize(group->getVariableCount());
695 copyJointGroupPositions(group, &gstate[0]);
696 }
697
701 void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
702
706 void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
707 {
708 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
709 if (jmg)
710 copyJointGroupPositions(jmg, values);
711 }
712
716 void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
717
726 void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
727 {
728 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
729 if (jmg)
730 setJointGroupVelocities(jmg, gstate);
731 }
732
735 void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
736 {
737 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
738 if (jmg)
739 setJointGroupVelocities(jmg, &gstate[0]);
740 }
741
744 void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
745 {
746 setJointGroupVelocities(group, &gstate[0]);
747 }
748
751 void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
752
755 void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
756 {
757 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
758 if (jmg)
759 setJointGroupVelocities(jmg, values);
760 }
761
764 void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
765
769 void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
770 {
771 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
772 if (jmg)
773 {
774 gstate.resize(jmg->getVariableCount());
775 copyJointGroupVelocities(jmg, &gstate[0]);
776 }
777 }
778
782 void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
783 {
784 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
785 if (jmg)
786 copyJointGroupVelocities(jmg, gstate);
787 }
788
792 void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
793 {
794 gstate.resize(group->getVariableCount());
795 copyJointGroupVelocities(group, &gstate[0]);
796 }
797
801 void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
802
806 void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
807 {
808 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
809 if (jmg)
810 copyJointGroupVelocities(jmg, values);
811 }
812
816 void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
817
826 void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
827 {
828 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
829 if (jmg)
830 setJointGroupAccelerations(jmg, gstate);
831 }
832
835 void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
836 {
837 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
838 if (jmg)
839 setJointGroupAccelerations(jmg, &gstate[0]);
840 }
841
844 void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
845 {
846 setJointGroupAccelerations(group, &gstate[0]);
847 }
848
851 void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
852
855 void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
856 {
857 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
858 if (jmg)
859 setJointGroupAccelerations(jmg, values);
860 }
861
864 void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
865
869 void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
870 {
871 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
872 if (jmg)
873 {
874 gstate.resize(jmg->getVariableCount());
875 copyJointGroupAccelerations(jmg, &gstate[0]);
876 }
877 }
878
882 void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
883 {
884 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
885 if (jmg)
886 copyJointGroupAccelerations(jmg, gstate);
887 }
888
892 void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
893 {
894 gstate.resize(group->getVariableCount());
895 copyJointGroupAccelerations(group, &gstate[0]);
896 }
897
901 void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
902
906 void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
907 {
908 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
909 if (jmg)
910 copyJointGroupAccelerations(jmg, values);
911 }
912
916 void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
917
930 bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0,
934
942 bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip,
943 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
946
953 bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
957
964 bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
965 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
968
977 bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
978 const std::vector<double>& consistency_limits, double timeout = 0.0,
982
992 bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
993 const std::vector<std::string>& tips, double timeout = 0.0,
997
1008 bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1009 const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
1010 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1013
1022 bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1023 const std::vector<std::string>& tips,
1024 const std::vector<std::vector<double>>& consistency_limits, double timeout = 0.0,
1027
1035 bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1037
1045 bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::msg::Twist& twist, const std::string& tip,
1046 double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());
1047
1057 bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1058 Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1059
1069 bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1070 Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1071 {
1073 return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1074 use_quaternion_representation);
1075 }
1076
1083 Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1084 const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1085
1092 Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1093 const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1094 {
1096 return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1097 }
1098
1101 void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1102 const LinkModel* tip) const;
1103
1106 void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1107 const LinkModel* tip)
1108 {
1110 static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1111 }
1112
1116 bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1118
1125 void setVariableValues(const sensor_msgs::msg::JointState& msg)
1126 {
1127 if (!msg.position.empty())
1128 setVariablePositions(msg.name, msg.position);
1129 if (!msg.velocity.empty())
1130 setVariableVelocities(msg.name, msg.velocity);
1131 }
1132
1136 void setToDefaultValues();
1137
1139 bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1140
1141 bool setToDefaultValues(const std::string& group_name, const std::string& state_name)
1142 {
1143 const JointModelGroup* jmg = getJointModelGroup(group_name);
1144 if (jmg)
1145 {
1146 return setToDefaultValues(jmg, state_name);
1147 }
1148 else
1149 {
1150 return false;
1151 }
1152 }
1153
1155 void setToRandomPositions();
1156
1158 void setToRandomPositions(const JointModelGroup* group);
1159
1162 void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1163
1169 void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance);
1170
1175 void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance,
1176 random_numbers::RandomNumberGenerator& rng);
1177
1185 void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
1186 const std::vector<double>& distances);
1187
1193 void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
1194 const std::vector<double>& distances, random_numbers::RandomNumberGenerator& rng);
1195
1205
1208 void updateLinkTransforms();
1209
1211 void update(bool force = false);
1212
1221 void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1222 {
1223 updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1224 }
1225
1227 void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1228
1235 getRigidlyConnectedParentLinkModel(const std::string& frame, const moveit::core::JointModelGroup* jmg = nullptr) const;
1236
1247 const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1248 {
1249 return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1250 }
1251
1252 const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1253 {
1254 if (!link)
1255 {
1256 throw Exception("Invalid link");
1257 }
1259 return global_link_transforms_[link->getLinkIndex()];
1260 }
1261
1262 const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1263 {
1264 return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1265 }
1266
1267 const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1268 {
1269 if (!link)
1270 {
1271 throw Exception("Invalid link");
1272 }
1273 assert(checkLinkTransforms());
1274 return global_link_transforms_[link->getLinkIndex()];
1275 }
1276
1287 const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1288 {
1289 return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1290 }
1291
1292 const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1293 {
1295 return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1296 }
1297
1298 const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1299 {
1300 return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1301 }
1302
1303 const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1304 {
1305 assert(checkCollisionTransforms());
1306 return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1307 }
1308
1309 const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1310 {
1311 return getJointTransform(robot_model_->getJointModel(joint_name));
1312 }
1313
1314 const Eigen::Isometry3d& getJointTransform(const JointModel* joint);
1315
1316 const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1317 {
1318 return getJointTransform(robot_model_->getJointModel(joint_name));
1319 }
1320
1321 const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1322 {
1323 assert(checkJointTransforms(joint));
1324 return variable_joint_transforms_[joint->getJointIndex()];
1325 }
1326
1327 bool dirtyJointTransform(const JointModel* joint) const
1328 {
1329 return dirty_joint_transforms_[joint->getJointIndex()];
1330 }
1331
1333 {
1334 return dirty_link_transforms_;
1335 }
1336
1338 {
1339 return dirty_link_transforms_ || dirty_collision_body_transforms_;
1340 }
1341
1343 bool dirty() const
1344 {
1346 }
1347
1355 double distance(const RobotState& other) const
1356 {
1357 return robot_model_->distance(position_.data(), other.getVariablePositions());
1358 }
1359
1361 double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1362
1364 double distance(const RobotState& other, const JointModel* joint) const;
1365
1374 void interpolate(const RobotState& to, double t, RobotState& state) const;
1375
1385 void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1386
1396 void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const;
1397
1398 void enforceBounds();
1399 void enforceBounds(const JointModelGroup* joint_group);
1400 void enforceBounds(const JointModel* joint)
1401 {
1402 enforcePositionBounds(joint);
1403 if (has_velocity_)
1404 enforceVelocityBounds(joint);
1405 }
1406 void enforcePositionBounds(const JointModel* joint);
1407
1409 void harmonizePositions();
1410 void harmonizePositions(const JointModelGroup* joint_group);
1411 void harmonizePosition(const JointModel* joint);
1412
1413 void enforceVelocityBounds(const JointModel* joint);
1414
1415 bool satisfiesBounds(double margin = 0.0) const;
1416 bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1417 bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1418 {
1419 return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1420 }
1421 bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1422 {
1423 return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1424 }
1425 bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1426 {
1427 return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1428 }
1429
1432 std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1433
1436 std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1437
1440 std::pair<double, const JointModel*>
1441 getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1442
1449 bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1450
1467 void attachBody(std::unique_ptr<AttachedBody> attached_body);
1468
1486 void attachBody(const std::string& id, const Eigen::Isometry3d& pose,
1487 const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
1488 const std::set<std::string>& touch_links, const std::string& link_name,
1489 const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(),
1491
1509 void attachBody(const std::string& id, const Eigen::Isometry3d& pose,
1510 const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
1511 const std::vector<std::string>& touch_links, const std::string& link_name,
1512 const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(),
1514 {
1515 std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1516 attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses);
1517 }
1518
1520 void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1521
1523 void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;
1524
1526 void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) const;
1527
1530 bool clearAttachedBody(const std::string& id);
1531
1533 void clearAttachedBodies(const LinkModel* link);
1534
1536 void clearAttachedBodies(const JointModelGroup* group);
1537
1539 void clearAttachedBodies();
1540
1542 const AttachedBody* getAttachedBody(const std::string& name) const;
1543
1545 bool hasAttachedBody(const std::string& id) const;
1546
1552 void computeAABB(std::vector<double>& aabb) const;
1553
1556 void computeAABB(std::vector<double>& aabb)
1557 {
1559 static_cast<const RobotState*>(this)->computeAABB(aabb);
1560 }
1561
1563 random_numbers::RandomNumberGenerator& getRandomNumberGenerator()
1564 {
1565 if (!rng_)
1566 rng_ = std::make_unique<random_numbers::RandomNumberGenerator>();
1567 return *rng_;
1568 }
1569
1575 const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr);
1576
1582 const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const;
1583
1590 const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
1591 bool& frame_found) const;
1592
1594 bool knowsFrameTransform(const std::string& frame_id) const;
1595
1603 void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
1604 const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur,
1605 bool include_attached = false) const;
1606
1614 void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
1615 const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur,
1616 bool include_attached = false)
1617 {
1619 static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1620 }
1621
1626 void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
1627 bool include_attached = false) const;
1628
1633 void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
1634 bool include_attached = false)
1635 {
1637 static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1638 }
1639
1640 void printStatePositions(std::ostream& out = std::cout) const;
1641
1643 void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const;
1644
1645 void printStateInfo(std::ostream& out = std::cout) const;
1646
1647 void printTransforms(std::ostream& out = std::cout) const;
1648
1649 void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1650
1651 void printDirtyInfo(std::ostream& out = std::cout) const;
1652
1653 std::string getStateTreeString() const;
1654
1661 bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
1662
1669 bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
1670
1671private:
1672 void allocMemory();
1673 void init();
1674 void copyFrom(const RobotState& other);
1675
1676 void markDirtyJointTransforms(const JointModel* joint)
1677 {
1678 dirty_joint_transforms_[joint->getJointIndex()] = 1;
1679 dirty_link_transforms_ =
1680 dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1681 }
1682
1683 void markDirtyJointTransforms(const JointModelGroup* group)
1684 {
1685 for (const JointModel* jm : group->getActiveJointModels())
1686 dirty_joint_transforms_[jm->getJointIndex()] = 1;
1687 dirty_link_transforms_ = dirty_link_transforms_ == nullptr ?
1688 group->getCommonRoot() :
1689 robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1690 }
1691
1692 void markVelocity();
1693 void markAcceleration();
1694 void markEffort();
1695
1696 void updateMimicJoint(const JointModel* joint);
1697
1699 void updateMimicJoints(const JointModelGroup* group);
1700
1701 void updateLinkTransformsInternal(const JointModel* start);
1702
1703 void getMissingKeys(const std::map<std::string, double>& variable_map,
1704 std::vector<std::string>& missing_variables) const;
1705 void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1706
1708 bool checkJointTransforms(const JointModel* joint) const;
1709
1711 bool checkLinkTransforms() const;
1712
1714 bool checkCollisionTransforms() const;
1715
1721 const moveit::core::LinkModel* getLinkModelIncludingAttachedBodies(const std::string& frame) const;
1722
1723 RobotModelConstPtr robot_model_;
1724
1725 std::vector<double> position_;
1726 std::vector<double> velocity_;
1727 std::vector<double> effort_or_acceleration_;
1728 bool has_velocity_ = false;
1729 bool has_acceleration_ = false;
1730 bool has_effort_ = false;
1731
1732 const JointModel* dirty_link_transforms_ = nullptr;
1733 const JointModel* dirty_collision_body_transforms_ = nullptr;
1734
1735 // All the following transform variables point into aligned memory.
1736 // They are updated lazily, based on the flags in dirty_joint_transforms_
1737 // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_
1738 std::vector<Eigen::Isometry3d> variable_joint_transforms_;
1739 std::vector<Eigen::Isometry3d> global_link_transforms_;
1740 std::vector<Eigen::Isometry3d> global_collision_body_transforms_;
1742 std::vector<unsigned char> dirty_joint_transforms_;
1743
1745 std::map<std::string, std::unique_ptr<AttachedBody>> attached_body_map_;
1746
1749 AttachedBodyCallback attached_body_update_callback_;
1750
1755 std::unique_ptr<random_numbers::RandomNumberGenerator> rng_;
1756};
1757
1759std::ostream& operator<<(std::ostream& out, const RobotState& s);
1760} // namespace core
1761} // namespace moveit
#define MOVEIT_CLASS_FORWARD(C)
std::function< double(const geometry_msgs::msg::Pose &, const moveit::core::RobotState &, const moveit::core::JointModelGroup *, const std::vector< double > &)> IKCostFn
Signature for a cost function used to evaluate IK solutions.
This may be thrown if unrecoverable errors occur.
Object defining bodies that can be attached to robot links.
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
bool satisfiesVelocityBounds(const double *values, double margin=0.0) const
Check if the set of velocities for the variables of this joint are within bounds.
size_t getJointIndex() const
Get the index of this joint within the robot model.
bool satisfiesPositionBounds(const double *values, double margin=0.0) const
Check if the set of values for the variables of this joint are within bounds.
A link from the robot. Contains the constant transform applied to the link and its geometry.
int getFirstCollisionBodyTransformIndex() const
size_t getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setJointGroupActivePositions(const std::string &joint_group_name, const std::vector< double > &gstate)
Given positions for the variables of active joints that make up a group, in the order found in the gr...
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.
void zeroVelocities()
Set all velocities to 0.0.
RobotState & operator=(const RobotState &other)
Copy operator.
double getVariableVelocity(int index) const
Get the velocity of a particular variable. The variable is specified by its index....
void setVariableAcceleration(int index, double value)
Set the acceleration of a single variable. The variable is specified by its index (a value associated...
void copyJointGroupVelocities(const std::string &joint_group_name, Eigen::VectorXd &values) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link) const
void setJointEfforts(const JointModel *joint, const double *effort)
void attachBody(const std::string &id, const Eigen::Isometry3d &pose, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &shape_poses, const std::vector< std::string > &touch_links, const std::string &link_name, const trajectory_msgs::msg::JointTrajectory &detach_posture=trajectory_msgs::msg::JointTrajectory(), const moveit::core::FixedTransformsMap &subframe_poses=moveit::core::FixedTransformsMap())
Add an attached body to a link.
void enforceBounds(const JointModel *joint)
double getVariableEffort(const std::string &variable) const
Get the effort of a particular variable. An exception is thrown if the variable is not known.
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory.
void setVariableAccelerations(const std::vector< double > &acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
void setJointGroupAccelerations(const std::string &joint_group_name, const std::vector< double > &gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
void setJointVelocities(const JointModel *joint, const double *velocity)
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
double getVariableAcceleration(int index) const
Get the acceleration of a particular variable. The variable is specified by its index....
void printTransforms(std::ostream &out=std::cout) const
void setVariableEffort(const std::string &variable, double value)
Set the effort of a variable. If an unknown variable name is specified, an exception is thrown.
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
void setJointGroupAccelerations(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
void copyJointGroupPositions(const std::string &joint_group_name, double *gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
void copyJointGroupAccelerations(const std::string &joint_group_name, double *gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
void copyJointGroupVelocities(const std::string &joint_group_name, double *gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
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.
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false)
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
const Eigen::Isometry3d & getJointTransform(const JointModel *joint) const
double getVariablePosition(int index) const
Get the position of a particular variable. The variable is specified by its index....
std::pair< double, const JointModel * > getMinDistanceToPositionBounds() const
Get the minimm distance from this state to the bounds. The minimum distance and the joint for which t...
void copyJointGroupVelocities(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
void setVariablePosition(int index, double value)
Set the position of a single variable. The variable is specified by its index (a value associated by ...
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
const double * getJointPositions(const std::string &joint_name) const
void getRobotMarkers(visualization_msgs::msg::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::msg::ColorRGBA &color, const std::string &ns, const rclcpp::Duration &dur, bool include_attached=false)
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first.
void setVariableEffort(int index, double value)
Set the effort of a single variable. The variable is specified by its index (a value associated by th...
void setVariableEffort(const std::vector< double > &effort)
Given an array with effort values for all variables, set those values as the effort in this state.
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
void copyJointGroupVelocities(const JointModelGroup *group, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
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 getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name) const
void setJointGroupAccelerations(const std::string &joint_group_name, const double *gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
Eigen::MatrixXd getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position=Eigen::Vector3d(0.0, 0.0, 0.0))
Compute the Jacobian with reference to the last link of a specified group, and origin at the group ro...
void setJointGroupVelocities(const std::string &joint_group_name, const double *gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip) const
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
void getRobotMarkers(visualization_msgs::msg::MarkerArray &arr, const std::vector< std::string > &link_names, bool include_attached=false)
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first.
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
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 computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx,...
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...
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)....
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
void setJointGroupVelocities(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given velocities for the variables that make up a group, in the order found in the group (including v...
bool dirtyCollisionBodyTransforms() const
const double * getJointEffort(const std::string &joint_name) const
void zeroAccelerations()
Set all accelerations to 0.0.
bool isValidVelocityMove(const RobotState &other, const JointModelGroup *group, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits and time step.
const double * getJointVelocities(const std::string &joint_name) const
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
bool setFromDiffIK(const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Set the joint values from a Cartesian velocity applied during a time dt.
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 LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
void setVariableVelocity(int index, double value)
Set the velocity of a single variable. The variable is specified by its index (a value associated by ...
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solv...
void dropDynamics()
Reduce RobotState to kinematic information (remove velocity, acceleration and effort,...
bool dirtyJointTransform(const JointModel *joint) const
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &seed, double distance)
Set all joints in group to random values near the value in seed. distance is the maximum amount each ...
bool dirty() const
Returns true if anything in this state is dirty.
bool hasEffort() const
By default, if effort is never set or initialized, the state remembers that there is no effort set....
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
void setJointGroupActivePositions(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given positions for the variables of active joints that make up a group, in the order found in the gr...
void copyJointGroupPositions(const std::string &joint_group_name, Eigen::VectorXd &values) const
For a given group, copy the position values of the variables that make up the group into another loca...
const double * getVariableAccelerations() const
Get const raw access to the accelerations of the variables that make up this state....
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 setJointGroupAccelerations(const JointModelGroup *group, const std::vector< double > &gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
std::string getStateTreeString() const
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index)
void copyJointGroupAccelerations(const JointModelGroup *group, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void setVariablePositions(const std::vector< double > &position)
It is assumed positions is an array containing the new positions for all variables in this state....
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
bool satisfiesBounds(double margin=0.0) const
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state.
void setVariableVelocities(const std::vector< double > &velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
const double * getVariableVelocities() const
Get const access to the velocities of the variables that make up this state. The values are in the sa...
void setJointGroupVelocities(const JointModelGroup *group, const std::vector< double > &gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
void setJointGroupActivePositions(const JointModelGroup *group, const std::vector< double > &gstate)
Given positions for the variables of active joints that make up a group, in the order found in the gr...
void printStateInfo(std::ostream &out=std::cout) 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 update(bool force=false)
Update all transforms.
void setJointGroupPositions(const std::string &joint_group_name, const std::vector< double > &gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
void dropAccelerations()
Remove accelerations from this state (this differs from setting them to zero)
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link)
void getRobotMarkers(visualization_msgs::msg::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::msg::ColorRGBA &color, const std::string &ns, const rclcpp::Duration &dur, bool include_attached=false) const
Get a MarkerArray that fully describes the robot markers for a given robot.
void setJointGroupVelocities(const std::string &joint_group_name, const std::vector< double > &gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
void setJointGroupPositions(const JointModelGroup *group, const std::vector< double > &gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return nullptr if not found.
bool setToIKSolverFrame(Eigen::Isometry3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
Transform pose from the robot model's base frame to the reference frame of the IK solver.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void printStatePositions(std::ostream &out=std::cout) const
std::size_t getVariableCount() const
Get the number of variables that make up this state.
void printDirtyInfo(std::ostream &out=std::cout) const
void copyJointGroupPositions(const JointModelGroup *group, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
const double * getJointAccelerations(const std::string &joint_name) const
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
void copyJointGroupAccelerations(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
void dropEffort()
Remove effort values from this state (this differs from setting them to zero)
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Given the velocities for the variables in this group (qdot) and an amount of time (dt),...
bool setToDefaultValues(const std::string &group_name, const std::string &state_name)
double getVariableEffort(int index) const
Get the effort of a particular variable. The variable is specified by its index. No checks are perfor...
double * getVariableVelocities()
Get raw access to the velocities of the variables that make up this state. The values are in the same...
void computeAABB(std::vector< double > &aabb)
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx,...
void setVariableValues(const sensor_msgs::msg::JointState &msg)
void enforcePositionBounds(const JointModel *joint)
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in 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...
void setJointPositions(const std::string &joint_name, const Eigen::Isometry3d &transform)
void copyJointGroupAccelerations(const std::string &joint_group_name, Eigen::VectorXd &values) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
void enforceVelocityBounds(const JointModel *joint)
void setJointGroupPositions(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given positions for the variables that make up a group, in the order found in the group (including va...
double * getVariableEffort()
Get raw access to the effort of the variables that make up this state. The values are in the same ord...
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name) const
void invertVelocity()
Invert velocity if present.
const Eigen::Isometry3d & getFrameInfo(const std::string &frame_id, const LinkModel *&robot_link, bool &frame_found) const
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
void harmonizePosition(const JointModel *joint)
void clearAttachedBodies()
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
void dropVelocities()
Remove velocities from this state (this differs from setting them to zero)
void zeroEffort()
Set all effort values to 0.0.
bool satisfiesBounds(const JointModel *joint, double margin=0.0) const
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
bool setFromIK(const JointModelGroup *group, const geometry_msgs::msg::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions(), const kinematics::KinematicsBase::IKCostFn &cost_function=kinematics::KinematicsBase::IKCostFn())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
double * getVariableAccelerations()
Get raw access to the accelerations of the variables that make up this state. The values are in the s...
const double * getVariableEffort() const
Get const raw access to the effort of the variables that make up this state. The values are in the sa...
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip)
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame, const moveit::core::JointModelGroup *jmg=nullptr) 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::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
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...
std::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Main namespace for MoveIt.
A set of options for the kinematics solver.