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 
43 #include <sensor_msgs/JointState.h>
44 #include <visualization_msgs/MarkerArray.h>
45 #include <std_msgs/ColorRGBA.h>
46 #include <geometry_msgs/Twist.h>
47 #include <cassert>
48 
49 #include <boost/assert.hpp>
50 
51 namespace moveit
52 {
53 namespace core
54 {
55 MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc
56 
61 typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
62  const double* joint_group_variable_values)>
64 
71 {
72  double factor;
73  double revolute; // Radians
74  double prismatic; // Meters
75 
76  explicit JumpThreshold() : factor(0.0), revolute(0.0), prismatic(0.0)
77  {
78  }
79 
80  explicit JumpThreshold(double jt_factor) : JumpThreshold()
81  {
82  factor = jt_factor;
83  }
84 
85  explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold()
86  {
87  revolute = jt_revolute; // Radians
88  prismatic = jt_prismatic; // Meters
89  }
90 };
91 
97 struct MaxEEFStep
98 {
99  MaxEEFStep(double translation, double rotation) : translation(translation), rotation(rotation)
100  {
101  }
102 
103  MaxEEFStep(double step_size) : translation(step_size), rotation(3.5 * step_size) // 0.035 rad = 2 deg
104  {
105  }
106 
107  double translation; // Meters
108  double rotation; // Radians
109 };
110 
129 {
130 public:
133  RobotState(const RobotModelConstPtr& robot_model);
134  ~RobotState();
135 
137  RobotState(const RobotState& other);
138 
140  RobotState& operator=(const RobotState& other);
141 
143  const RobotModelConstPtr& getRobotModel() const
144  {
145  return robot_model_;
146  }
147 
149  std::size_t getVariableCount() const
150  {
151  return robot_model_->getVariableCount();
152  }
153 
155  const std::vector<std::string>& getVariableNames() const
156  {
157  return robot_model_->getVariableNames();
158  }
159 
161  const LinkModel* getLinkModel(const std::string& link) const
162  {
163  return robot_model_->getLinkModel(link);
164  }
165 
167  const JointModel* getJointModel(const std::string& joint) const
168  {
169  return robot_model_->getJointModel(joint);
170  }
171 
173  const JointModelGroup* getJointModelGroup(const std::string& group) const
174  {
175  return robot_model_->getJointModelGroup(group);
176  }
177 
187  {
188  return position_;
189  }
190 
193  const double* getVariablePositions() const
194  {
195  return position_;
196  }
197 
201  void setVariablePositions(const double* position);
202 
206  void setVariablePositions(const std::vector<double>& position)
207  {
208  assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
209  setVariablePositions(&position[0]);
210  }
211 
214  void setVariablePositions(const std::map<std::string, double>& variable_map);
215 
218  void setVariablePositions(const std::map<std::string, double>& variable_map,
219  std::vector<std::string>& missing_variables);
220 
223  void setVariablePositions(const std::vector<std::string>& variable_names,
224  const std::vector<double>& variable_position);
225 
227  void setVariablePosition(const std::string& variable, double value)
228  {
229  setVariablePosition(robot_model_->getVariableIndex(variable), value);
230  }
231 
234  void setVariablePosition(int index, double value)
235  {
236  position_[index] = value;
237  const JointModel* jm = robot_model_->getJointOfVariable(index);
238  if (jm)
239  {
240  markDirtyJointTransforms(jm);
241  updateMimicJoint(jm);
242  }
243  }
244 
246  double getVariablePosition(const std::string& variable) const
247  {
248  return position_[robot_model_->getVariableIndex(variable)];
249  }
250 
254  double getVariablePosition(int index) const
255  {
256  return position_[index];
257  }
258 
268  bool hasVelocities() const
269  {
270  return has_velocity_;
271  }
272 
276  {
277  markVelocity();
278  return velocity_;
279  }
280 
283  const double* getVariableVelocities() const
284  {
285  return velocity_;
286  }
287 
289  void zeroVelocities();
290 
292  void setVariableVelocities(const double* velocity)
293  {
294  has_velocity_ = true;
295  // assume everything is in order in terms of array lengths (for efficiency reasons)
296  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
297  }
298 
300  void setVariableVelocities(const std::vector<double>& velocity)
301  {
302  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
303  setVariableVelocities(&velocity[0]);
304  }
305 
308  void setVariableVelocities(const std::map<std::string, double>& variable_map);
309 
312  void setVariableVelocities(const std::map<std::string, double>& variable_map,
313  std::vector<std::string>& missing_variables);
314 
317  void setVariableVelocities(const std::vector<std::string>& variable_names,
318  const std::vector<double>& variable_velocity);
319 
321  void setVariableVelocity(const std::string& variable, double value)
322  {
323  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
324  }
325 
328  void setVariableVelocity(int index, double value)
329  {
330  markVelocity();
331  velocity_[index] = value;
332  }
333 
335  double getVariableVelocity(const std::string& variable) const
336  {
337  return velocity_[robot_model_->getVariableIndex(variable)];
338  }
339 
343  double getVariableVelocity(int index) const
344  {
345  return velocity_[index];
346  }
347 
349  void dropVelocities();
350 
361  bool hasAccelerations() const
362  {
363  return has_acceleration_;
364  }
365 
370  {
371  markAcceleration();
372  return acceleration_;
373  }
374 
377  const double* getVariableAccelerations() const
378  {
379  return acceleration_;
380  }
381 
383  void zeroAccelerations();
384 
387  void setVariableAccelerations(const double* acceleration)
388  {
389  has_acceleration_ = true;
390  has_effort_ = false;
391 
392  // assume everything is in order in terms of array lengths (for efficiency reasons)
393  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
394  }
395 
398  void setVariableAccelerations(const std::vector<double>& acceleration)
399  {
400  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
401  setVariableAccelerations(&acceleration[0]);
402  }
403 
406  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
407 
411  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
412  std::vector<std::string>& missing_variables);
413 
416  void setVariableAccelerations(const std::vector<std::string>& variable_names,
417  const std::vector<double>& variable_acceleration);
418 
420  void setVariableAcceleration(const std::string& variable, double value)
421  {
422  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
423  }
424 
427  void setVariableAcceleration(int index, double value)
428  {
429  markAcceleration();
430  acceleration_[index] = value;
431  }
432 
434  double getVariableAcceleration(const std::string& variable) const
435  {
436  return acceleration_[robot_model_->getVariableIndex(variable)];
437  }
438 
442  double getVariableAcceleration(int index) const
443  {
444  return acceleration_[index];
445  }
446 
448  void dropAccelerations();
449 
459  bool hasEffort() const
460  {
461  return has_effort_;
462  }
463 
468  {
469  markEffort();
470  return effort_;
471  }
472 
475  const double* getVariableEffort() const
476  {
477  return effort_;
478  }
479 
481  void zeroEffort();
482 
484  void setVariableEffort(const double* effort)
485  {
486  has_effort_ = true;
487  has_acceleration_ = false;
488  // assume everything is in order in terms of array lengths (for efficiency reasons)
489  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
490  }
491 
493  void setVariableEffort(const std::vector<double>& effort)
494  {
495  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
496  setVariableEffort(&effort[0]);
497  }
498 
500  void setVariableEffort(const std::map<std::string, double>& variable_map);
501 
504  void setVariableEffort(const std::map<std::string, double>& variable_map, std::vector<std::string>& missing_variables);
505 
507  void setVariableEffort(const std::vector<std::string>& variable_names,
508  const std::vector<double>& variable_acceleration);
509 
511  void setVariableEffort(const std::string& variable, double value)
512  {
513  setVariableEffort(robot_model_->getVariableIndex(variable), value);
514  }
515 
518  void setVariableEffort(int index, double value)
519  {
520  markEffort();
521  effort_[index] = value;
522  }
523 
525  double getVariableEffort(const std::string& variable) const
526  {
527  return effort_[robot_model_->getVariableIndex(variable)];
528  }
529 
533  double getVariableEffort(int index) const
534  {
535  return effort_[index];
536  }
537 
539  void dropEffort();
540 
542  void dropDynamics();
543 
545  void invertVelocity();
546 
554  void setJointPositions(const std::string& joint_name, const double* position)
555  {
556  setJointPositions(robot_model_->getJointModel(joint_name), position);
557  }
558 
559  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
560  {
561  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
562  }
563 
564  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
565  {
566  setJointPositions(joint, &position[0]);
567  }
568 
569  void setJointPositions(const JointModel* joint, const double* position)
570  {
571  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
572  markDirtyJointTransforms(joint);
573  updateMimicJoint(joint);
574  }
575 
576  void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
577  {
578  setJointPositions(robot_model_->getJointModel(joint_name), transform);
579  }
580 
581  void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform)
582  {
583  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
584  markDirtyJointTransforms(joint);
585  updateMimicJoint(joint);
586  }
587 
588  void setJointVelocities(const JointModel* joint, const double* velocity)
589  {
590  has_velocity_ = true;
591  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
592  }
593 
594  void setJointEfforts(const JointModel* joint, const double* effort);
595 
596  const double* getJointPositions(const std::string& joint_name) const
597  {
598  return getJointPositions(robot_model_->getJointModel(joint_name));
599  }
600 
601  const double* getJointPositions(const JointModel* joint) const
602  {
603  return position_ + joint->getFirstVariableIndex();
604  }
605 
606  const double* getJointVelocities(const std::string& joint_name) const
607  {
608  return getJointVelocities(robot_model_->getJointModel(joint_name));
609  }
610 
611  const double* getJointVelocities(const JointModel* joint) const
612  {
613  return velocity_ + joint->getFirstVariableIndex();
614  }
615 
616  const double* getJointAccelerations(const std::string& joint_name) const
617  {
618  return getJointAccelerations(robot_model_->getJointModel(joint_name));
619  }
620 
621  const double* getJointAccelerations(const JointModel* joint) const
622  {
623  return acceleration_ + joint->getFirstVariableIndex();
624  }
625 
626  const double* getJointEffort(const std::string& joint_name) const
627  {
628  return getJointEffort(robot_model_->getJointModel(joint_name));
629  }
630 
631  const double* getJointEffort(const JointModel* joint) const
632  {
633  return effort_ + joint->getFirstVariableIndex();
634  }
635 
645  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
646  {
647  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
648  if (jmg)
649  setJointGroupPositions(jmg, gstate);
650  }
651 
655  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
656  {
657  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
658  if (jmg)
659  {
660  assert(gstate.size() == jmg->getVariableCount());
661  setJointGroupPositions(jmg, &gstate[0]);
662  }
663  }
664 
668  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
669  {
670  assert(gstate.size() == group->getVariableCount());
671  setJointGroupPositions(group, &gstate[0]);
672  }
673 
677  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
678 
682  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
683  {
684  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
685  if (jmg)
686  {
687  assert(values.size() == jmg->getVariableCount());
688  setJointGroupPositions(jmg, values);
689  }
690  }
691 
695  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
696 
700  void setJointGroupActivePositions(const JointModelGroup* group, const std::vector<double>& gstate);
701 
705  void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector<double>& gstate)
706  {
707  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
708  if (jmg)
709  {
710  assert(gstate.size() == jmg->getActiveVariableCount());
711  setJointGroupActivePositions(jmg, gstate);
712  }
713  }
714 
718  void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values);
719 
723  void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
724  {
725  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
726  if (jmg)
727  {
728  assert(values.size() == jmg->getActiveVariableCount());
729  setJointGroupActivePositions(jmg, values);
730  }
731  }
732 
736  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
737  {
738  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
739  if (jmg)
740  {
741  gstate.resize(jmg->getVariableCount());
742  copyJointGroupPositions(jmg, &gstate[0]);
743  }
744  }
745 
749  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
750  {
751  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
752  if (jmg)
753  copyJointGroupPositions(jmg, gstate);
754  }
755 
759  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
760  {
761  gstate.resize(group->getVariableCount());
762  copyJointGroupPositions(group, &gstate[0]);
763  }
764 
768  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
769 
773  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
774  {
775  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
776  if (jmg)
777  copyJointGroupPositions(jmg, values);
778  }
779 
783  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
784 
794  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
795  {
796  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
797  if (jmg)
798  setJointGroupVelocities(jmg, gstate);
799  }
800 
804  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
805  {
806  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
807  if (jmg)
808  setJointGroupVelocities(jmg, &gstate[0]);
809  }
810 
814  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
815  {
816  setJointGroupVelocities(group, &gstate[0]);
817  }
818 
822  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
823 
827  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
828  {
829  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
830  if (jmg)
831  setJointGroupVelocities(jmg, values);
832  }
833 
837  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
838 
842  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
843  {
844  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
845  if (jmg)
846  {
847  gstate.resize(jmg->getVariableCount());
848  copyJointGroupVelocities(jmg, &gstate[0]);
849  }
850  }
851 
855  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
856  {
857  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
858  if (jmg)
859  copyJointGroupVelocities(jmg, gstate);
860  }
861 
865  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
866  {
867  gstate.resize(group->getVariableCount());
868  copyJointGroupVelocities(group, &gstate[0]);
869  }
870 
874  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
875 
879  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
880  {
881  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
882  if (jmg)
883  copyJointGroupVelocities(jmg, values);
884  }
885 
889  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
890 
900  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
901  {
902  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
903  if (jmg)
904  setJointGroupAccelerations(jmg, gstate);
905  }
906 
910  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
911  {
912  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
913  if (jmg)
914  setJointGroupAccelerations(jmg, &gstate[0]);
915  }
916 
920  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
921  {
922  setJointGroupAccelerations(group, &gstate[0]);
923  }
924 
928  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
929 
933  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
934  {
935  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
936  if (jmg)
937  setJointGroupAccelerations(jmg, values);
938  }
939 
943  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
944 
948  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
949  {
950  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
951  if (jmg)
952  {
953  gstate.resize(jmg->getVariableCount());
954  copyJointGroupAccelerations(jmg, &gstate[0]);
955  }
956  }
957 
961  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
962  {
963  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
964  if (jmg)
965  copyJointGroupAccelerations(jmg, gstate);
966  }
967 
971  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
972  {
973  gstate.resize(group->getVariableCount());
974  copyJointGroupAccelerations(group, &gstate[0]);
975  }
976 
980  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
981 
985  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
986  {
987  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
988  if (jmg)
989  copyJointGroupAccelerations(jmg, values);
990  }
991 
995  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
996 
1009  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
1012  [[deprecated("The attempts argument is not supported anymore.")]] bool
1013  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int /*attempts*/,
1014  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1016  {
1017  return setFromIK(group, pose, timeout, constraint, options);
1018  }
1019 
1027  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
1028  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1030  [[deprecated("The attempts argument is not supported anymore.")]] bool
1031  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
1032  unsigned int /*attempts*/, double timeout = 0.0,
1035  {
1036  return setFromIK(group, pose, tip, timeout, constraint, options);
1037  }
1038 
1045  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
1048  [[deprecated("The attempts argument is not supported anymore.")]] bool
1049  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int /*attempts*/,
1050  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1052  {
1053  return setFromIK(group, pose, timeout, constraint, options);
1054  }
1055 
1062  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1063  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1065  [[deprecated("The attempts argument is not supported anymore.")]] bool
1066  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1067  unsigned int /*attempts*/, double timeout = 0.0,
1070  {
1071  return setFromIK(group, pose, tip, timeout, constraint, options);
1072  }
1073 
1082  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1083  const std::vector<double>& consistency_limits, double timeout = 0.0,
1086  [[deprecated("The attempts argument is not supported anymore.")]] bool
1087  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1088  const std::vector<double>& consistency_limits, unsigned int /*attempts*/, double timeout = 0.0,
1091  {
1092  return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options);
1093  }
1094 
1104  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1105  const std::vector<std::string>& tips, double timeout = 0.0,
1108  [[deprecated("The attempts argument is not supported anymore.")]] bool
1110  const std::vector<std::string>& tips, unsigned int /*attempts*/, double timeout = 0.0,
1113  {
1114  return setFromIK(group, poses, tips, timeout, constraint, options);
1115  }
1116 
1127  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1128  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1129  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1131  [[deprecated("The attempts argument is not supported anymore.")]] bool
1133  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1134  unsigned int /*attempts*/, double timeout = 0.0,
1137  {
1138  return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options);
1139  }
1140 
1149  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1150  const std::vector<std::string>& tips,
1151  const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
1154  [[deprecated("The attempts argument is not supported anymore.")]] bool
1156  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1157  unsigned int /*attempts*/, double timeout = 0.0,
1160  {
1161  return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options);
1162  }
1163 
1171  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1173 
1181  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1183 
1214  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1215  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1216  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1219 
1220  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1221  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1222  double max_step, double jump_threshold_factor,
1225  {
1226  return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance,
1227  MaxEEFStep(max_step, max_step), JumpThreshold(jump_threshold_factor), validCallback,
1228  options);
1229  }
1230 
1237  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1238  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
1239  const JumpThreshold& jump_threshold,
1242 
1243  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1244  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1245  double jump_threshold_factor,
1248  {
1249  return computeCartesianPath(group, traj, link, target, global_reference_frame, MaxEEFStep(max_step),
1250  JumpThreshold(jump_threshold_factor), validCallback, options);
1251  }
1252 
1259  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1260  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1261  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1264 
1265  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1266  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1267  double max_step, double jump_threshold_factor,
1270  {
1271  return computeCartesianPath(group, traj, link, waypoints, global_reference_frame, MaxEEFStep(max_step),
1272  JumpThreshold(jump_threshold_factor), validCallback, options);
1273  }
1274 
1290  static double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1291  const JumpThreshold& jump_threshold);
1292 
1303  static double testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1304  double jump_threshold_factor);
1305 
1318  static double testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1319  double revolute_jump_threshold, double prismatic_jump_threshold);
1320 
1330  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1331  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1332 
1342  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1343  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1344  {
1345  updateLinkTransforms();
1346  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1347  use_quaternion_representation);
1348  }
1349 
1356  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1357  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1358 
1365  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1366  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1367  {
1368  updateLinkTransforms();
1369  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1370  }
1371 
1374  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1375  const LinkModel* tip) const;
1376 
1379  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1380  const LinkModel* tip)
1381  {
1382  updateLinkTransforms();
1383  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1384  }
1385 
1389  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1391 
1398  void setVariableValues(const sensor_msgs::JointState& msg)
1399  {
1400  if (!msg.position.empty())
1401  setVariablePositions(msg.name, msg.position);
1402  if (!msg.velocity.empty())
1403  setVariableVelocities(msg.name, msg.velocity);
1404  }
1405 
1409  void setToDefaultValues();
1410 
1412  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1413 
1415  void setToRandomPositions();
1416 
1418  void setToRandomPositions(const JointModelGroup* group);
1419 
1422  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1423 
1429  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1430 
1438  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1439  const std::vector<double>& distances);
1440 
1449  void updateCollisionBodyTransforms();
1450 
1453  void updateLinkTransforms();
1454 
1456  void update(bool force = false);
1457 
1466  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1467  {
1468  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1469  }
1470 
1472  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1473 
1474  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1475  {
1476  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1477  }
1478 
1479  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1480  {
1481  updateLinkTransforms();
1482  return global_link_transforms_[link->getLinkIndex()];
1483  }
1484 
1485  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1486  {
1487  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1488  }
1489 
1490  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1491  {
1492  updateCollisionBodyTransforms();
1493  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1494  }
1495 
1496  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1497  {
1498  return getJointTransform(robot_model_->getJointModel(joint_name));
1499  }
1500 
1501  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1502  {
1503  const int idx = joint->getJointIndex();
1504  unsigned char& dirty = dirty_joint_transforms_[idx];
1505  if (dirty)
1506  {
1507  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1508  dirty = 0;
1509  }
1510  return variable_joint_transforms_[idx];
1511  }
1512 
1513  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1514  {
1515  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1516  }
1517 
1518  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1519  {
1520  BOOST_VERIFY(checkLinkTransforms());
1521  return global_link_transforms_[link->getLinkIndex()];
1522  }
1523 
1524  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1525  {
1526  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1527  }
1528 
1529  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1530  {
1531  BOOST_VERIFY(checkCollisionTransforms());
1532  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1533  }
1534 
1535  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1536  {
1537  return getJointTransform(robot_model_->getJointModel(joint_name));
1538  }
1539 
1540  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1541  {
1542  BOOST_VERIFY(checkJointTransforms(joint));
1543  return variable_joint_transforms_[joint->getJointIndex()];
1544  }
1545 
1546  bool dirtyJointTransform(const JointModel* joint) const
1547  {
1548  return dirty_joint_transforms_[joint->getJointIndex()];
1549  }
1550 
1551  bool dirtyLinkTransforms() const
1552  {
1553  return dirty_link_transforms_;
1554  }
1555 
1557  {
1558  return dirty_link_transforms_ || dirty_collision_body_transforms_;
1559  }
1560 
1562  bool dirty() const
1563  {
1564  return dirtyCollisionBodyTransforms();
1565  }
1566 
1574  double distance(const RobotState& other) const
1575  {
1576  return robot_model_->distance(position_, other.getVariablePositions());
1577  }
1578 
1580  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1581 
1583  double distance(const RobotState& other, const JointModel* joint) const
1584  {
1585  const int idx = joint->getFirstVariableIndex();
1586  return joint->distance(position_ + idx, other.position_ + idx);
1587  }
1588 
1597  void interpolate(const RobotState& to, double t, RobotState& state) const;
1598 
1608  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1609 
1619  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1620  {
1621  const int idx = joint->getFirstVariableIndex();
1622  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1623  state.markDirtyJointTransforms(joint);
1624  state.updateMimicJoint(joint);
1625  }
1626 
1627  void enforceBounds();
1628  void enforceBounds(const JointModelGroup* joint_group);
1629  void enforceBounds(const JointModel* joint)
1630  {
1631  enforcePositionBounds(joint);
1632  if (has_velocity_)
1633  enforceVelocityBounds(joint);
1634  }
1636  {
1637  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1638  {
1639  markDirtyJointTransforms(joint);
1640  updateMimicJoint(joint);
1641  }
1642  }
1643 
1645  void harmonizePositions();
1646  void harmonizePositions(const JointModelGroup* joint_group);
1647  void harmonizePosition(const JointModel* joint)
1648  {
1649  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1650  // no need to mark transforms dirty, as the transform hasn't changed
1651  updateMimicJoint(joint);
1652  }
1653 
1655  {
1656  joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
1657  }
1658 
1659  bool satisfiesBounds(double margin = 0.0) const;
1660  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1661  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1662  {
1663  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1664  }
1665  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1666  {
1667  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1668  }
1669  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1670  {
1671  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1672  }
1673 
1676  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1677 
1680  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1681 
1684  std::pair<double, const JointModel*>
1685  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1686 
1693  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1694 
1720  void attachBody(AttachedBody* attached_body);
1721 
1736  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1737  const EigenSTL::vector_Isometry3d& attach_trans, const std::set<std::string>& touch_links,
1738  const std::string& link_name,
1739  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1740 
1755  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1756  const EigenSTL::vector_Isometry3d& attach_trans, const std::vector<std::string>& touch_links,
1757  const std::string& link_name,
1758  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1759  {
1760  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1761  attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1762  }
1763 
1765  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1766 
1768  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
1769 
1771  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
1772 
1775  bool clearAttachedBody(const std::string& id);
1776 
1778  void clearAttachedBodies(const LinkModel* link);
1779 
1781  void clearAttachedBodies(const JointModelGroup* group);
1782 
1784  void clearAttachedBodies();
1785 
1787  const AttachedBody* getAttachedBody(const std::string& name) const;
1788 
1790  bool hasAttachedBody(const std::string& id) const;
1791 
1792  void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback);
1797  void computeAABB(std::vector<double>& aabb) const;
1798 
1801  void computeAABB(std::vector<double>& aabb)
1802  {
1803  updateLinkTransforms();
1804  static_cast<const RobotState*>(this)->computeAABB(aabb);
1805  }
1806 
1809  {
1810  if (!rng_)
1812  return *rng_;
1813  }
1814 
1816  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id);
1817 
1819  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id) const;
1820 
1822  bool knowsFrameTransform(const std::string& frame_id) const;
1823 
1831  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1832  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1833  bool include_attached = false) const;
1834 
1842  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1843  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1844  bool include_attached = false)
1845  {
1846  updateCollisionBodyTransforms();
1847  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1848  }
1849 
1854  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1855  bool include_attached = false) const;
1856 
1861  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1862  bool include_attached = false)
1863  {
1864  updateCollisionBodyTransforms();
1865  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1866  }
1867 
1868  void printStatePositions(std::ostream& out = std::cout) const;
1869 
1871  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const;
1872 
1873  void printStateInfo(std::ostream& out = std::cout) const;
1874 
1875  void printTransforms(std::ostream& out = std::cout) const;
1876 
1877  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1878 
1879  void printDirtyInfo(std::ostream& out = std::cout) const;
1880 
1881  std::string getStateTreeString(const std::string& prefix = "") const;
1882 
1889  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
1890 
1897  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
1898 
1899 private:
1900  void allocMemory();
1901  void initTransforms();
1902  void copyFrom(const RobotState& other);
1903 
1905  {
1906  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1907  dirty_link_transforms_ =
1908  dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1909  }
1910 
1912  {
1913  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
1914  for (std::size_t i = 0; i < jm.size(); ++i)
1915  dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1916  dirty_link_transforms_ = dirty_link_transforms_ == nullptr ?
1917  group->getCommonRoot() :
1918  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1919  }
1920 
1921  void markVelocity();
1922  void markAcceleration();
1923  void markEffort();
1924 
1925  void updateMimicJoint(const JointModel* joint)
1926  {
1927  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
1928  double v = position_[joint->getFirstVariableIndex()];
1929  for (std::size_t i = 0; i < mim.size(); ++i)
1930  {
1931  position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1932  markDirtyJointTransforms(mim[i]);
1933  }
1934  }
1935 
1937  /* use updateMimicJoints() instead, which also marks joints dirty */
1938  [[deprecated]] void updateMimicJoint(const std::vector<const JointModel*>& mim)
1939  {
1940  for (std::size_t i = 0; i < mim.size(); ++i)
1941  {
1942  const int fvi = mim[i]->getFirstVariableIndex();
1943  position_[fvi] =
1944  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1945  // Only mark joint transform dirty, but not the associated link transform
1946  // as this function is always used in combination of
1947  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1948  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1949  }
1950  }
1951 
1954  {
1955  for (const JointModel* jm : group->getMimicJointModels())
1956  {
1957  const int fvi = jm->getFirstVariableIndex();
1958  position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
1959  markDirtyJointTransforms(jm);
1960  }
1961  markDirtyJointTransforms(group);
1962  }
1963 
1964  void updateLinkTransformsInternal(const JointModel* start);
1965 
1966  void getMissingKeys(const std::map<std::string, double>& variable_map,
1967  std::vector<std::string>& missing_variables) const;
1968  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1969 
1971  bool checkJointTransforms(const JointModel* joint) const;
1972 
1974  bool checkLinkTransforms() const;
1975 
1977  bool checkCollisionTransforms() const;
1978 
1979  RobotModelConstPtr robot_model_;
1980  void* memory_;
1981 
1982  double* position_;
1983  double* velocity_;
1984  double* acceleration_;
1985  double* effort_;
1989 
1992 
1993  Eigen::Isometry3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned
1994  Eigen::Isometry3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned
1995  Eigen::Isometry3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned
1996  unsigned char* dirty_joint_transforms_;
1997 
1999  std::map<std::string, AttachedBody*> attached_body_map_;
2000 
2004 
2011 };
2012 
2014 std::ostream& operator<<(std::ostream& out, const RobotState& s);
2015 } // namespace core
2016 } // namespace moveit
2017 
2018 #endif
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:554
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Definition: robot_state.h:1485
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:335
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1808
JumpThreshold(double jt_factor)
Definition: robot_state.h:80
A set of options for the kinematics solver.
Eigen::Isometry3d * variable_joint_transforms_
Definition: robot_state.h:1993
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:1801
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1925
const double * getJointEffort(const JointModel *joint) const
Definition: robot_state.h:631
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:865
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:961
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:1861
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:655
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:900
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:246
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:283
ROSCPP_DECL void start()
void harmonizePosition(const JointModel *joint)
Definition: robot_state.h:1647
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:606
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:1755
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:1953
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:467
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:268
const double * getJointVelocities(const JointModel *joint) const
Definition: robot_state.h:611
std::vector< double > values
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1546
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1635
const double * getJointAccelerations(const JointModel *joint) const
Definition: robot_state.h:621
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:879
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:1265
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1049
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:292
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1991
MaxEEFStep(double translation, double rotation)
Definition: robot_state.h:99
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, const std::string &tip, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1031
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1496
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1654
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:206
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:1490
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1066
void setJointPositions(const JointModel *joint, const double *position)
Definition: robot_state.h:569
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:948
Eigen::Isometry3d * global_link_transforms_
Definition: robot_state.h:1994
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:794
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name) const
Definition: robot_state.h:1513
void interpolate(const RobotState &to, double t, RobotState &state, const JointModel *joint) const
Definition: robot_state.h:1619
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:186
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:427
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, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1132
geometry_msgs::TransformStamped t
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1904
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:518
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:668
bool dirtyLinkTransforms() const
Definition: robot_state.h:1551
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
Definition: robot_state.h:559
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:173
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:1365
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link) const
Definition: robot_state.h:1518
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:434
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:842
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:814
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:511
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:626
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
Definition: robot_state.h:1524
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:804
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:616
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1665
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:910
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:493
void enforceBounds(const JointModel *joint)
Definition: robot_state.h:1629
const Eigen::Isometry3d & getJointTransform(const JointModel *joint)
Definition: robot_state.h:1501
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
Definition: robot_state.h:1529
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1013
Struct for containing jump_threshold.
Definition: robot_state.h:70
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:773
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...
Definition: robot_state.h:705
MOVEIT_CLASS_FORWARD(RobotModel)
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
Definition: robot_state.h:564
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:855
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:398
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:827
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:361
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:1342
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:1996
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values.
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:1562
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name) const
Definition: robot_state.h:1535
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:2003
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:933
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:143
Struct for containing max_step for computeCartesianPath.
Definition: robot_state.h:97
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:1243
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:343
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:475
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:682
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:1842
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:749
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:1466
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:736
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, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1155
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:1911
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:275
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1556
void setJointPositions(const std::string &joint_name, const Eigen::Isometry3d &transform)
Definition: robot_state.h:576
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:985
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1087
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:328
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:161
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:1379
JumpThreshold(double jt_revolute, double jt_prismatic)
Definition: robot_state.h:85
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:525
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1990
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:149
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...
Definition: robot_state.h:723
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:369
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:971
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1474
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:645
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:2010
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
Definition: robot_state.h:193
RobotModelConstPtr robot_model_
Definition: robot_state.h:1979
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1398
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:387
Eigen::Isometry3d * global_collision_body_transforms_
Definition: robot_state.h:1995
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:533
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:128
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:420
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1669
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:484
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:459
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:1661
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:920
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:227
Main namespace for MoveIt!
double distance(const RobotState &other, const JointModel *joint) const
Return the sum of joint distances to "other" state. Only considers active joints. ...
Definition: robot_state.h:1583
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:254
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:442
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:167
const Eigen::Isometry3d & getJointTransform(const JointModel *joint) const
Definition: robot_state.h:1540
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1109
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:155
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:234
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. Only considers active joints. ...
Definition: robot_state.h:1574
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:300
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:321
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:63
void setJointPositions(const JointModel *joint, const Eigen::Isometry3d &transform)
Definition: robot_state.h:581
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:759
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:1220
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:596
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:1999
const double * getJointPositions(const JointModel *joint) const
Definition: robot_state.h:601
MaxEEFStep(double step_size)
Definition: robot_state.h:103
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link)
Definition: robot_state.h:1479
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:377
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:588
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:1938


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Mar 17 2022 02:51:04