robot_state.h
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 #ifndef MOVEIT_CORE_ROBOT_STATE_
39 #define MOVEIT_CORE_ROBOT_STATE_
40 
44 #include <sensor_msgs/JointState.h>
45 #include <visualization_msgs/MarkerArray.h>
46 #include <std_msgs/ColorRGBA.h>
47 #include <geometry_msgs/Twist.h>
48 #include <cassert>
49 
50 #include <boost/assert.hpp>
51 
52 namespace moveit
53 {
54 namespace core
55 {
56 MOVEIT_CLASS_FORWARD(RobotState);
57 
62 typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
63  const double* joint_group_variable_values)>
65 
72 {
73  double factor;
74  double revolute; // Radians
75  double prismatic; // Meters
76 
77  explicit JumpThreshold() : factor(0.0), revolute(0.0), prismatic(0.0)
78  {
79  }
80 
81  explicit JumpThreshold(double jt_factor) : JumpThreshold()
82  {
83  factor = jt_factor;
84  }
85 
86  explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold()
87  {
88  revolute = jt_revolute; // Radians
89  prismatic = jt_prismatic; // Meters
90  }
91 };
92 
96 struct MaxEEFStep
97 {
98  MaxEEFStep(double translation = 0.0, double rotation = 0.0) : translation(translation), rotation(rotation)
99  {
100  }
101 
102  double translation; // Meters
103  double rotation; // Radians
104 };
105 
124 {
125 public:
128  RobotState(const RobotModelConstPtr& robot_model);
129  ~RobotState();
130 
132  RobotState(const RobotState& other);
133 
135  RobotState& operator=(const RobotState& other);
136 
138  const RobotModelConstPtr& getRobotModel() const
139  {
140  return robot_model_;
141  }
142 
144  std::size_t getVariableCount() const
145  {
146  return robot_model_->getVariableCount();
147  }
148 
150  const std::vector<std::string>& getVariableNames() const
151  {
152  return robot_model_->getVariableNames();
153  }
154 
156  const LinkModel* getLinkModel(const std::string& link) const
157  {
158  return robot_model_->getLinkModel(link);
159  }
160 
162  const JointModel* getJointModel(const std::string& joint) const
163  {
164  return robot_model_->getJointModel(joint);
165  }
166 
168  const JointModelGroup* getJointModelGroup(const std::string& group) const
169  {
170  return robot_model_->getJointModelGroup(group);
171  }
172 
182  {
183  return position_;
184  }
185 
188  const double* getVariablePositions() const
189  {
190  return position_;
191  }
192 
196  void setVariablePositions(const double* position);
197 
201  void setVariablePositions(const std::vector<double>& position)
202  {
203  assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
204  setVariablePositions(&position[0]);
205  }
206 
209  void setVariablePositions(const std::map<std::string, double>& variable_map);
210 
213  void setVariablePositions(const std::map<std::string, double>& variable_map,
214  std::vector<std::string>& missing_variables);
215 
218  void setVariablePositions(const std::vector<std::string>& variable_names,
219  const std::vector<double>& variable_position);
220 
222  void setVariablePosition(const std::string& variable, double value)
223  {
224  setVariablePosition(robot_model_->getVariableIndex(variable), value);
225  }
226 
229  void setVariablePosition(int index, double value)
230  {
231  position_[index] = value;
232  const JointModel* jm = robot_model_->getJointOfVariable(index);
233  if (jm)
234  {
235  markDirtyJointTransforms(jm);
236  updateMimicJoint(jm);
237  }
238  }
239 
241  double getVariablePosition(const std::string& variable) const
242  {
243  return position_[robot_model_->getVariableIndex(variable)];
244  }
245 
249  double getVariablePosition(int index) const
250  {
251  return position_[index];
252  }
253 
263  bool hasVelocities() const
264  {
265  return has_velocity_;
266  }
267 
271  {
272  markVelocity();
273  return velocity_;
274  }
275 
278  const double* getVariableVelocities() const
279  {
280  return velocity_;
281  }
282 
284  void setVariableVelocities(const double* velocity)
285  {
286  has_velocity_ = true;
287  // assume everything is in order in terms of array lengths (for efficiency reasons)
288  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
289  }
290 
292  void setVariableVelocities(const std::vector<double>& velocity)
293  {
294  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
295  setVariableVelocities(&velocity[0]);
296  }
297 
300  void setVariableVelocities(const std::map<std::string, double>& variable_map);
301 
304  void setVariableVelocities(const std::map<std::string, double>& variable_map,
305  std::vector<std::string>& missing_variables);
306 
309  void setVariableVelocities(const std::vector<std::string>& variable_names,
310  const std::vector<double>& variable_velocity);
311 
313  void setVariableVelocity(const std::string& variable, double value)
314  {
315  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
316  }
317 
320  void setVariableVelocity(int index, double value)
321  {
322  markVelocity();
323  velocity_[index] = value;
324  }
325 
327  double getVariableVelocity(const std::string& variable) const
328  {
329  return velocity_[robot_model_->getVariableIndex(variable)];
330  }
331 
335  double getVariableVelocity(int index) const
336  {
337  return velocity_[index];
338  }
339 
350  bool hasAccelerations() const
351  {
352  return has_acceleration_;
353  }
354 
359  {
360  markAcceleration();
361  return acceleration_;
362  }
363 
366  const double* getVariableAccelerations() const
367  {
368  return acceleration_;
369  }
370 
373  void setVariableAccelerations(const double* acceleration)
374  {
375  has_acceleration_ = true;
376  has_effort_ = false;
377 
378  // assume everything is in order in terms of array lengths (for efficiency reasons)
379  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
380  }
381 
384  void setVariableAccelerations(const std::vector<double>& acceleration)
385  {
386  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
387  setVariableAccelerations(&acceleration[0]);
388  }
389 
392  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
393 
397  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
398  std::vector<std::string>& missing_variables);
399 
402  void setVariableAccelerations(const std::vector<std::string>& variable_names,
403  const std::vector<double>& variable_acceleration);
404 
406  void setVariableAcceleration(const std::string& variable, double value)
407  {
408  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
409  }
410 
413  void setVariableAcceleration(int index, double value)
414  {
415  markAcceleration();
416  acceleration_[index] = value;
417  }
418 
420  double getVariableAcceleration(const std::string& variable) const
421  {
422  return acceleration_[robot_model_->getVariableIndex(variable)];
423  }
424 
428  double getVariableAcceleration(int index) const
429  {
430  return acceleration_[index];
431  }
432 
442  bool hasEffort() const
443  {
444  return has_effort_;
445  }
446 
451  {
452  markEffort();
453  return effort_;
454  }
455 
458  const double* getVariableEffort() const
459  {
460  return effort_;
461  }
462 
464  void setVariableEffort(const double* effort)
465  {
466  has_effort_ = true;
467  has_acceleration_ = false;
468  // assume everything is in order in terms of array lengths (for efficiency reasons)
469  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
470  }
471 
473  void setVariableEffort(const std::vector<double>& effort)
474  {
475  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
476  setVariableEffort(&effort[0]);
477  }
478 
480  void setVariableEffort(const std::map<std::string, double>& variable_map);
481 
484  void setVariableEffort(const std::map<std::string, double>& variable_map,
485  std::vector<std::string>& missing_variables);
486 
488  void setVariableEffort(const std::vector<std::string>& variable_names,
489  const std::vector<double>& variable_acceleration);
490 
492  void setVariableEffort(const std::string& variable, double value)
493  {
494  setVariableEffort(robot_model_->getVariableIndex(variable), value);
495  }
496 
499  void setVariableEffort(int index, double value)
500  {
501  markEffort();
502  effort_[index] = value;
503  }
504 
506  double getVariableEffort(const std::string& variable) const
507  {
508  return effort_[robot_model_->getVariableIndex(variable)];
509  }
510 
514  double getVariableEffort(int index) const
515  {
516  return effort_[index];
517  }
518 
520  void invertVelocity();
521 
527  void setJointPositions(const std::string& joint_name, const double* position)
528  {
529  setJointPositions(robot_model_->getJointModel(joint_name), position);
530  }
531 
532  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
533  {
534  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
535  }
536 
537  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
538  {
539  setJointPositions(joint, &position[0]);
540  }
541 
542  void setJointPositions(const JointModel* joint, const double* position)
543  {
544  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
545  markDirtyJointTransforms(joint);
546  updateMimicJoint(joint);
547  }
548 
549  void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
550  {
551  setJointPositions(robot_model_->getJointModel(joint_name), transform);
552  }
553 
554  void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform)
555  {
556  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
557  markDirtyJointTransforms(joint);
558  updateMimicJoint(joint);
559  }
560 
561  void setJointVelocities(const JointModel* joint, const double* velocity)
562  {
563  has_velocity_ = true;
564  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
565  }
566 
567  void setJointEfforts(const JointModel* joint, const double* effort);
568 
569  const double* getJointPositions(const std::string& joint_name) const
570  {
571  return getJointPositions(robot_model_->getJointModel(joint_name));
572  }
573 
574  const double* getJointPositions(const JointModel* joint) const
575  {
576  return position_ + joint->getFirstVariableIndex();
577  }
578 
579  const double* getJointVelocities(const std::string& joint_name) const
580  {
581  return getJointVelocities(robot_model_->getJointModel(joint_name));
582  }
583 
584  const double* getJointVelocities(const JointModel* joint) const
585  {
586  return velocity_ + joint->getFirstVariableIndex();
587  }
588 
589  const double* getJointAccelerations(const std::string& joint_name) const
590  {
591  return getJointAccelerations(robot_model_->getJointModel(joint_name));
592  }
593 
594  const double* getJointAccelerations(const JointModel* joint) const
595  {
596  return acceleration_ + joint->getFirstVariableIndex();
597  }
598 
599  const double* getJointEffort(const std::string& joint_name) const
600  {
601  return getJointEffort(robot_model_->getJointModel(joint_name));
602  }
603 
604  const double* getJointEffort(const JointModel* joint) const
605  {
606  return effort_ + joint->getFirstVariableIndex();
607  }
608 
618  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
619  {
620  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
621  if (jmg)
622  setJointGroupPositions(jmg, gstate);
623  }
624 
628  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
629  {
630  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
631  if (jmg)
632  setJointGroupPositions(jmg, &gstate[0]);
633  }
634 
638  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
639  {
640  setJointGroupPositions(group, &gstate[0]);
641  }
642 
646  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
647 
651  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
652  {
653  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
654  if (jmg)
655  setJointGroupPositions(jmg, values);
656  }
657 
661  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
662 
666  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
667  {
668  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
669  if (jmg)
670  {
671  gstate.resize(jmg->getVariableCount());
672  copyJointGroupPositions(jmg, &gstate[0]);
673  }
674  }
675 
679  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
680  {
681  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
682  if (jmg)
683  copyJointGroupPositions(jmg, gstate);
684  }
685 
689  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
690  {
691  gstate.resize(group->getVariableCount());
692  copyJointGroupPositions(group, &gstate[0]);
693  }
694 
698  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
699 
703  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
704  {
705  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
706  if (jmg)
707  copyJointGroupPositions(jmg, values);
708  }
709 
713  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
714 
724  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
725  {
726  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
727  if (jmg)
728  setJointGroupVelocities(jmg, gstate);
729  }
730 
734  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
735  {
736  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
737  if (jmg)
738  setJointGroupVelocities(jmg, &gstate[0]);
739  }
740 
744  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
745  {
746  setJointGroupVelocities(group, &gstate[0]);
747  }
748 
752  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
753 
757  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
758  {
759  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
760  if (jmg)
761  setJointGroupVelocities(jmg, values);
762  }
763 
767  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
768 
772  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
773  {
774  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
775  if (jmg)
776  {
777  gstate.resize(jmg->getVariableCount());
778  copyJointGroupVelocities(jmg, &gstate[0]);
779  }
780  }
781 
785  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
786  {
787  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
788  if (jmg)
789  copyJointGroupVelocities(jmg, gstate);
790  }
791 
795  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
796  {
797  gstate.resize(group->getVariableCount());
798  copyJointGroupVelocities(group, &gstate[0]);
799  }
800 
804  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
805 
809  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
810  {
811  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
812  if (jmg)
813  copyJointGroupVelocities(jmg, values);
814  }
815 
819  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
820 
830  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
831  {
832  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
833  if (jmg)
834  setJointGroupAccelerations(jmg, gstate);
835  }
836 
840  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
841  {
842  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
843  if (jmg)
844  setJointGroupAccelerations(jmg, &gstate[0]);
845  }
846 
850  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
851  {
852  setJointGroupAccelerations(group, &gstate[0]);
853  }
854 
858  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
859 
863  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
864  {
865  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
866  if (jmg)
867  setJointGroupAccelerations(jmg, values);
868  }
869 
873  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
874 
878  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
879  {
880  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
881  if (jmg)
882  {
883  gstate.resize(jmg->getVariableCount());
884  copyJointGroupAccelerations(jmg, &gstate[0]);
885  }
886  }
887 
891  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
892  {
893  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
894  if (jmg)
895  copyJointGroupAccelerations(jmg, gstate);
896  }
897 
901  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
902  {
903  gstate.resize(group->getVariableCount());
904  copyJointGroupAccelerations(group, &gstate[0]);
905  }
906 
910  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
911 
915  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
916  {
917  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
918  if (jmg)
919  copyJointGroupAccelerations(jmg, values);
920  }
921 
925  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
926 
939  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
940 
947  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
948 
955  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
958  [[deprecated("The attempts argument is not supported anymore.")]] bool
959  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts, double timeout = 0.0,
962  {
963  return setFromIK(group, pose, timeout, constraint, options);
964  }
965 
973  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
974  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
976  [[deprecated("The attempts argument is not supported anymore.")]] bool
977  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
978  unsigned int attempts, double timeout = 0.0,
981  {
982  return setFromIK(group, pose, tip, timeout, constraint, options);
983  }
984 
991  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
994  [[deprecated("The attempts argument is not supported anymore.")]] bool
995  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts, double timeout = 0.0,
998  {
999  return setFromIK(group, pose, timeout, constraint, options);
1000  }
1001 
1008  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1009  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1011  [[deprecated("The attempts argument is not supported anymore.")]] bool
1012  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, unsigned int attempts,
1013  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1015  {
1016  return setFromIK(group, pose, tip, timeout, constraint, options);
1017  }
1018 
1027  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1028  const std::vector<double>& consistency_limits, double timeout = 0.0,
1031  [[deprecated("The attempts argument is not supported anymore.")]] bool
1032  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1033  const std::vector<double>& consistency_limits, unsigned int attempts, double timeout = 0.0,
1036  {
1037  return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options);
1038  }
1039 
1049  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1050  const std::vector<std::string>& tips, double timeout = 0.0,
1053  [[deprecated("The attempts argument is not supported anymore.")]] bool
1055  const std::vector<std::string>& tips, unsigned int attempts, double timeout = 0.0,
1058  {
1059  return setFromIK(group, poses, tips, timeout, constraint, options);
1060  }
1061 
1072  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1073  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1074  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1076  [[deprecated("The attempts argument is not supported anymore.")]] bool
1078  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1079  unsigned int attempts, double timeout = 0.0,
1082  {
1083  return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options);
1084  }
1085 
1094  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1095  const std::vector<std::string>& tips,
1096  const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
1099  [[deprecated("The attempts argument is not supported anymore.")]] bool
1101  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1102  unsigned int attempts, double timeout = 0.0,
1105  {
1106  return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options);
1107  }
1108 
1116  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1118 
1126  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1128 
1159  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1160  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1161  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1164 
1165  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1166  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1167  double max_step, double jump_threshold_factor,
1170  {
1171  return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance,
1172  MaxEEFStep(max_step, max_step), JumpThreshold(jump_threshold_factor), validCallback,
1173  options);
1174  }
1175 
1182  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1183  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
1184  const JumpThreshold& jump_threshold,
1187 
1188  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1189  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1190  double jump_threshold_factor,
1193  {
1194  return computeCartesianPath(group, traj, link, target, global_reference_frame, MaxEEFStep(max_step),
1195  JumpThreshold(jump_threshold_factor), validCallback, options);
1196  }
1197 
1204  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1205  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1206  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1209 
1210  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1211  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1212  double max_step, double jump_threshold_factor,
1215  {
1216  return computeCartesianPath(group, traj, link, waypoints, global_reference_frame, MaxEEFStep(max_step),
1217  JumpThreshold(jump_threshold_factor), validCallback, options);
1218  }
1219 
1235  static double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1236  const JumpThreshold& jump_threshold);
1237 
1248  static double testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1249  double jump_threshold_factor);
1250 
1263  static double testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1264  double revolute_jump_threshold, double prismatic_jump_threshold);
1265 
1275  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1276  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1277 
1287  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1288  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1289  {
1290  updateLinkTransforms();
1291  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1292  use_quaternion_representation);
1293  }
1294 
1301  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1302  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1303 
1310  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1311  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1312  {
1313  updateLinkTransforms();
1314  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1315  }
1316 
1319  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1320  const LinkModel* tip) const;
1321 
1324  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1325  const LinkModel* tip)
1326  {
1327  updateLinkTransforms();
1328  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1329  }
1330 
1334  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1336 
1343  void setVariableValues(const sensor_msgs::JointState& msg)
1344  {
1345  if (!msg.position.empty())
1346  setVariablePositions(msg.name, msg.position);
1347  if (!msg.velocity.empty())
1348  setVariableVelocities(msg.name, msg.velocity);
1349  }
1350 
1354  void setToDefaultValues();
1355 
1357  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1358 
1360  void setToRandomPositions();
1361 
1363  void setToRandomPositions(const JointModelGroup* group);
1364 
1367  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1368 
1374  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1375 
1383  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1384  const std::vector<double>& distances);
1385 
1394  void updateCollisionBodyTransforms();
1395 
1398  void updateLinkTransforms();
1399 
1401  void update(bool force = false);
1402 
1411  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1412  {
1413  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1414  }
1415 
1417  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1418 
1419  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1420  {
1421  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1422  }
1423 
1424  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1425  {
1426  updateLinkTransforms();
1427  return global_link_transforms_[link->getLinkIndex()];
1428  }
1429 
1430  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1431  {
1432  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1433  }
1434 
1435  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1436  {
1437  updateCollisionBodyTransforms();
1438  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1439  }
1440 
1441  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1442  {
1443  return getJointTransform(robot_model_->getJointModel(joint_name));
1444  }
1445 
1446  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1447  {
1448  const int idx = joint->getJointIndex();
1449  unsigned char& dirty = dirty_joint_transforms_[idx];
1450  if (dirty)
1451  {
1452  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1453  dirty = 0;
1454  }
1455  return variable_joint_transforms_[idx];
1456  }
1457 
1458  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1459  {
1460  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1461  }
1462 
1463  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1464  {
1465  BOOST_VERIFY(checkLinkTransforms());
1466  return global_link_transforms_[link->getLinkIndex()];
1467  }
1468 
1469  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1470  {
1471  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1472  }
1473 
1474  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1475  {
1476  BOOST_VERIFY(checkCollisionTransforms());
1477  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1478  }
1479 
1480  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1481  {
1482  return getJointTransform(robot_model_->getJointModel(joint_name));
1483  }
1484 
1485  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1486  {
1487  BOOST_VERIFY(checkJointTransforms(joint));
1488  return variable_joint_transforms_[joint->getJointIndex()];
1489  }
1490 
1491  bool dirtyJointTransform(const JointModel* joint) const
1492  {
1493  return dirty_joint_transforms_[joint->getJointIndex()];
1494  }
1495 
1496  bool dirtyLinkTransforms() const
1497  {
1498  return dirty_link_transforms_;
1499  }
1500 
1502  {
1503  return dirty_link_transforms_ || dirty_collision_body_transforms_;
1504  }
1505 
1507  bool dirty() const
1508  {
1509  return dirtyCollisionBodyTransforms();
1510  }
1511 
1518  double distance(const RobotState& other) const
1519  {
1520  return robot_model_->distance(position_, other.getVariablePositions());
1521  }
1522 
1523  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1524 
1525  double distance(const RobotState& other, const JointModel* joint) const
1526  {
1527  const int idx = joint->getFirstVariableIndex();
1528  return joint->distance(position_ + idx, other.position_ + idx);
1529  }
1530 
1534  void interpolate(const RobotState& to, double t, RobotState& state) const;
1535 
1539  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1540 
1544  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1545  {
1546  const int idx = joint->getFirstVariableIndex();
1547  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1548  state.markDirtyJointTransforms(joint);
1549  state.updateMimicJoint(joint);
1550  }
1551 
1552  void enforceBounds();
1553  void enforceBounds(const JointModelGroup* joint_group);
1554  void enforceBounds(const JointModel* joint)
1555  {
1556  enforcePositionBounds(joint);
1557  if (has_velocity_)
1558  enforceVelocityBounds(joint);
1559  }
1561  {
1562  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1563  {
1564  markDirtyJointTransforms(joint);
1565  updateMimicJoint(joint);
1566  }
1567  }
1568 
1570  void harmonizePositions();
1571  void harmonizePositions(const JointModelGroup* joint_group);
1572  void harmonizePosition(const JointModel* joint)
1573  {
1574  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1575  // no need to mark transforms dirty, as the transform hasn't changed
1576  updateMimicJoint(joint);
1577  }
1578 
1580  {
1581  joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
1582  }
1583 
1584  bool satisfiesBounds(double margin = 0.0) const;
1585  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1586  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1587  {
1588  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1589  }
1590  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1591  {
1592  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1593  }
1594  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1595  {
1596  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1597  }
1598 
1601  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1602 
1605  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1606 
1609  std::pair<double, const JointModel*>
1610  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1611 
1618  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1619 
1645  void attachBody(AttachedBody* attached_body);
1646 
1661  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1662  const EigenSTL::vector_Isometry3d& attach_trans, const std::set<std::string>& touch_links,
1663  const std::string& link_name,
1664  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1665 
1680  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1681  const EigenSTL::vector_Isometry3d& attach_trans, const std::vector<std::string>& touch_links,
1682  const std::string& link_name,
1683  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1684  {
1685  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1686  attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1687  }
1688 
1690  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1691 
1693  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
1694 
1696  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
1697 
1700  bool clearAttachedBody(const std::string& id);
1701 
1703  void clearAttachedBodies(const LinkModel* link);
1704 
1706  void clearAttachedBodies(const JointModelGroup* group);
1707 
1709  void clearAttachedBodies();
1710 
1712  const AttachedBody* getAttachedBody(const std::string& name) const;
1713 
1715  bool hasAttachedBody(const std::string& id) const;
1716 
1717  void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback);
1722  void computeAABB(std::vector<double>& aabb) const;
1723 
1726  void computeAABB(std::vector<double>& aabb)
1727  {
1728  updateLinkTransforms();
1729  static_cast<const RobotState*>(this)->computeAABB(aabb);
1730  }
1731 
1734  {
1735  if (!rng_)
1737  return *rng_;
1738  }
1739 
1741  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
1742 
1744  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
1745 
1747  bool knowsFrameTransform(const std::string& id) const;
1748 
1756  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1757  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1758  bool include_attached = false) const;
1759 
1767  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1768  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1769  bool include_attached = false)
1770  {
1771  updateCollisionBodyTransforms();
1772  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1773  }
1774 
1779  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1780  bool include_attached = false) const;
1781 
1786  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1787  bool include_attached = false)
1788  {
1789  updateCollisionBodyTransforms();
1790  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1791  }
1792 
1793  void printStatePositions(std::ostream& out = std::cout) const;
1794 
1796  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg,
1797  std::ostream& out = std::cout) const;
1798 
1799  void printStateInfo(std::ostream& out = std::cout) const;
1800 
1801  void printTransforms(std::ostream& out = std::cout) const;
1802 
1803  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1804 
1805  void printDirtyInfo(std::ostream& out = std::cout) const;
1806 
1807  std::string getStateTreeString(const std::string& prefix = "") const;
1808 
1809 private:
1810  void allocMemory();
1811  void initTransforms();
1812  void copyFrom(const RobotState& other);
1813 
1815  {
1816  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1817  dirty_link_transforms_ =
1818  dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1819  }
1820 
1822  {
1823  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
1824  for (std::size_t i = 0; i < jm.size(); ++i)
1825  dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1826  dirty_link_transforms_ = dirty_link_transforms_ == NULL ?
1827  group->getCommonRoot() :
1828  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1829  }
1830 
1831  void markVelocity();
1832  void markAcceleration();
1833  void markEffort();
1834 
1835  void updateMimicJoint(const JointModel* joint)
1836  {
1837  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
1838  double v = position_[joint->getFirstVariableIndex()];
1839  for (std::size_t i = 0; i < mim.size(); ++i)
1840  {
1841  position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1842  markDirtyJointTransforms(mim[i]);
1843  }
1844  }
1845 
1847  /* use updateMimicJoints() instead, which also marks joints dirty */
1848  MOVEIT_DEPRECATED void updateMimicJoint(const std::vector<const JointModel*>& mim)
1849  {
1850  for (std::size_t i = 0; i < mim.size(); ++i)
1851  {
1852  const int fvi = mim[i]->getFirstVariableIndex();
1853  position_[fvi] =
1854  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1855  // Only mark joint transform dirty, but not the associated link transform
1856  // as this function is always used in combination of
1857  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1858  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1859  }
1860  }
1861 
1864  {
1865  for (const JointModel* jm : group->getMimicJointModels())
1866  {
1867  const int fvi = jm->getFirstVariableIndex();
1868  position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
1869  markDirtyJointTransforms(jm);
1870  }
1871  markDirtyJointTransforms(group);
1872  }
1873 
1874  void updateLinkTransformsInternal(const JointModel* start);
1875 
1876  void getMissingKeys(const std::map<std::string, double>& variable_map,
1877  std::vector<std::string>& missing_variables) const;
1878  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1879 
1881  bool checkJointTransforms(const JointModel* joint) const;
1882 
1884  bool checkLinkTransforms() const;
1885 
1887  bool checkCollisionTransforms() const;
1888 
1889  RobotModelConstPtr robot_model_;
1890  void* memory_;
1891 
1892  double* position_;
1893  double* velocity_;
1894  double* acceleration_;
1895  double* effort_;
1899 
1902 
1903  Eigen::Isometry3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned
1904  Eigen::Isometry3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned
1905  Eigen::Isometry3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned
1906  unsigned char* dirty_joint_transforms_;
1907 
1909  std::map<std::string, AttachedBody*> attached_body_map_;
1910 
1914 
1921 };
1922 
1924 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1925 }
1926 }
1927 
1928 #endif
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:527
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Definition: robot_state.h:1430
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:327
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1733
JumpThreshold(double jt_factor)
Definition: robot_state.h:81
A set of options for the kinematics solver.
Eigen::Isometry3d * variable_joint_transforms_
Definition: robot_state.h:1903
void computeAABB(std::vector< double > &aabb)
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
Definition: robot_state.h:1726
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1835
const double * getJointEffort(const JointModel *joint) const
Definition: robot_state.h:604
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1100
Core components of MoveIt!
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...
Definition: robot_state.h:795
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 ...
Definition: robot_state.h:891
void getRobotMarkers(visualization_msgs::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...
Definition: robot_state.h:1786
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...
Definition: robot_state.h:628
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...
Definition: robot_state.h:830
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:241
const double * getVariableVelocities() const
Get const access to the velocities of the variables that make up this state. The values are in the sa...
Definition: robot_state.h:278
ROSCPP_DECL void start()
void harmonizePosition(const JointModel *joint)
Definition: robot_state.h:1572
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:579
void attachBody(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &attach_trans, const std::vector< std::string > &touch_links, const std::string &link_name, const trajectory_msgs::JointTrajectory &detach_posture=trajectory_msgs::JointTrajectory())
Add an attached body to a link.
Definition: robot_state.h:1680
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
const std::vector< const JointModel * > & getMimicRequests() const
The joint models whose values would be modified if the value of this joint changed.
Definition: joint_model.h:391
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
Definition: robot_state.h:1863
double * getVariableEffort()
Get raw access to the effort of the variables that make up this state. The values are in the same ord...
Definition: robot_state.h:450
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, const std::string &tip, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:977
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:263
const double * getJointVelocities(const JointModel *joint) const
Definition: robot_state.h:584
std::vector< double > values
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1491
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1560
const double * getJointAccelerations(const JointModel *joint) const
Definition: robot_state.h:594
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...
Definition: robot_state.h:809
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const EigenSTL::vector_Isometry3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1210
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Definition: link_model.h:87
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
Definition: robot_state.h:284
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:959
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1901
#define MOVEIT_DEPRECATED
Definition: deprecation.h:48
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1441
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1579
void setVariablePositions(const std::vector< double > &position)
It is assumed positions is an array containing the new positions for all variables in this state...
Definition: robot_state.h:201
virtual bool harmonizePosition(double *values, const Bounds &other_bounds) const
Definition: joint_model.cpp:93
virtual void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform.
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index)
Definition: robot_state.h:1435
void setJointPositions(const JointModel *joint, const double *position)
Definition: robot_state.h:542
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 ...
Definition: robot_state.h:878
Eigen::Isometry3d * global_link_transforms_
Definition: robot_state.h:1904
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.
Definition: joint_model.h:278
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...
Definition: robot_state.h:724
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name) const
Definition: robot_state.h:1458
void interpolate(const RobotState &to, double t, RobotState &state, const JointModel *joint) const
Update state by interpolating form this state towards to, at time t in [0,1] but only for the joint j...
Definition: robot_state.h:1544
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition: robot_state.h:181
void setVariableAcceleration(int index, double value)
Set the acceleration of a single variable. The variable is specified by its index (a value associated...
Definition: robot_state.h:413
geometry_msgs::TransformStamped t
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1814
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...
Definition: robot_state.h:499
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...
Definition: robot_state.h:638
bool dirtyLinkTransforms() const
Definition: robot_state.h:1496
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:995
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
Definition: robot_state.h:532
virtual void interpolate(const double *from, const double *to, const double t, double *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:168
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. If the group is not a chai...
Definition: robot_state.h:1310
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link) const
Definition: robot_state.h:1463
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
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:420
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...
Definition: robot_state.h:772
MOVEIT_DEPRECATED void updateMimicJoint(const std::vector< const JointModel *> &mim)
Update a set of joints that are certain to be mimicking other joints.
Definition: robot_state.h:1848
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...
Definition: robot_state.h:744
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...
Definition: robot_state.h:492
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:599
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1012
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
Definition: robot_state.h:1469
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...
Definition: robot_state.h:734
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:589
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1590
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...
Definition: robot_state.h:840
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...
Definition: robot_state.h:473
void enforceBounds(const JointModel *joint)
Definition: robot_state.h:1554
const Eigen::Isometry3d & getJointTransform(const JointModel *joint)
Definition: robot_state.h:1446
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
Definition: robot_state.h:1474
Struct for containing jump_threshold.
Definition: robot_state.h:71
virtual double distance(const double *value1, const double *value2) const =0
Compute the distance between two joint states of the same model (represented by the variable values) ...
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...
Definition: robot_state.h:703
MOVEIT_CLASS_FORWARD(RobotModel)
MaxEEFStep(double translation=0.0, double rotation=0.0)
Definition: robot_state.h:98
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
Definition: robot_state.h:537
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...
Definition: robot_state.h:785
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
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. ...
Definition: joint_model.h:309
void setVariableAccelerations(const std::vector< double > &acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Definition: robot_state.h:384
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...
Definition: robot_state.h:757
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:350
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...
Definition: robot_state.h:1287
options
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. Returns true if changes were made.
Definition: joint_model.h:290
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
unsigned char * dirty_joint_transforms_
Definition: robot_state.h:1906
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values.
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1077
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
bool dirty() const
Returns true if anything in this state is dirty.
Definition: robot_state.h:1507
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name) const
Definition: robot_state.h:1480
AttachedBodyCallback attached_body_update_callback_
This event is called when there is a change in the attached bodies for this state; The event specifie...
Definition: robot_state.h:1913
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...
Definition: robot_state.h:863
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1032
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:138
Struct for containing max_step for computeCartesianPath.
Definition: robot_state.h:96
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Isometry3d &target, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1188
double getVariableVelocity(int index) const
Get the velocity of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition: robot_state.h:335
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...
Definition: robot_state.h:458
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...
Definition: robot_state.h:651
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
void getRobotMarkers(visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false)
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first...
Definition: robot_state.h:1767
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...
Definition: robot_state.h:679
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.
Definition: robot_state.h:1411
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
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:666
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:318
void markDirtyJointTransforms(const JointModelGroup *group)
Definition: robot_state.h:1821
int getFirstCollisionBodyTransformIndex() const
Definition: link_model.h:97
double * getVariableVelocities()
Get raw access to the velocities of the variables that make up this state. The values are in the same...
Definition: robot_state.h:270
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1501
void setJointPositions(const std::string &joint_name, const Eigen::Isometry3d &transform)
Definition: robot_state.h:549
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 ...
Definition: robot_state.h:915
void setVariableVelocity(int index, double value)
Set the velocity of a single variable. The variable is specified by its index (a value associated by ...
Definition: robot_state.h:320
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
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 ...
Definition: robot_state.h:1324
JumpThreshold(double jt_revolute, double jt_prismatic)
Definition: robot_state.h:86
double getVariableEffort(const std::string &variable) const
Get the effort of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:506
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1900
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:144
double * getVariableAccelerations()
Get raw access to the accelerations of the variables that make up this state. The values are in the s...
Definition: robot_state.h:358
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 ...
Definition: robot_state.h:901
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1419
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:618
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1920
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
Definition: robot_state.h:188
RobotModelConstPtr robot_model_
Definition: robot_state.h:1889
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1343
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Definition: robot_state.h:373
Eigen::Isometry3d * global_collision_body_transforms_
Definition: robot_state.h:1905
double getVariableEffort(int index) const
Get the effort of a particular variable. The variable is specified by its index. No checks are perfor...
Definition: robot_state.h:514
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
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:406
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1594
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state...
Definition: robot_state.h:464
bool hasEffort() const
By default, if effort is never set or initialized, the state remembers that there is no effort set...
Definition: robot_state.h:442
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
bool satisfiesBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1586
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
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...
Definition: robot_state.h:850
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:222
Main namespace for MoveIt!
double distance(const RobotState &other, const JointModel *joint) const
Definition: robot_state.h:1525
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1054
double getVariablePosition(int index) const
Get the position of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition: robot_state.h:249
double getVariableAcceleration(int index) const
Get the acceleration of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition: robot_state.h:428
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:162
const Eigen::Isometry3d & getJointTransform(const JointModel *joint) const
Definition: robot_state.h:1485
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
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...
Definition: robot_state.h:150
void setVariablePosition(int index, double value)
Set the position of a single variable. The variable is specified by its index (a value associated by ...
Definition: robot_state.h:229
double distance(const RobotState &other) const
Definition: robot_state.h:1518
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...
Definition: robot_state.h:292
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:313
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
boost::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_...
Definition: robot_state.h:64
void setJointPositions(const JointModel *joint, const Eigen::Isometry3d &transform)
Definition: robot_state.h:554
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...
Definition: robot_state.h:689
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1165
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:569
std::map< std::string, AttachedBody * > attached_body_map_
All attached bodies that are part of this state, indexed by their name.
Definition: robot_state.h:1909
const double * getJointPositions(const JointModel *joint) const
Definition: robot_state.h:574
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link)
Definition: robot_state.h:1424
const double * getVariableAccelerations() const
Get const raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames()
Definition: robot_state.h:366
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:561


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Jun 25 2019 19:56:01