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);
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 setVariableVelocities(const double* velocity)
284  {
285  has_velocity_ = true;
286  // assume everything is in order in terms of array lengths (for efficiency reasons)
287  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
288  }
289 
291  void setVariableVelocities(const std::vector<double>& velocity)
292  {
293  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
294  setVariableVelocities(&velocity[0]);
295  }
296 
299  void setVariableVelocities(const std::map<std::string, double>& variable_map);
300 
303  void setVariableVelocities(const std::map<std::string, double>& variable_map,
304  std::vector<std::string>& missing_variables);
305 
308  void setVariableVelocities(const std::vector<std::string>& variable_names,
309  const std::vector<double>& variable_velocity);
310 
312  void setVariableVelocity(const std::string& variable, double value)
313  {
314  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
315  }
316 
319  void setVariableVelocity(int index, double value)
320  {
321  markVelocity();
322  velocity_[index] = value;
323  }
324 
326  double getVariableVelocity(const std::string& variable) const
327  {
328  return velocity_[robot_model_->getVariableIndex(variable)];
329  }
330 
334  double getVariableVelocity(int index) const
335  {
336  return velocity_[index];
337  }
338 
349  bool hasAccelerations() const
350  {
351  return has_acceleration_;
352  }
353 
358  {
359  markAcceleration();
360  return acceleration_;
361  }
362 
365  const double* getVariableAccelerations() const
366  {
367  return acceleration_;
368  }
369 
372  void setVariableAccelerations(const double* acceleration)
373  {
374  has_acceleration_ = true;
375  has_effort_ = false;
376 
377  // assume everything is in order in terms of array lengths (for efficiency reasons)
378  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
379  }
380 
383  void setVariableAccelerations(const std::vector<double>& acceleration)
384  {
385  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
386  setVariableAccelerations(&acceleration[0]);
387  }
388 
391  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
392 
396  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
397  std::vector<std::string>& missing_variables);
398 
401  void setVariableAccelerations(const std::vector<std::string>& variable_names,
402  const std::vector<double>& variable_acceleration);
403 
405  void setVariableAcceleration(const std::string& variable, double value)
406  {
407  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
408  }
409 
412  void setVariableAcceleration(int index, double value)
413  {
414  markAcceleration();
415  acceleration_[index] = value;
416  }
417 
419  double getVariableAcceleration(const std::string& variable) const
420  {
421  return acceleration_[robot_model_->getVariableIndex(variable)];
422  }
423 
427  double getVariableAcceleration(int index) const
428  {
429  return acceleration_[index];
430  }
431 
441  bool hasEffort() const
442  {
443  return has_effort_;
444  }
445 
450  {
451  markEffort();
452  return effort_;
453  }
454 
457  const double* getVariableEffort() const
458  {
459  return effort_;
460  }
461 
463  void setVariableEffort(const double* effort)
464  {
465  has_effort_ = true;
466  has_acceleration_ = false;
467  // assume everything is in order in terms of array lengths (for efficiency reasons)
468  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
469  }
470 
472  void setVariableEffort(const std::vector<double>& effort)
473  {
474  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
475  setVariableEffort(&effort[0]);
476  }
477 
479  void setVariableEffort(const std::map<std::string, double>& variable_map);
480 
483  void setVariableEffort(const std::map<std::string, double>& variable_map,
484  std::vector<std::string>& missing_variables);
485 
487  void setVariableEffort(const std::vector<std::string>& variable_names,
488  const std::vector<double>& variable_acceleration);
489 
491  void setVariableEffort(const std::string& variable, double value)
492  {
493  setVariableEffort(robot_model_->getVariableIndex(variable), value);
494  }
495 
498  void setVariableEffort(int index, double value)
499  {
500  markEffort();
501  effort_[index] = value;
502  }
503 
505  double getVariableEffort(const std::string& variable) const
506  {
507  return effort_[robot_model_->getVariableIndex(variable)];
508  }
509 
513  double getVariableEffort(int index) const
514  {
515  return effort_[index];
516  }
517 
519  void invertVelocity();
520 
526  void setJointPositions(const std::string& joint_name, const double* position)
527  {
528  setJointPositions(robot_model_->getJointModel(joint_name), position);
529  }
530 
531  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
532  {
533  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
534  }
535 
536  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
537  {
538  setJointPositions(joint, &position[0]);
539  }
540 
541  void setJointPositions(const JointModel* joint, const double* position)
542  {
543  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
544  markDirtyJointTransforms(joint);
545  updateMimicJoint(joint);
546  }
547 
548  void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
549  {
550  setJointPositions(robot_model_->getJointModel(joint_name), transform);
551  }
552 
553  void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform)
554  {
555  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
556  markDirtyJointTransforms(joint);
557  updateMimicJoint(joint);
558  }
559 
560  void setJointVelocities(const JointModel* joint, const double* velocity)
561  {
562  has_velocity_ = true;
563  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
564  }
565 
566  void setJointEfforts(const JointModel* joint, const double* effort);
567 
568  const double* getJointPositions(const std::string& joint_name) const
569  {
570  return getJointPositions(robot_model_->getJointModel(joint_name));
571  }
572 
573  const double* getJointPositions(const JointModel* joint) const
574  {
575  return position_ + joint->getFirstVariableIndex();
576  }
577 
578  const double* getJointVelocities(const std::string& joint_name) const
579  {
580  return getJointVelocities(robot_model_->getJointModel(joint_name));
581  }
582 
583  const double* getJointVelocities(const JointModel* joint) const
584  {
585  return velocity_ + joint->getFirstVariableIndex();
586  }
587 
588  const double* getJointAccelerations(const std::string& joint_name) const
589  {
590  return getJointAccelerations(robot_model_->getJointModel(joint_name));
591  }
592 
593  const double* getJointAccelerations(const JointModel* joint) const
594  {
595  return acceleration_ + joint->getFirstVariableIndex();
596  }
597 
598  const double* getJointEffort(const std::string& joint_name) const
599  {
600  return getJointEffort(robot_model_->getJointModel(joint_name));
601  }
602 
603  const double* getJointEffort(const JointModel* joint) const
604  {
605  return effort_ + joint->getFirstVariableIndex();
606  }
607 
617  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
618  {
619  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
620  if (jmg)
621  setJointGroupPositions(jmg, gstate);
622  }
623 
627  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
628  {
629  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
630  if (jmg)
631  setJointGroupPositions(jmg, &gstate[0]);
632  }
633 
637  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
638  {
639  setJointGroupPositions(group, &gstate[0]);
640  }
641 
645  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
646 
650  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
651  {
652  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
653  if (jmg)
654  setJointGroupPositions(jmg, values);
655  }
656 
660  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
661 
665  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
666  {
667  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
668  if (jmg)
669  {
670  gstate.resize(jmg->getVariableCount());
671  copyJointGroupPositions(jmg, &gstate[0]);
672  }
673  }
674 
678  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
679  {
680  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
681  if (jmg)
682  copyJointGroupPositions(jmg, gstate);
683  }
684 
688  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
689  {
690  gstate.resize(group->getVariableCount());
691  copyJointGroupPositions(group, &gstate[0]);
692  }
693 
697  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
698 
702  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
703  {
704  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
705  if (jmg)
706  copyJointGroupPositions(jmg, values);
707  }
708 
712  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
713 
723  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
724  {
725  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
726  if (jmg)
727  setJointGroupVelocities(jmg, gstate);
728  }
729 
733  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
734  {
735  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
736  if (jmg)
737  setJointGroupVelocities(jmg, &gstate[0]);
738  }
739 
743  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
744  {
745  setJointGroupVelocities(group, &gstate[0]);
746  }
747 
751  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
752 
756  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
757  {
758  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
759  if (jmg)
760  setJointGroupVelocities(jmg, values);
761  }
762 
766  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
767 
771  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
772  {
773  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
774  if (jmg)
775  {
776  gstate.resize(jmg->getVariableCount());
777  copyJointGroupVelocities(jmg, &gstate[0]);
778  }
779  }
780 
784  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
785  {
786  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
787  if (jmg)
788  copyJointGroupVelocities(jmg, gstate);
789  }
790 
794  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
795  {
796  gstate.resize(group->getVariableCount());
797  copyJointGroupVelocities(group, &gstate[0]);
798  }
799 
803  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
804 
808  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
809  {
810  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
811  if (jmg)
812  copyJointGroupVelocities(jmg, values);
813  }
814 
818  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
819 
829  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
830  {
831  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
832  if (jmg)
833  setJointGroupAccelerations(jmg, gstate);
834  }
835 
839  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
840  {
841  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
842  if (jmg)
843  setJointGroupAccelerations(jmg, &gstate[0]);
844  }
845 
849  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
850  {
851  setJointGroupAccelerations(group, &gstate[0]);
852  }
853 
857  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
858 
862  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
863  {
864  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
865  if (jmg)
866  setJointGroupAccelerations(jmg, values);
867  }
868 
872  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
873 
877  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
878  {
879  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
880  if (jmg)
881  {
882  gstate.resize(jmg->getVariableCount());
883  copyJointGroupAccelerations(jmg, &gstate[0]);
884  }
885  }
886 
890  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
891  {
892  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
893  if (jmg)
894  copyJointGroupAccelerations(jmg, gstate);
895  }
896 
900  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
901  {
902  gstate.resize(group->getVariableCount());
903  copyJointGroupAccelerations(group, &gstate[0]);
904  }
905 
909  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
910 
914  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
915  {
916  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
917  if (jmg)
918  copyJointGroupAccelerations(jmg, values);
919  }
920 
924  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
925 
938  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
939 
946  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
947 
954  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
957  [[deprecated("The attempts argument is not supported anymore.")]] bool
958  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts, double timeout = 0.0,
961  {
962  return setFromIK(group, pose, timeout, constraint, options);
963  }
964 
972  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
973  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
975  [[deprecated("The attempts argument is not supported anymore.")]] bool
976  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
977  unsigned int attempts, double timeout = 0.0,
980  {
981  return setFromIK(group, pose, tip, timeout, constraint, options);
982  }
983 
990  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
993  [[deprecated("The attempts argument is not supported anymore.")]] bool
994  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int attempts, double timeout = 0.0,
997  {
998  return setFromIK(group, pose, timeout, constraint, options);
999  }
1000 
1007  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1008  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1010  [[deprecated("The attempts argument is not supported anymore.")]] bool
1011  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip, unsigned int attempts,
1012  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1014  {
1015  return setFromIK(group, pose, tip, timeout, constraint, options);
1016  }
1017 
1026  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1027  const std::vector<double>& consistency_limits, double timeout = 0.0,
1030  [[deprecated("The attempts argument is not supported anymore.")]] bool
1031  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1032  const std::vector<double>& consistency_limits, unsigned int attempts, double timeout = 0.0,
1035  {
1036  return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options);
1037  }
1038 
1048  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1049  const std::vector<std::string>& tips, double timeout = 0.0,
1052  [[deprecated("The attempts argument is not supported anymore.")]] bool
1054  const std::vector<std::string>& tips, unsigned int attempts, double timeout = 0.0,
1057  {
1058  return setFromIK(group, poses, tips, timeout, constraint, options);
1059  }
1060 
1071  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1072  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1073  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1075  [[deprecated("The attempts argument is not supported anymore.")]] bool
1077  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1078  unsigned int attempts, double timeout = 0.0,
1081  {
1082  return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options);
1083  }
1084 
1093  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1094  const std::vector<std::string>& tips,
1095  const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
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 setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options);
1106  }
1107 
1115  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1117 
1125  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1127 
1158  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1159  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1160  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1163 
1164  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1165  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1166  double max_step, double jump_threshold_factor,
1169  {
1170  return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance,
1171  MaxEEFStep(max_step, max_step), JumpThreshold(jump_threshold_factor), validCallback,
1172  options);
1173  }
1174 
1181  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1182  const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
1183  const JumpThreshold& jump_threshold,
1186 
1187  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1188  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1189  double jump_threshold_factor,
1192  {
1193  return computeCartesianPath(group, traj, link, target, global_reference_frame, MaxEEFStep(max_step),
1194  JumpThreshold(jump_threshold_factor), validCallback, options);
1195  }
1196 
1203  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1204  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1205  const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
1208 
1209  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1210  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
1211  double max_step, double jump_threshold_factor,
1214  {
1215  return computeCartesianPath(group, traj, link, waypoints, global_reference_frame, MaxEEFStep(max_step),
1216  JumpThreshold(jump_threshold_factor), validCallback, options);
1217  }
1218 
1234  static double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1235  const JumpThreshold& jump_threshold);
1236 
1247  static double testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1248  double jump_threshold_factor);
1249 
1262  static double testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1263  double revolute_jump_threshold, double prismatic_jump_threshold);
1264 
1274  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1275  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1276 
1286  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1287  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1288  {
1289  updateLinkTransforms();
1290  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1291  use_quaternion_representation);
1292  }
1293 
1300  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1301  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1302 
1309  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1310  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1311  {
1312  updateLinkTransforms();
1313  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1314  }
1315 
1318  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1319  const LinkModel* tip) const;
1320 
1323  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1324  const LinkModel* tip)
1325  {
1326  updateLinkTransforms();
1327  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1328  }
1329 
1333  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1335 
1342  void setVariableValues(const sensor_msgs::JointState& msg)
1343  {
1344  if (!msg.position.empty())
1345  setVariablePositions(msg.name, msg.position);
1346  if (!msg.velocity.empty())
1347  setVariableVelocities(msg.name, msg.velocity);
1348  }
1349 
1353  void setToDefaultValues();
1354 
1356  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1357 
1359  void setToRandomPositions();
1360 
1362  void setToRandomPositions(const JointModelGroup* group);
1363 
1366  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1367 
1373  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1374 
1382  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1383  const std::vector<double>& distances);
1384 
1393  void updateCollisionBodyTransforms();
1394 
1397  void updateLinkTransforms();
1398 
1400  void update(bool force = false);
1401 
1410  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1411  {
1412  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1413  }
1414 
1416  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1417 
1418  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1419  {
1420  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1421  }
1422 
1423  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1424  {
1425  updateLinkTransforms();
1426  return global_link_transforms_[link->getLinkIndex()];
1427  }
1428 
1429  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1430  {
1431  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1432  }
1433 
1434  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1435  {
1436  updateCollisionBodyTransforms();
1437  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1438  }
1439 
1440  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1441  {
1442  return getJointTransform(robot_model_->getJointModel(joint_name));
1443  }
1444 
1445  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1446  {
1447  const int idx = joint->getJointIndex();
1448  unsigned char& dirty = dirty_joint_transforms_[idx];
1449  if (dirty)
1450  {
1451  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1452  dirty = 0;
1453  }
1454  return variable_joint_transforms_[idx];
1455  }
1456 
1457  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1458  {
1459  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1460  }
1461 
1462  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1463  {
1464  BOOST_VERIFY(checkLinkTransforms());
1465  return global_link_transforms_[link->getLinkIndex()];
1466  }
1467 
1468  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1469  {
1470  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1471  }
1472 
1473  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1474  {
1475  BOOST_VERIFY(checkCollisionTransforms());
1476  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1477  }
1478 
1479  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1480  {
1481  return getJointTransform(robot_model_->getJointModel(joint_name));
1482  }
1483 
1484  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1485  {
1486  BOOST_VERIFY(checkJointTransforms(joint));
1487  return variable_joint_transforms_[joint->getJointIndex()];
1488  }
1489 
1490  bool dirtyJointTransform(const JointModel* joint) const
1491  {
1492  return dirty_joint_transforms_[joint->getJointIndex()];
1493  }
1494 
1495  bool dirtyLinkTransforms() const
1496  {
1497  return dirty_link_transforms_;
1498  }
1499 
1501  {
1502  return dirty_link_transforms_ || dirty_collision_body_transforms_;
1503  }
1504 
1506  bool dirty() const
1507  {
1508  return dirtyCollisionBodyTransforms();
1509  }
1510 
1517  double distance(const RobotState& other) const
1518  {
1519  return robot_model_->distance(position_, other.getVariablePositions());
1520  }
1521 
1522  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1523 
1524  double distance(const RobotState& other, const JointModel* joint) const
1525  {
1526  const int idx = joint->getFirstVariableIndex();
1527  return joint->distance(position_ + idx, other.position_ + idx);
1528  }
1529 
1533  void interpolate(const RobotState& to, double t, RobotState& state) const;
1534 
1538  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1539 
1543  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1544  {
1545  const int idx = joint->getFirstVariableIndex();
1546  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1547  state.markDirtyJointTransforms(joint);
1548  state.updateMimicJoint(joint);
1549  }
1550 
1551  void enforceBounds();
1552  void enforceBounds(const JointModelGroup* joint_group);
1553  void enforceBounds(const JointModel* joint)
1554  {
1555  enforcePositionBounds(joint);
1556  if (has_velocity_)
1557  enforceVelocityBounds(joint);
1558  }
1560  {
1561  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1562  {
1563  markDirtyJointTransforms(joint);
1564  updateMimicJoint(joint);
1565  }
1566  }
1567 
1569  void harmonizePositions();
1570  void harmonizePositions(const JointModelGroup* joint_group);
1571  void harmonizePosition(const JointModel* joint)
1572  {
1573  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1574  // no need to mark transforms dirty, as the transform hasn't changed
1575  updateMimicJoint(joint);
1576  }
1577 
1579  {
1580  joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
1581  }
1582 
1583  bool satisfiesBounds(double margin = 0.0) const;
1584  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1585  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1586  {
1587  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1588  }
1589  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1590  {
1591  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1592  }
1593  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1594  {
1595  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1596  }
1597 
1600  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1601 
1604  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1605 
1608  std::pair<double, const JointModel*>
1609  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1610 
1617  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1618 
1644  void attachBody(AttachedBody* attached_body);
1645 
1660  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1661  const EigenSTL::vector_Isometry3d& attach_trans, const std::set<std::string>& touch_links,
1662  const std::string& link_name,
1663  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1664 
1679  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1680  const EigenSTL::vector_Isometry3d& attach_trans, const std::vector<std::string>& touch_links,
1681  const std::string& link_name,
1682  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1683  {
1684  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1685  attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1686  }
1687 
1689  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1690 
1692  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
1693 
1695  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
1696 
1699  bool clearAttachedBody(const std::string& id);
1700 
1702  void clearAttachedBodies(const LinkModel* link);
1703 
1705  void clearAttachedBodies(const JointModelGroup* group);
1706 
1708  void clearAttachedBodies();
1709 
1711  const AttachedBody* getAttachedBody(const std::string& name) const;
1712 
1714  bool hasAttachedBody(const std::string& id) const;
1715 
1716  void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback);
1721  void computeAABB(std::vector<double>& aabb) const;
1722 
1725  void computeAABB(std::vector<double>& aabb)
1726  {
1727  updateLinkTransforms();
1728  static_cast<const RobotState*>(this)->computeAABB(aabb);
1729  }
1730 
1733  {
1734  if (!rng_)
1736  return *rng_;
1737  }
1738 
1740  const Eigen::Isometry3d& getFrameTransform(const std::string& id);
1741 
1743  const Eigen::Isometry3d& getFrameTransform(const std::string& id) const;
1744 
1746  bool knowsFrameTransform(const std::string& id) const;
1747 
1755  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1756  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1757  bool include_attached = false) const;
1758 
1766  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1767  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1768  bool include_attached = false)
1769  {
1770  updateCollisionBodyTransforms();
1771  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1772  }
1773 
1778  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1779  bool include_attached = false) const;
1780 
1785  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1786  bool include_attached = false)
1787  {
1788  updateCollisionBodyTransforms();
1789  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1790  }
1791 
1792  void printStatePositions(std::ostream& out = std::cout) const;
1793 
1795  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg,
1796  std::ostream& out = std::cout) const;
1797 
1798  void printStateInfo(std::ostream& out = std::cout) const;
1799 
1800  void printTransforms(std::ostream& out = std::cout) const;
1801 
1802  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1803 
1804  void printDirtyInfo(std::ostream& out = std::cout) const;
1805 
1806  std::string getStateTreeString(const std::string& prefix = "") const;
1807 
1808 private:
1809  void allocMemory();
1810  void initTransforms();
1811  void copyFrom(const RobotState& other);
1812 
1814  {
1815  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1816  dirty_link_transforms_ =
1817  dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1818  }
1819 
1821  {
1822  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
1823  for (std::size_t i = 0; i < jm.size(); ++i)
1824  dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1825  dirty_link_transforms_ = dirty_link_transforms_ == NULL ?
1826  group->getCommonRoot() :
1827  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1828  }
1829 
1830  void markVelocity();
1831  void markAcceleration();
1832  void markEffort();
1833 
1834  void updateMimicJoint(const JointModel* joint)
1835  {
1836  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
1837  double v = position_[joint->getFirstVariableIndex()];
1838  for (std::size_t i = 0; i < mim.size(); ++i)
1839  {
1840  position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1841  markDirtyJointTransforms(mim[i]);
1842  }
1843  }
1844 
1846  /* use updateMimicJoints() instead, which also marks joints dirty */
1847  [[deprecated]] void updateMimicJoint(const std::vector<const JointModel*>& mim)
1848  {
1849  for (std::size_t i = 0; i < mim.size(); ++i)
1850  {
1851  const int fvi = mim[i]->getFirstVariableIndex();
1852  position_[fvi] =
1853  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1854  // Only mark joint transform dirty, but not the associated link transform
1855  // as this function is always used in combination of
1856  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1857  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1858  }
1859  }
1860 
1863  {
1864  for (const JointModel* jm : group->getMimicJointModels())
1865  {
1866  const int fvi = jm->getFirstVariableIndex();
1867  position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
1868  markDirtyJointTransforms(jm);
1869  }
1870  markDirtyJointTransforms(group);
1871  }
1872 
1873  void updateLinkTransformsInternal(const JointModel* start);
1874 
1875  void getMissingKeys(const std::map<std::string, double>& variable_map,
1876  std::vector<std::string>& missing_variables) const;
1877  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1878 
1880  bool checkJointTransforms(const JointModel* joint) const;
1881 
1883  bool checkLinkTransforms() const;
1884 
1886  bool checkCollisionTransforms() const;
1887 
1888  RobotModelConstPtr robot_model_;
1889  void* memory_;
1890 
1891  double* position_;
1892  double* velocity_;
1893  double* acceleration_;
1894  double* effort_;
1898 
1901 
1902  Eigen::Isometry3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned
1903  Eigen::Isometry3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned
1904  Eigen::Isometry3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned
1905  unsigned char* dirty_joint_transforms_;
1906 
1908  std::map<std::string, AttachedBody*> attached_body_map_;
1909 
1913 
1920 };
1921 
1923 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1924 }
1925 }
1926 
1927 #endif
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:526
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Definition: robot_state.h:1429
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:326
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1732
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:1902
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:1725
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1834
const double * getJointEffort(const JointModel *joint) const
Definition: robot_state.h:603
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1099
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:794
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:890
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:1785
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:627
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:829
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:1571
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:578
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:1679
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:1862
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:449
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, const std::string &tip, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:976
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:583
std::vector< double > values
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1490
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1559
const double * getJointAccelerations(const JointModel *joint) const
Definition: robot_state.h:593
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:808
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:1209
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Definition: link_model.h:87
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
Definition: robot_state.h:283
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:958
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1900
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1440
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1578
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:1434
void setJointPositions(const JointModel *joint, const double *position)
Definition: robot_state.h:541
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:877
Eigen::Isometry3d * global_link_transforms_
Definition: robot_state.h:1903
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:723
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name) const
Definition: robot_state.h:1457
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:1543
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:412
geometry_msgs::TransformStamped t
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1813
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:498
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:637
bool dirtyLinkTransforms() const
Definition: robot_state.h:1495
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:994
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
Definition: robot_state.h:531
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:1309
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link) const
Definition: robot_state.h:1462
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:419
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:771
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:743
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:491
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:598
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1011
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
Definition: robot_state.h:1468
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:733
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:588
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1589
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:839
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:472
void enforceBounds(const JointModel *joint)
Definition: robot_state.h:1553
const Eigen::Isometry3d & getJointTransform(const JointModel *joint)
Definition: robot_state.h:1445
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
Definition: robot_state.h:1473
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:702
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:536
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:784
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:383
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:756
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:349
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:1286
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:1905
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values.
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1076
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:1506
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name) const
Definition: robot_state.h:1479
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:1912
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:862
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1031
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:1187
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:334
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:457
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:650
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:1766
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:678
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:1410
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:665
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:1820
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:1500
void setJointPositions(const std::string &joint_name, const Eigen::Isometry3d &transform)
Definition: robot_state.h:548
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:914
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:319
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:1323
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:505
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1899
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:357
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:900
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1418
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:617
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1919
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:1888
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1342
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:372
Eigen::Isometry3d * global_collision_body_transforms_
Definition: robot_state.h:1904
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:513
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:405
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1593
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:463
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:441
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:1585
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:849
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:1524
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, unsigned int attempts, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1053
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:427
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:1484
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory...
Definition: robot_state.h: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:1517
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:291
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:312
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:553
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:688
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:1164
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:568
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:1908
const double * getJointPositions(const JointModel *joint) const
Definition: robot_state.h:573
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link)
Definition: robot_state.h:1423
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:365
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:560
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:1847


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Dec 2 2019 03:56:59