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 
95 struct MaxEEFStep
96 {
97  MaxEEFStep(double translation = 0.0, double rotation = 0.0) : translation(translation), rotation(rotation)
98  {
99  }
100 
101  double translation; // Meters
102  double rotation; // Radians
103 };
104 
123 {
124 public:
127  RobotState(const RobotModelConstPtr& robot_model);
128  ~RobotState();
129 
131  RobotState(const RobotState& other);
132 
134  RobotState& operator=(const RobotState& other);
135 
137  const RobotModelConstPtr& getRobotModel() const
138  {
139  return robot_model_;
140  }
141 
143  std::size_t getVariableCount() const
144  {
145  return robot_model_->getVariableCount();
146  }
147 
149  const std::vector<std::string>& getVariableNames() const
150  {
151  return robot_model_->getVariableNames();
152  }
153 
155  const LinkModel* getLinkModel(const std::string& link) const
156  {
157  return robot_model_->getLinkModel(link);
158  }
159 
161  const JointModel* getJointModel(const std::string& joint) const
162  {
163  return robot_model_->getJointModel(joint);
164  }
165 
167  const JointModelGroup* getJointModelGroup(const std::string& group) const
168  {
169  return robot_model_->getJointModelGroup(group);
170  }
171 
181  {
182  return position_;
183  }
184 
187  const double* getVariablePositions() const
188  {
189  return position_;
190  }
191 
195  void setVariablePositions(const double* position);
196 
200  void setVariablePositions(const std::vector<double>& position)
201  {
202  assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
203  setVariablePositions(&position[0]);
204  }
205 
208  void setVariablePositions(const std::map<std::string, double>& variable_map);
209 
212  void setVariablePositions(const std::map<std::string, double>& variable_map,
213  std::vector<std::string>& missing_variables);
214 
217  void setVariablePositions(const std::vector<std::string>& variable_names,
218  const std::vector<double>& variable_position);
219 
221  void setVariablePosition(const std::string& variable, double value)
222  {
223  setVariablePosition(robot_model_->getVariableIndex(variable), value);
224  }
225 
228  void setVariablePosition(int index, double value)
229  {
230  position_[index] = value;
231  const JointModel* jm = robot_model_->getJointOfVariable(index);
232  if (jm)
233  {
234  markDirtyJointTransforms(jm);
235  updateMimicJoint(jm);
236  }
237  }
238 
240  double getVariablePosition(const std::string& variable) const
241  {
242  return position_[robot_model_->getVariableIndex(variable)];
243  }
244 
248  double getVariablePosition(int index) const
249  {
250  return position_[index];
251  }
252 
262  bool hasVelocities() const
263  {
264  return has_velocity_;
265  }
266 
270  {
271  markVelocity();
272  return velocity_;
273  }
274 
277  const double* getVariableVelocities() const
278  {
279  return velocity_;
280  }
281 
283  void zeroVelocities();
284 
286  void setVariableVelocities(const double* velocity)
287  {
288  has_velocity_ = true;
289  // assume everything is in order in terms of array lengths (for efficiency reasons)
290  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
291  }
292 
294  void setVariableVelocities(const std::vector<double>& velocity)
295  {
296  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
297  setVariableVelocities(&velocity[0]);
298  }
299 
302  void setVariableVelocities(const std::map<std::string, double>& variable_map);
303 
306  void setVariableVelocities(const std::map<std::string, double>& variable_map,
307  std::vector<std::string>& missing_variables);
308 
311  void setVariableVelocities(const std::vector<std::string>& variable_names,
312  const std::vector<double>& variable_velocity);
313 
315  void setVariableVelocity(const std::string& variable, double value)
316  {
317  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
318  }
319 
322  void setVariableVelocity(int index, double value)
323  {
324  markVelocity();
325  velocity_[index] = value;
326  }
327 
329  double getVariableVelocity(const std::string& variable) const
330  {
331  return velocity_[robot_model_->getVariableIndex(variable)];
332  }
333 
337  double getVariableVelocity(int index) const
338  {
339  return velocity_[index];
340  }
341 
343  void dropVelocities();
344 
355  bool hasAccelerations() const
356  {
357  return has_acceleration_;
358  }
359 
364  {
365  markAcceleration();
366  return acceleration_;
367  }
368 
371  const double* getVariableAccelerations() const
372  {
373  return acceleration_;
374  }
375 
377  void zeroAccelerations();
378 
381  void setVariableAccelerations(const double* acceleration)
382  {
383  has_acceleration_ = true;
384  has_effort_ = false;
385 
386  // assume everything is in order in terms of array lengths (for efficiency reasons)
387  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
388  }
389 
392  void setVariableAccelerations(const std::vector<double>& acceleration)
393  {
394  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
395  setVariableAccelerations(&acceleration[0]);
396  }
397 
400  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
401 
405  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
406  std::vector<std::string>& missing_variables);
407 
410  void setVariableAccelerations(const std::vector<std::string>& variable_names,
411  const std::vector<double>& variable_acceleration);
412 
414  void setVariableAcceleration(const std::string& variable, double value)
415  {
416  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
417  }
418 
421  void setVariableAcceleration(int index, double value)
422  {
423  markAcceleration();
424  acceleration_[index] = value;
425  }
426 
428  double getVariableAcceleration(const std::string& variable) const
429  {
430  return acceleration_[robot_model_->getVariableIndex(variable)];
431  }
432 
436  double getVariableAcceleration(int index) const
437  {
438  return acceleration_[index];
439  }
440 
442  void dropAccelerations();
443 
453  bool hasEffort() const
454  {
455  return has_effort_;
456  }
457 
462  {
463  markEffort();
464  return effort_;
465  }
466 
469  const double* getVariableEffort() const
470  {
471  return effort_;
472  }
473 
475  void zeroEffort();
476 
478  void setVariableEffort(const double* effort)
479  {
480  has_effort_ = true;
481  has_acceleration_ = false;
482  // assume everything is in order in terms of array lengths (for efficiency reasons)
483  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
484  }
485 
487  void setVariableEffort(const std::vector<double>& effort)
488  {
489  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
490  setVariableEffort(&effort[0]);
491  }
492 
494  void setVariableEffort(const std::map<std::string, double>& variable_map);
495 
498  void setVariableEffort(const std::map<std::string, double>& variable_map, std::vector<std::string>& missing_variables);
499 
501  void setVariableEffort(const std::vector<std::string>& variable_names,
502  const std::vector<double>& variable_acceleration);
503 
505  void setVariableEffort(const std::string& variable, double value)
506  {
507  setVariableEffort(robot_model_->getVariableIndex(variable), value);
508  }
509 
512  void setVariableEffort(int index, double value)
513  {
514  markEffort();
515  effort_[index] = value;
516  }
517 
519  double getVariableEffort(const std::string& variable) const
520  {
521  return effort_[robot_model_->getVariableIndex(variable)];
522  }
523 
527  double getVariableEffort(int index) const
528  {
529  return effort_[index];
530  }
531 
533  void dropEffort();
534 
536  void dropDynamics();
537 
539  void invertVelocity();
540 
548  void setJointPositions(const std::string& joint_name, const double* position)
549  {
550  setJointPositions(robot_model_->getJointModel(joint_name), position);
551  }
552 
553  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
554  {
555  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
556  }
557 
558  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
559  {
560  setJointPositions(joint, &position[0]);
561  }
562 
563  void setJointPositions(const JointModel* joint, const double* position)
564  {
565  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
566  markDirtyJointTransforms(joint);
567  updateMimicJoint(joint);
568  }
569 
570  void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
571  {
572  setJointPositions(robot_model_->getJointModel(joint_name), transform);
573  }
574 
575  void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform)
576  {
577  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
578  markDirtyJointTransforms(joint);
579  updateMimicJoint(joint);
580  }
581 
582  void setJointVelocities(const JointModel* joint, const double* velocity)
583  {
584  has_velocity_ = true;
585  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
586  }
587 
588  void setJointEfforts(const JointModel* joint, const double* effort);
589 
590  const double* getJointPositions(const std::string& joint_name) const
591  {
592  return getJointPositions(robot_model_->getJointModel(joint_name));
593  }
594 
595  const double* getJointPositions(const JointModel* joint) const
596  {
597  return position_ + joint->getFirstVariableIndex();
598  }
599 
600  const double* getJointVelocities(const std::string& joint_name) const
601  {
602  return getJointVelocities(robot_model_->getJointModel(joint_name));
603  }
604 
605  const double* getJointVelocities(const JointModel* joint) const
606  {
607  return velocity_ + joint->getFirstVariableIndex();
608  }
609 
610  const double* getJointAccelerations(const std::string& joint_name) const
611  {
612  return getJointAccelerations(robot_model_->getJointModel(joint_name));
613  }
614 
615  const double* getJointAccelerations(const JointModel* joint) const
616  {
617  return acceleration_ + joint->getFirstVariableIndex();
618  }
619 
620  const double* getJointEffort(const std::string& joint_name) const
621  {
622  return getJointEffort(robot_model_->getJointModel(joint_name));
623  }
624 
625  const double* getJointEffort(const JointModel* joint) const
626  {
627  return effort_ + joint->getFirstVariableIndex();
628  }
629 
639  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
640  {
641  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
642  if (jmg)
643  setJointGroupPositions(jmg, gstate);
644  }
645 
649  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
650  {
651  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
652  if (jmg)
653  setJointGroupPositions(jmg, &gstate[0]);
654  }
655 
659  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
660  {
661  setJointGroupPositions(group, &gstate[0]);
662  }
663 
667  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
668 
672  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
673  {
674  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
675  if (jmg)
676  setJointGroupPositions(jmg, values);
677  }
678 
682  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
683 
687  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
688  {
689  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
690  if (jmg)
691  {
692  gstate.resize(jmg->getVariableCount());
693  copyJointGroupPositions(jmg, &gstate[0]);
694  }
695  }
696 
700  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
701  {
702  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
703  if (jmg)
704  copyJointGroupPositions(jmg, gstate);
705  }
706 
710  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
711  {
712  gstate.resize(group->getVariableCount());
713  copyJointGroupPositions(group, &gstate[0]);
714  }
715 
719  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
720 
724  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
725  {
726  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
727  if (jmg)
728  copyJointGroupPositions(jmg, values);
729  }
730 
734  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
735 
745  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
746  {
747  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
748  if (jmg)
749  setJointGroupVelocities(jmg, gstate);
750  }
751 
755  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
756  {
757  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
758  if (jmg)
759  setJointGroupVelocities(jmg, &gstate[0]);
760  }
761 
765  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
766  {
767  setJointGroupVelocities(group, &gstate[0]);
768  }
769 
773  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
774 
778  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
779  {
780  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
781  if (jmg)
782  setJointGroupVelocities(jmg, values);
783  }
784 
788  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
789 
793  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
794  {
795  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
796  if (jmg)
797  {
798  gstate.resize(jmg->getVariableCount());
799  copyJointGroupVelocities(jmg, &gstate[0]);
800  }
801  }
802 
806  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
807  {
808  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
809  if (jmg)
810  copyJointGroupVelocities(jmg, gstate);
811  }
812 
816  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
817  {
818  gstate.resize(group->getVariableCount());
819  copyJointGroupVelocities(group, &gstate[0]);
820  }
821 
825  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
826 
830  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
831  {
832  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
833  if (jmg)
834  copyJointGroupVelocities(jmg, values);
835  }
836 
840  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
841 
851  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
852  {
853  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
854  if (jmg)
855  setJointGroupAccelerations(jmg, gstate);
856  }
857 
861  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
862  {
863  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
864  if (jmg)
865  setJointGroupAccelerations(jmg, &gstate[0]);
866  }
867 
871  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
872  {
873  setJointGroupAccelerations(group, &gstate[0]);
874  }
875 
879  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
880 
884  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
885  {
886  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
887  if (jmg)
888  setJointGroupAccelerations(jmg, values);
889  }
890 
894  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
895 
899  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
900  {
901  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
902  if (jmg)
903  {
904  gstate.resize(jmg->getVariableCount());
905  copyJointGroupAccelerations(jmg, &gstate[0]);
906  }
907  }
908 
912  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
913  {
914  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
915  if (jmg)
916  copyJointGroupAccelerations(jmg, gstate);
917  }
918 
922  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
923  {
924  gstate.resize(group->getVariableCount());
925  copyJointGroupAccelerations(group, &gstate[0]);
926  }
927 
931  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
932 
936  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
937  {
938  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
939  if (jmg)
940  copyJointGroupAccelerations(jmg, values);
941  }
942 
946  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
947 
960  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
961 
968  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
969 
976  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
979  [[deprecated("The attempts argument is not supported anymore.")]] bool
980  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int /*attempts*/,
981  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
983  {
984  return setFromIK(group, pose, timeout, constraint, options);
985  }
986 
994  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
995  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
997  [[deprecated("The attempts argument is not supported anymore.")]] bool
998  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
999  unsigned int /*attempts*/, double timeout = 0.0,
1002  {
1003  return setFromIK(group, pose, tip, timeout, constraint, options);
1004  }
1005 
1012  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
1015  [[deprecated("The attempts argument is not supported anymore.")]] bool
1016  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int /*attempts*/,
1017  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1019  {
1020  return setFromIK(group, pose, timeout, constraint, options);
1021  }
1022 
1029  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1030  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1032  [[deprecated("The attempts argument is not supported anymore.")]] bool
1033  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1034  unsigned int /*attempts*/, double timeout = 0.0,
1037  {
1038  return setFromIK(group, pose, tip, timeout, constraint, options);
1039  }
1040 
1049  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1050  const std::vector<double>& consistency_limits, double timeout = 0.0,
1053  [[deprecated("The attempts argument is not supported anymore.")]] bool
1054  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1055  const std::vector<double>& consistency_limits, unsigned int /*attempts*/, double timeout = 0.0,
1058  {
1059  return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options);
1060  }
1061 
1071  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1072  const std::vector<std::string>& tips, double timeout = 0.0,
1075  [[deprecated("The attempts argument is not supported anymore.")]] bool
1077  const std::vector<std::string>& tips, unsigned int /*attempts*/, double timeout = 0.0,
1080  {
1081  return setFromIK(group, poses, tips, timeout, constraint, options);
1082  }
1083 
1094  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1095  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1096  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1098  [[deprecated("The attempts argument is not supported anymore.")]] bool
1100  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1101  unsigned int /*attempts*/, double timeout = 0.0,
1104  {
1105  return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options);
1106  }
1107 
1116  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1117  const std::vector<std::string>& tips,
1118  const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
1121  [[deprecated("The attempts argument is not supported anymore.")]] bool
1123  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1124  unsigned int /*attempts*/, double timeout = 0.0,
1127  {
1128  return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options);
1129  }
1130 
1138  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1140 
1148  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1150 
1181  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1182  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1183  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1186 
1187  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1188  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1189  double max_step, double jump_threshold_factor,
1192  {
1193  return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance,
1194  MaxEEFStep(max_step, max_step), JumpThreshold(jump_threshold_factor), validCallback,
1195  options);
1196  }
1197 
1204  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1205  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
1206  const JumpThreshold& jump_threshold,
1209 
1210  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1211  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1212  double jump_threshold_factor,
1215  {
1216  return computeCartesianPath(group, traj, link, target, global_reference_frame, MaxEEFStep(max_step),
1217  JumpThreshold(jump_threshold_factor), validCallback, options);
1218  }
1219 
1226  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1227  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1228  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1231 
1232  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1233  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1234  double max_step, double jump_threshold_factor,
1237  {
1238  return computeCartesianPath(group, traj, link, waypoints, global_reference_frame, MaxEEFStep(max_step),
1239  JumpThreshold(jump_threshold_factor), validCallback, options);
1240  }
1241 
1257  static double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1258  const JumpThreshold& jump_threshold);
1259 
1270  static double testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1271  double jump_threshold_factor);
1272 
1285  static double testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1286  double revolute_jump_threshold, double prismatic_jump_threshold);
1287 
1297  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1298  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1299 
1309  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1310  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1311  {
1312  updateLinkTransforms();
1313  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1314  use_quaternion_representation);
1315  }
1316 
1323  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1324  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1325 
1332  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1333  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1334  {
1335  updateLinkTransforms();
1336  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1337  }
1338 
1341  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1342  const LinkModel* tip) const;
1343 
1346  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1347  const LinkModel* tip)
1348  {
1349  updateLinkTransforms();
1350  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1351  }
1352 
1356  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1358 
1365  void setVariableValues(const sensor_msgs::JointState& msg)
1366  {
1367  if (!msg.position.empty())
1368  setVariablePositions(msg.name, msg.position);
1369  if (!msg.velocity.empty())
1370  setVariableVelocities(msg.name, msg.velocity);
1371  }
1372 
1376  void setToDefaultValues();
1377 
1379  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1380 
1382  void setToRandomPositions();
1383 
1385  void setToRandomPositions(const JointModelGroup* group);
1386 
1389  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1390 
1396  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1397 
1405  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1406  const std::vector<double>& distances);
1407 
1416  void updateCollisionBodyTransforms();
1417 
1420  void updateLinkTransforms();
1421 
1423  void update(bool force = false);
1424 
1433  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1434  {
1435  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1436  }
1437 
1439  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1440 
1441  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1442  {
1443  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1444  }
1445 
1446  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1447  {
1448  updateLinkTransforms();
1449  return global_link_transforms_[link->getLinkIndex()];
1450  }
1451 
1452  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1453  {
1454  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1455  }
1456 
1457  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1458  {
1459  updateCollisionBodyTransforms();
1460  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1461  }
1462 
1463  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1464  {
1465  return getJointTransform(robot_model_->getJointModel(joint_name));
1466  }
1467 
1468  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1469  {
1470  const int idx = joint->getJointIndex();
1471  unsigned char& dirty = dirty_joint_transforms_[idx];
1472  if (dirty)
1473  {
1474  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1475  dirty = 0;
1476  }
1477  return variable_joint_transforms_[idx];
1478  }
1479 
1480  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1481  {
1482  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1483  }
1484 
1485  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1486  {
1487  BOOST_VERIFY(checkLinkTransforms());
1488  return global_link_transforms_[link->getLinkIndex()];
1489  }
1490 
1491  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1492  {
1493  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1494  }
1495 
1496  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1497  {
1498  BOOST_VERIFY(checkCollisionTransforms());
1499  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1500  }
1501 
1502  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1503  {
1504  return getJointTransform(robot_model_->getJointModel(joint_name));
1505  }
1506 
1507  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1508  {
1509  BOOST_VERIFY(checkJointTransforms(joint));
1510  return variable_joint_transforms_[joint->getJointIndex()];
1511  }
1512 
1513  bool dirtyJointTransform(const JointModel* joint) const
1514  {
1515  return dirty_joint_transforms_[joint->getJointIndex()];
1516  }
1517 
1518  bool dirtyLinkTransforms() const
1519  {
1520  return dirty_link_transforms_;
1521  }
1522 
1524  {
1525  return dirty_link_transforms_ || dirty_collision_body_transforms_;
1526  }
1527 
1529  bool dirty() const
1530  {
1531  return dirtyCollisionBodyTransforms();
1532  }
1533 
1540  double distance(const RobotState& other) const
1541  {
1542  return robot_model_->distance(position_, other.getVariablePositions());
1543  }
1544 
1545  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1546 
1547  double distance(const RobotState& other, const JointModel* joint) const
1548  {
1549  const int idx = joint->getFirstVariableIndex();
1550  return joint->distance(position_ + idx, other.position_ + idx);
1551  }
1552 
1556  void interpolate(const RobotState& to, double t, RobotState& state) const;
1557 
1561  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1562 
1566  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1567  {
1568  const int idx = joint->getFirstVariableIndex();
1569  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1570  state.markDirtyJointTransforms(joint);
1571  state.updateMimicJoint(joint);
1572  }
1573 
1574  void enforceBounds();
1575  void enforceBounds(const JointModelGroup* joint_group);
1576  void enforceBounds(const JointModel* joint)
1577  {
1578  enforcePositionBounds(joint);
1579  if (has_velocity_)
1580  enforceVelocityBounds(joint);
1581  }
1583  {
1584  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1585  {
1586  markDirtyJointTransforms(joint);
1587  updateMimicJoint(joint);
1588  }
1589  }
1590 
1592  void harmonizePositions();
1593  void harmonizePositions(const JointModelGroup* joint_group);
1594  void harmonizePosition(const JointModel* joint)
1595  {
1596  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1597  // no need to mark transforms dirty, as the transform hasn't changed
1598  updateMimicJoint(joint);
1599  }
1600 
1602  {
1603  joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
1604  }
1605 
1606  bool satisfiesBounds(double margin = 0.0) const;
1607  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1608  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1609  {
1610  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1611  }
1612  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1613  {
1614  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1615  }
1616  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1617  {
1618  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1619  }
1620 
1623  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1624 
1627  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1628 
1631  std::pair<double, const JointModel*>
1632  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1633 
1640  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1641 
1667  void attachBody(AttachedBody* attached_body);
1668 
1683  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1684  const EigenSTL::vector_Isometry3d& attach_trans, const std::set<std::string>& touch_links,
1685  const std::string& link_name,
1686  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1687 
1702  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1703  const EigenSTL::vector_Isometry3d& attach_trans, const std::vector<std::string>& touch_links,
1704  const std::string& link_name,
1705  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1706  {
1707  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1708  attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1709  }
1710 
1712  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1713 
1715  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
1716 
1718  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
1719 
1722  bool clearAttachedBody(const std::string& id);
1723 
1725  void clearAttachedBodies(const LinkModel* link);
1726 
1728  void clearAttachedBodies(const JointModelGroup* group);
1729 
1731  void clearAttachedBodies();
1732 
1734  const AttachedBody* getAttachedBody(const std::string& name) const;
1735 
1737  bool hasAttachedBody(const std::string& id) const;
1738 
1739  void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback);
1744  void computeAABB(std::vector<double>& aabb) const;
1745 
1748  void computeAABB(std::vector<double>& aabb)
1749  {
1750  updateLinkTransforms();
1751  static_cast<const RobotState*>(this)->computeAABB(aabb);
1752  }
1753 
1756  {
1757  if (!rng_)
1759  return *rng_;
1760  }
1761 
1763  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id);
1764 
1766  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id) const;
1767 
1769  bool knowsFrameTransform(const std::string& frame_id) const;
1770 
1778  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1779  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1780  bool include_attached = false) const;
1781 
1789  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1790  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1791  bool include_attached = false)
1792  {
1793  updateCollisionBodyTransforms();
1794  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1795  }
1796 
1801  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1802  bool include_attached = false) const;
1803 
1808  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1809  bool include_attached = false)
1810  {
1811  updateCollisionBodyTransforms();
1812  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1813  }
1814 
1815  void printStatePositions(std::ostream& out = std::cout) const;
1816 
1818  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const;
1819 
1820  void printStateInfo(std::ostream& out = std::cout) const;
1821 
1822  void printTransforms(std::ostream& out = std::cout) const;
1823 
1824  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1825 
1826  void printDirtyInfo(std::ostream& out = std::cout) const;
1827 
1828  std::string getStateTreeString(const std::string& prefix = "") const;
1829 
1830 private:
1831  void allocMemory();
1832  void initTransforms();
1833  void copyFrom(const RobotState& other);
1834 
1836  {
1837  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1838  dirty_link_transforms_ =
1839  dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1840  }
1841 
1843  {
1844  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
1845  for (std::size_t i = 0; i < jm.size(); ++i)
1846  dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1847  dirty_link_transforms_ = dirty_link_transforms_ == NULL ?
1848  group->getCommonRoot() :
1849  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1850  }
1851 
1852  void markVelocity();
1853  void markAcceleration();
1854  void markEffort();
1855 
1856  void updateMimicJoint(const JointModel* joint)
1857  {
1858  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
1859  double v = position_[joint->getFirstVariableIndex()];
1860  for (std::size_t i = 0; i < mim.size(); ++i)
1861  {
1862  position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1863  markDirtyJointTransforms(mim[i]);
1864  }
1865  }
1866 
1868  /* use updateMimicJoints() instead, which also marks joints dirty */
1869  [[deprecated]] void updateMimicJoint(const std::vector<const JointModel*>& mim)
1870  {
1871  for (std::size_t i = 0; i < mim.size(); ++i)
1872  {
1873  const int fvi = mim[i]->getFirstVariableIndex();
1874  position_[fvi] =
1875  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1876  // Only mark joint transform dirty, but not the associated link transform
1877  // as this function is always used in combination of
1878  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1879  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1880  }
1881  }
1882 
1885  {
1886  for (const JointModel* jm : group->getMimicJointModels())
1887  {
1888  const int fvi = jm->getFirstVariableIndex();
1889  position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
1890  markDirtyJointTransforms(jm);
1891  }
1892  markDirtyJointTransforms(group);
1893  }
1894 
1895  void updateLinkTransformsInternal(const JointModel* start);
1896 
1897  void getMissingKeys(const std::map<std::string, double>& variable_map,
1898  std::vector<std::string>& missing_variables) const;
1899  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1900 
1902  bool checkJointTransforms(const JointModel* joint) const;
1903 
1905  bool checkLinkTransforms() const;
1906 
1908  bool checkCollisionTransforms() const;
1909 
1910  RobotModelConstPtr robot_model_;
1911  void* memory_;
1912 
1913  double* position_;
1914  double* velocity_;
1915  double* acceleration_;
1916  double* effort_;
1920 
1923 
1924  Eigen::Isometry3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned
1925  Eigen::Isometry3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned
1926  Eigen::Isometry3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned
1927  unsigned char* dirty_joint_transforms_;
1928 
1930  std::map<std::string, AttachedBody*> attached_body_map_;
1931 
1935 
1942 };
1943 
1945 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1946 } // namespace core
1947 } // namespace moveit
1948 
1949 #endif
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:548
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Definition: robot_state.h:1452
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:329
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1755
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:1924
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:1748
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1856
const double * getJointEffort(const JointModel *joint) const
Definition: robot_state.h:625
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:816
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:912
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:1808
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:649
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:851
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:240
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:277
ROSCPP_DECL void start()
void harmonizePosition(const JointModel *joint)
Definition: robot_state.h:1594
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:600
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:1702
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:1884
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:461
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:262
const double * getJointVelocities(const JointModel *joint) const
Definition: robot_state.h:605
std::vector< double > values
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1513
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1582
const double * getJointAccelerations(const JointModel *joint) const
Definition: robot_state.h:615
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:830
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:1232
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:1016
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:286
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1922
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:998
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1463
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1601
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:200
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:1457
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:1033
void setJointPositions(const JointModel *joint, const double *position)
Definition: robot_state.h:563
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:899
Eigen::Isometry3d * global_link_transforms_
Definition: robot_state.h:1925
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:745
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name) const
Definition: robot_state.h:1480
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:1566
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:180
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:421
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:1099
geometry_msgs::TransformStamped t
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1835
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:512
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:659
bool dirtyLinkTransforms() const
Definition: robot_state.h:1518
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
Definition: robot_state.h:553
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:167
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:1332
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link) const
Definition: robot_state.h:1485
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:428
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:793
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:765
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:505
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:620
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
Definition: robot_state.h:1491
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:755
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:610
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1612
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:861
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:487
void enforceBounds(const JointModel *joint)
Definition: robot_state.h:1576
const Eigen::Isometry3d & getJointTransform(const JointModel *joint)
Definition: robot_state.h:1468
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
Definition: robot_state.h:1496
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:980
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:724
MOVEIT_CLASS_FORWARD(RobotModel)
MaxEEFStep(double translation=0.0, double rotation=0.0)
Definition: robot_state.h:97
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
Definition: robot_state.h:558
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:806
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:392
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:778
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:355
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:1309
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:1927
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:1529
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name) const
Definition: robot_state.h:1502
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:1934
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:884
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:137
Struct for containing max_step for computeCartesianPath.
Definition: robot_state.h:95
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:1210
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:337
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:469
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:672
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:1789
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:700
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:1433
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:687
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:1122
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:1842
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:269
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1523
void setJointPositions(const std::string &joint_name, const Eigen::Isometry3d &transform)
Definition: robot_state.h:570
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:936
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:1054
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:322
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:155
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:1346
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:519
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1921
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:143
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:363
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:922
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1441
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:639
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1941
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
Definition: robot_state.h:187
RobotModelConstPtr robot_model_
Definition: robot_state.h:1910
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1365
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:381
Eigen::Isometry3d * global_collision_body_transforms_
Definition: robot_state.h:1926
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:527
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
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:414
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1616
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:478
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:453
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:1608
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:871
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:221
Main namespace for MoveIt!
double distance(const RobotState &other, const JointModel *joint) const
Definition: robot_state.h:1547
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:248
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:436
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:161
const Eigen::Isometry3d & getJointTransform(const JointModel *joint) const
Definition: robot_state.h:1507
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:1076
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:149
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:228
double distance(const RobotState &other) const
Definition: robot_state.h:1540
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:294
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:315
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:575
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:710
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:1187
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:590
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:1930
const double * getJointPositions(const JointModel *joint) const
Definition: robot_state.h:595
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link)
Definition: robot_state.h:1446
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:371
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:582
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:1869


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 23 2020 03:52:30