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 #pragma once
39 
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 /* Terminology
52  * Model Frame: RobotModel's root frame == PlanningScene's planning frame
53  If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint.
54  Otherwise, it is the root link of the URDF model.
55  * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics
56 */
57 
58 namespace moveit
59 {
60 namespace core
61 {
62 MOVEIT_CLASS_FORWARD(RobotState); // Defines RobotStatePtr, ConstPtr, WeakPtr... etc
63 
68 typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
69  const double* joint_group_variable_values)>
71 
89 class RobotState
90 {
91 public:
94  RobotState(const RobotModelConstPtr& robot_model);
95  ~RobotState();
96 
98  RobotState(const RobotState& other);
99 
101  RobotState& operator=(const RobotState& other);
102 
104  const RobotModelConstPtr& getRobotModel() const
105  {
106  return robot_model_;
107  }
108 
110  std::size_t getVariableCount() const
111  {
112  return robot_model_->getVariableCount();
113  }
114 
116  const std::vector<std::string>& getVariableNames() const
117  {
118  return robot_model_->getVariableNames();
119  }
120 
122  const LinkModel* getLinkModel(const std::string& link) const
123  {
124  return robot_model_->getLinkModel(link);
125  }
126 
128  const JointModel* getJointModel(const std::string& joint) const
129  {
130  return robot_model_->getJointModel(joint);
131  }
132 
134  const JointModelGroup* getJointModelGroup(const std::string& group) const
135  {
136  return robot_model_->getJointModelGroup(group);
137  }
138 
147  double* getVariablePositions()
148  {
149  return position_;
150  }
151 
154  const double* getVariablePositions() const
155  {
156  return position_;
157  }
158 
162  void setVariablePositions(const double* position);
163 
167  void setVariablePositions(const std::vector<double>& position)
168  {
169  assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
170  setVariablePositions(&position[0]);
171  }
172 
175  void setVariablePositions(const std::map<std::string, double>& variable_map);
176 
179  void setVariablePositions(const std::map<std::string, double>& variable_map,
180  std::vector<std::string>& missing_variables);
181 
184  void setVariablePositions(const std::vector<std::string>& variable_names,
185  const std::vector<double>& variable_position);
186 
188  void setVariablePosition(const std::string& variable, double value)
189  {
190  setVariablePosition(robot_model_->getVariableIndex(variable), value);
191  }
192 
195  void setVariablePosition(int index, double value)
196  {
197  position_[index] = value;
198  const JointModel* jm = robot_model_->getJointOfVariable(index);
199  if (jm)
200  {
202  updateMimicJoint(jm);
203  }
204  }
205 
207  double getVariablePosition(const std::string& variable) const
208  {
209  return position_[robot_model_->getVariableIndex(variable)];
210  }
211 
215  double getVariablePosition(int index) const
216  {
217  return position_[index];
218  }
219 
229  bool hasVelocities() const
230  {
231  return has_velocity_;
232  }
233 
236  double* getVariableVelocities()
237  {
238  markVelocity();
239  return velocity_;
240  }
241 
244  const double* getVariableVelocities() const
245  {
246  return velocity_;
247  }
248 
250  void zeroVelocities();
251 
253  void setVariableVelocities(const double* velocity)
254  {
255  has_velocity_ = true;
256  // assume everything is in order in terms of array lengths (for efficiency reasons)
257  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
258  }
259 
261  void setVariableVelocities(const std::vector<double>& velocity)
262  {
263  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
264  setVariableVelocities(&velocity[0]);
265  }
266 
269  void setVariableVelocities(const std::map<std::string, double>& variable_map);
270 
273  void setVariableVelocities(const std::map<std::string, double>& variable_map,
274  std::vector<std::string>& missing_variables);
275 
278  void setVariableVelocities(const std::vector<std::string>& variable_names,
279  const std::vector<double>& variable_velocity);
280 
282  void setVariableVelocity(const std::string& variable, double value)
283  {
284  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
285  }
286 
289  void setVariableVelocity(int index, double value)
290  {
291  markVelocity();
292  velocity_[index] = value;
293  }
294 
296  double getVariableVelocity(const std::string& variable) const
297  {
298  return velocity_[robot_model_->getVariableIndex(variable)];
299  }
300 
304  double getVariableVelocity(int index) const
305  {
306  return velocity_[index];
307  }
308 
311 
322  bool hasAccelerations() const
323  {
324  return has_acceleration_;
325  }
326 
330  double* getVariableAccelerations()
331  {
333  return acceleration_;
334  }
335 
338  const double* getVariableAccelerations() const
339  {
340  return acceleration_;
341  }
342 
344  void zeroAccelerations();
345 
348  void setVariableAccelerations(const double* acceleration)
349  {
350  has_acceleration_ = true;
351  has_effort_ = false;
352 
353  // assume everything is in order in terms of array lengths (for efficiency reasons)
354  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
355  }
356 
359  void setVariableAccelerations(const std::vector<double>& acceleration)
360  {
361  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
362  setVariableAccelerations(&acceleration[0]);
363  }
364 
367  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
368 
372  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
373  std::vector<std::string>& missing_variables);
374 
377  void setVariableAccelerations(const std::vector<std::string>& variable_names,
378  const std::vector<double>& variable_acceleration);
379 
381  void setVariableAcceleration(const std::string& variable, double value)
382  {
383  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
384  }
385 
388  void setVariableAcceleration(int index, double value)
389  {
391  acceleration_[index] = value;
392  }
393 
395  double getVariableAcceleration(const std::string& variable) const
396  {
397  return acceleration_[robot_model_->getVariableIndex(variable)];
398  }
399 
403  double getVariableAcceleration(int index) const
404  {
405  return acceleration_[index];
406  }
407 
409  void dropAccelerations();
410 
420  bool hasEffort() const
421  {
422  return has_effort_;
423  }
424 
428  double* getVariableEffort()
429  {
430  markEffort();
431  return effort_;
432  }
433 
436  const double* getVariableEffort() const
437  {
438  return effort_;
439  }
440 
442  void zeroEffort();
443 
445  void setVariableEffort(const double* effort)
446  {
447  has_effort_ = true;
448  has_acceleration_ = false;
449  // assume everything is in order in terms of array lengths (for efficiency reasons)
450  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
451  }
452 
454  void setVariableEffort(const std::vector<double>& effort)
455  {
456  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
457  setVariableEffort(&effort[0]);
458  }
459 
461  void setVariableEffort(const std::map<std::string, double>& variable_map);
462 
465  void setVariableEffort(const std::map<std::string, double>& variable_map, std::vector<std::string>& missing_variables);
466 
468  void setVariableEffort(const std::vector<std::string>& variable_names,
469  const std::vector<double>& variable_acceleration);
470 
472  void setVariableEffort(const std::string& variable, double value)
473  {
474  setVariableEffort(robot_model_->getVariableIndex(variable), value);
475  }
476 
479  void setVariableEffort(int index, double value)
480  {
481  markEffort();
482  effort_[index] = value;
483  }
484 
486  double getVariableEffort(const std::string& variable) const
487  {
488  return effort_[robot_model_->getVariableIndex(variable)];
489  }
490 
494  double getVariableEffort(int index) const
495  {
496  return effort_[index];
497  }
498 
500  void dropEffort();
501 
503  void dropDynamics();
504 
506  void invertVelocity();
507 
515  void setJointPositions(const std::string& joint_name, const double* position)
516  {
517  setJointPositions(robot_model_->getJointModel(joint_name), position);
518  }
519 
520  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
521  {
522  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
523  }
524 
525  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
526  {
527  setJointPositions(joint, &position[0]);
528  }
529 
530  void setJointPositions(const JointModel* joint, const double* position)
531  {
532  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
534  updateMimicJoint(joint);
535  }
536 
537  void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
538  {
539  setJointPositions(robot_model_->getJointModel(joint_name), transform);
540  }
541 
542  void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform)
543  {
544  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
546  updateMimicJoint(joint);
547  }
548 
549  void setJointVelocities(const JointModel* joint, const double* velocity)
550  {
551  has_velocity_ = true;
552  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
553  }
554 
555  void setJointEfforts(const JointModel* joint, const double* effort);
556 
557  const double* getJointPositions(const std::string& joint_name) const
558  {
559  return getJointPositions(robot_model_->getJointModel(joint_name));
560  }
561 
562  const double* getJointPositions(const JointModel* joint) const
563  {
564  return position_ + joint->getFirstVariableIndex();
565  }
566 
567  const double* getJointVelocities(const std::string& joint_name) const
568  {
569  return getJointVelocities(robot_model_->getJointModel(joint_name));
570  }
571 
572  const double* getJointVelocities(const JointModel* joint) const
573  {
574  return velocity_ + joint->getFirstVariableIndex();
575  }
576 
577  const double* getJointAccelerations(const std::string& joint_name) const
578  {
579  return getJointAccelerations(robot_model_->getJointModel(joint_name));
580  }
581 
582  const double* getJointAccelerations(const JointModel* joint) const
583  {
584  return acceleration_ + joint->getFirstVariableIndex();
585  }
586 
587  const double* getJointEffort(const std::string& joint_name) const
588  {
589  return getJointEffort(robot_model_->getJointModel(joint_name));
590  }
591 
592  const double* getJointEffort(const JointModel* joint) const
593  {
594  return effort_ + joint->getFirstVariableIndex();
595  }
596 
605  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
606  {
607  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
608  if (jmg)
609  setJointGroupPositions(jmg, gstate);
610  }
611 
614  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
615  {
616  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
617  if (jmg)
618  setJointGroupPositions(jmg, &gstate[0]);
619  }
620 
623  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
624  {
625  setJointGroupPositions(group, &gstate[0]);
626  }
627 
630  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
631 
634  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
635  {
636  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
637  if (jmg)
639  }
640 
643  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
644 
648  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
649  {
650  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
651  if (jmg)
652  {
653  gstate.resize(jmg->getVariableCount());
654  copyJointGroupPositions(jmg, &gstate[0]);
655  }
656  }
657 
661  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
662  {
663  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
664  if (jmg)
665  copyJointGroupPositions(jmg, gstate);
666  }
667 
671  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
672  {
673  gstate.resize(group->getVariableCount());
674  copyJointGroupPositions(group, &gstate[0]);
675  }
676 
680  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
681 
685  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
686  {
687  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
688  if (jmg)
690  }
691 
695  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
696 
705  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
706  {
707  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
708  if (jmg)
709  setJointGroupVelocities(jmg, gstate);
710  }
711 
714  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
715  {
716  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
717  if (jmg)
718  setJointGroupVelocities(jmg, &gstate[0]);
719  }
720 
723  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
724  {
725  setJointGroupVelocities(group, &gstate[0]);
726  }
727 
730  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
731 
734  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
735  {
736  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
737  if (jmg)
738  setJointGroupVelocities(jmg, values);
739  }
740 
743  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
744 
748  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
749  {
750  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
751  if (jmg)
752  {
753  gstate.resize(jmg->getVariableCount());
754  copyJointGroupVelocities(jmg, &gstate[0]);
755  }
756  }
757 
761  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
762  {
763  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
764  if (jmg)
765  copyJointGroupVelocities(jmg, gstate);
766  }
767 
771  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
772  {
773  gstate.resize(group->getVariableCount());
774  copyJointGroupVelocities(group, &gstate[0]);
775  }
776 
780  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
781 
785  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
786  {
787  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
788  if (jmg)
790  }
791 
795  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
796 
805  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
806  {
807  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
808  if (jmg)
809  setJointGroupAccelerations(jmg, gstate);
810  }
811 
814  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
815  {
816  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
817  if (jmg)
818  setJointGroupAccelerations(jmg, &gstate[0]);
819  }
820 
823  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
824  {
825  setJointGroupAccelerations(group, &gstate[0]);
826  }
827 
830  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
831 
834  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
835  {
836  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
837  if (jmg)
838  setJointGroupAccelerations(jmg, values);
839  }
840 
843  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
844 
848  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
849  {
850  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
851  if (jmg)
852  {
853  gstate.resize(jmg->getVariableCount());
854  copyJointGroupAccelerations(jmg, &gstate[0]);
855  }
856  }
857 
861  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
862  {
863  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
864  if (jmg)
865  copyJointGroupAccelerations(jmg, gstate);
866  }
867 
871  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
872  {
873  gstate.resize(group->getVariableCount());
874  copyJointGroupAccelerations(group, &gstate[0]);
875  }
876 
880  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
881 
885  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
886  {
887  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
888  if (jmg)
890  }
891 
895  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
896 
909  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
910 
917  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
918 
925  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
928  [[deprecated("The attempts argument is not supported anymore.")]] bool
929  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int /* attempts */,
930  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
932  {
933  return setFromIK(group, pose, timeout, constraint, options);
934  }
935 
943  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
944  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
946  [[deprecated("The attempts argument is not supported anymore.")]] bool
947  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
948  unsigned int /* attempts */, double timeout = 0.0,
951  {
952  return setFromIK(group, pose, tip, timeout, constraint, options);
953  }
954 
961  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
964  [[deprecated("The attempts argument is not supported anymore.")]] bool
965  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int /* attempts */,
966  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
968  {
969  return setFromIK(group, pose, timeout, constraint, options);
970  }
971 
978  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
979  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
981  [[deprecated("The attempts argument is not supported anymore.")]] bool
982  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
983  unsigned int /* attempts */, double timeout = 0.0,
986  {
987  return setFromIK(group, pose, tip, timeout, constraint, options);
988  }
989 
998  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
999  const std::vector<double>& consistency_limits, double timeout = 0.0,
1002  [[deprecated("The attempts argument is not supported anymore.")]] bool
1003  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1004  const std::vector<double>& consistency_limits, unsigned int /* attempts */, double timeout = 0.0,
1007  {
1008  return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options);
1009  }
1010 
1020  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1021  const std::vector<std::string>& tips, double timeout = 0.0,
1024  [[deprecated("The attempts argument is not supported anymore.")]] bool
1025  setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1026  const std::vector<std::string>& tips, unsigned int /* attempts */, double timeout = 0.0,
1029  {
1030  return setFromIK(group, poses, tips, timeout, constraint, options);
1031  }
1032 
1043  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1044  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1045  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1047  [[deprecated("The attempts argument is not supported anymore.")]] bool
1048  setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1049  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1050  unsigned int /* attempts */, double timeout = 0.0,
1053  {
1054  return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options);
1055  }
1056 
1065  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1066  const std::vector<std::string>& tips,
1067  const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
1070  [[deprecated("The attempts argument is not supported anymore.")]] bool
1071  setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1072  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1073  unsigned int /* attempts */, double timeout = 0.0,
1076  {
1077  return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options);
1078  }
1079 
1087  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1089 
1097  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1099 
1133  [[deprecated]] double
1134  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1135  const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step,
1136  double jump_threshold_factor,
1139 
1149  [[deprecated]] double
1150  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1151  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1152  double jump_threshold_factor,
1155 
1165  [[deprecated]] double
1166  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1167  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step,
1168  double jump_threshold_factor,
1171 
1181  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1182  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1183 
1193  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1194  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1195  {
1197  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1198  use_quaternion_representation);
1199  }
1200 
1207  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1208  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1209 
1216  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1217  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1218  {
1220  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1221  }
1222 
1225  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1226  const LinkModel* tip) const;
1227 
1230  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1231  const LinkModel* tip)
1232  {
1234  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1235  }
1236 
1240  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1242 
1249  void setVariableValues(const sensor_msgs::JointState& msg)
1250  {
1251  if (!msg.position.empty())
1252  setVariablePositions(msg.name, msg.position);
1253  if (!msg.velocity.empty())
1254  setVariableVelocities(msg.name, msg.velocity);
1255  }
1256 
1260  void setToDefaultValues();
1261 
1263  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1264 
1266  void setToRandomPositions();
1267 
1269  void setToRandomPositions(const JointModelGroup* group);
1270 
1273  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1274 
1280  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1281 
1289  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1290  const std::vector<double>& distances);
1291 
1301 
1304  void updateLinkTransforms();
1305 
1307  void update(bool force = false);
1308 
1317  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1318  {
1319  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1320  }
1321 
1323  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1324 
1333  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1334  {
1335  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1336  }
1337 
1338  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1339  {
1341  return global_link_transforms_[link->getLinkIndex()];
1342  }
1343 
1344  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1345  {
1346  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1347  }
1348 
1349  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1350  {
1351  BOOST_VERIFY(checkLinkTransforms());
1352  return global_link_transforms_[link->getLinkIndex()];
1353  }
1354 
1365  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1366  {
1367  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1368  }
1369 
1370  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1371  {
1373  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1374  }
1375 
1376  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1377  {
1378  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1379  }
1380 
1381  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1382  {
1383  BOOST_VERIFY(checkCollisionTransforms());
1384  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1385  }
1386 
1387  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1388  {
1389  return getJointTransform(robot_model_->getJointModel(joint_name));
1390  }
1391 
1392  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1393  {
1394  const int idx = joint->getJointIndex();
1395  unsigned char& dirty = dirty_joint_transforms_[idx];
1396  if (dirty)
1397  {
1398  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1399  dirty = 0;
1400  }
1401  return variable_joint_transforms_[idx];
1402  }
1403 
1404  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1405  {
1406  return getJointTransform(robot_model_->getJointModel(joint_name));
1407  }
1408 
1409  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1410  {
1411  BOOST_VERIFY(checkJointTransforms(joint));
1412  return variable_joint_transforms_[joint->getJointIndex()];
1413  }
1414 
1415  bool dirtyJointTransform(const JointModel* joint) const
1416  {
1417  return dirty_joint_transforms_[joint->getJointIndex()];
1418  }
1419 
1420  bool dirtyLinkTransforms() const
1421  {
1422  return dirty_link_transforms_;
1423  }
1424 
1425  bool dirtyCollisionBodyTransforms() const
1426  {
1428  }
1429 
1431  bool dirty() const
1432  {
1434  }
1435 
1442  double distance(const RobotState& other) const
1443  {
1444  return robot_model_->distance(position_, other.getVariablePositions());
1445  }
1446 
1447  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1448 
1449  double distance(const RobotState& other, const JointModel* joint) const
1450  {
1451  const int idx = joint->getFirstVariableIndex();
1452  return joint->distance(position_ + idx, other.position_ + idx);
1453  }
1454 
1458  void interpolate(const RobotState& to, double t, RobotState& state) const;
1459 
1463  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1464 
1468  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1469  {
1470  const int idx = joint->getFirstVariableIndex();
1471  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1472  state.markDirtyJointTransforms(joint);
1473  state.updateMimicJoint(joint);
1474  }
1476  void enforceBounds();
1477  void enforceBounds(const JointModelGroup* joint_group);
1478  void enforceBounds(const JointModel* joint)
1479  {
1480  enforcePositionBounds(joint);
1482  enforceVelocityBounds(joint);
1483  }
1484  void enforcePositionBounds(const JointModel* joint)
1485  {
1487  {
1488  markDirtyJointTransforms(joint);
1489  updateMimicJoint(joint);
1490  }
1491  }
1492 
1494  void harmonizePositions();
1495  void harmonizePositions(const JointModelGroup* joint_group);
1496  void harmonizePosition(const JointModel* joint)
1497  {
1498  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1499  // no need to mark transforms dirty, as the transform hasn't changed
1500  updateMimicJoint(joint);
1501  }
1502 
1503  void enforceVelocityBounds(const JointModel* joint)
1504  {
1506  }
1507 
1508  bool satisfiesBounds(double margin = 0.0) const;
1509  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1510  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1511  {
1512  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1513  }
1514  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1515  {
1516  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1517  }
1518  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1519  {
1520  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1521  }
1522 
1525  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1526 
1529  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1530 
1533  std::pair<double, const JointModel*>
1534  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1535 
1542  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1543 
1569  void attachBody(AttachedBody* attached_body);
1570 
1587  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1588  const EigenSTL::vector_Isometry3d& shape_poses, const std::set<std::string>& touch_links,
1589  const std::string& link_name,
1590  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
1592 
1609  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1610  const EigenSTL::vector_Isometry3d& shape_poses, const std::vector<std::string>& touch_links,
1611  const std::string& link_name,
1612  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
1614  {
1615  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1616  attachBody(id, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses);
1617  }
1618 
1620  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1621 
1623  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;
1624 
1626  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) const;
1627 
1630  bool clearAttachedBody(const std::string& id);
1631 
1633  void clearAttachedBodies(const LinkModel* link);
1634 
1636  void clearAttachedBodies(const JointModelGroup* group);
1637 
1639  void clearAttachedBodies();
1640 
1642  const AttachedBody* getAttachedBody(const std::string& name) const;
1643 
1645  bool hasAttachedBody(const std::string& id) const;
1646 
1652  void computeAABB(std::vector<double>& aabb) const;
1653 
1656  void computeAABB(std::vector<double>& aabb)
1657  {
1659  static_cast<const RobotState*>(this)->computeAABB(aabb);
1660  }
1661 
1664  {
1665  if (!rng_)
1667  return *rng_;
1668  }
1669 
1675  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr);
1676 
1682  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const;
1683 
1690  const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
1691  bool& frame_found) const;
1692 
1694  bool knowsFrameTransform(const std::string& frame_id) const;
1695 
1703  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1704  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1705  bool include_attached = false) const;
1706 
1714  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1715  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1716  bool include_attached = false)
1717  {
1719  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1720  }
1721 
1726  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1727  bool include_attached = false) const;
1728 
1733  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1734  bool include_attached = false)
1735  {
1737  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1738  }
1739 
1740  void printStatePositions(std::ostream& out = std::cout) const;
1741 
1743  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const;
1744 
1745  void printStateInfo(std::ostream& out = std::cout) const;
1746 
1747  void printTransforms(std::ostream& out = std::cout) const;
1748 
1749  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1750 
1751  void printDirtyInfo(std::ostream& out = std::cout) const;
1752 
1753  std::string getStateTreeString(const std::string& prefix = "") const;
1754 
1755 private:
1756  void allocMemory();
1757  void initTransforms();
1758  void copyFrom(const RobotState& other);
1759 
1760  void markDirtyJointTransforms(const JointModel* joint)
1761  {
1762  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1764  dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1765  }
1766 
1767  void markDirtyJointTransforms(const JointModelGroup* group)
1768  {
1769  for (const JointModel* jm : group->getActiveJointModels())
1772  group->getCommonRoot() :
1773  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1774  }
1775 
1776  void markVelocity();
1777  void markAcceleration();
1778  void markEffort();
1779 
1780  void updateMimicJoint(const JointModel* joint)
1781  {
1782  double v = position_[joint->getFirstVariableIndex()];
1783  for (const JointModel* jm : joint->getMimicRequests())
1784  {
1785  position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset();
1787  }
1788  }
1789 
1791  /* use updateMimicJoints() instead, which also marks joints dirty */
1792  [[deprecated]] void updateMimicJoint(const std::vector<const JointModel*>& mim)
1793  {
1794  for (const JointModel* jm : mim)
1795  {
1796  const int fvi = jm->getFirstVariableIndex();
1798  // Only mark joint transform dirty, but not the associated link transform
1799  // as this function is always used in combination of
1800  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1802  }
1803  }
1804 
1806  void updateMimicJoints(const JointModelGroup* group)
1807  {
1808  for (const JointModel* jm : group->getMimicJointModels())
1809  {
1810  const int fvi = jm->getFirstVariableIndex();
1813  }
1814  markDirtyJointTransforms(group);
1815  }
1816 
1817  void updateLinkTransformsInternal(const JointModel* start);
1818 
1819  void getMissingKeys(const std::map<std::string, double>& variable_map,
1820  std::vector<std::string>& missing_variables) const;
1821  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1822 
1824  bool checkJointTransforms(const JointModel* joint) const;
1825 
1827  bool checkLinkTransforms() const;
1828 
1830  bool checkCollisionTransforms() const;
1831 
1832  RobotModelConstPtr robot_model_;
1833  void* memory_;
1834 
1835  double* position_;
1836  double* velocity_;
1837  double* acceleration_;
1838  double* effort_;
1839  bool has_velocity_;
1840  bool has_acceleration_;
1841  bool has_effort_;
1842 
1845 
1846  // All the following transform variables point into aligned memory in memory_
1847  // They are updated lazily, based on the flags in dirty_joint_transforms_
1848  // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_
1849  Eigen::Isometry3d* variable_joint_transforms_;
1850  Eigen::Isometry3d* global_link_transforms_;
1851  Eigen::Isometry3d* global_collision_body_transforms_;
1852  unsigned char* dirty_joint_transforms_;
1853 
1855  std::map<std::string, AttachedBody*> attached_body_map_;
1856 
1860 
1867 };
1868 
1870 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1871 } // namespace core
1872 } // namespace moveit
moveit::core::RobotState::dropAccelerations
void dropAccelerations()
Remove accelerations from this state (this differs from setting them to zero)
Definition: robot_state.cpp:324
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:81
moveit::core::RobotState::checkJointTransforms
bool checkJointTransforms(const JointModel *joint) const
This function is only called in debug mode.
Definition: robot_state.cpp:242
moveit::core::GroupStateValidityCallbackFn
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:136
moveit::core::JointModel::enforcePositionBounds
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized,...
Definition: joint_model.h:355
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:581
moveit::core::RobotState::operator=
RobotState & operator=(const RobotState &other)
Copy operator.
Definition: robot_state.cpp:194
moveit::core::RobotState::distance
double distance(const RobotState &other) const
Definition: robot_state.h:1508
moveit::core::RobotState::dirtyJointTransform
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1481
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
Definition: robot_state.cpp:414
shapes
moveit::core::operator<<
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
Definition: joint_model.cpp:287
moveit::core::RobotState::getVariableVelocity
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:362
moveit::core::RobotState::harmonizePosition
void harmonizePosition(const JointModel *joint)
Definition: robot_state.h:1562
moveit::core::JointModel::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:261
moveit::core::JointModel::getMimicOffset
double getMimicOffset() const
If mimicking a joint, this is the offset added to that joint's value.
Definition: joint_model.h:441
moveit::core::RobotState::getJointModel
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:194
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:176
moveit::core::JointModel::enforceVelocityBounds
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:383
moveit::core::RobotState::setJointGroupVelocities
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:771
moveit::core::RobotState::velocity_
double * velocity_
Definition: robot_state.h:1902
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::RobotState::getStateTreeJointString
void getStateTreeJointString(std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
Definition: robot_state.cpp:2274
moveit::core::JointModel::interpolate
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....
moveit::core::RobotState::setVariableVelocities
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:319
moveit::core::RobotState::getFrameInfo
const Eigen::Isometry3d & getFrameInfo(const std::string &frame_id, const LinkModel *&robot_link, bool &frame_found) const
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1113
moveit::core::RobotState::knowsFrameTransform
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
Definition: robot_state.cpp:1174
moveit::core::RobotState::zeroAccelerations
void zeroAccelerations()
Set all accelerations to 0.0.
Definition: robot_state.cpp:307
moveit::core::RobotState::getJacobian
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
Definition: robot_state.cpp:1289
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:188
moveit::core::RobotState::getVariableAccelerations
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:396
moveit::core::RobotState::hasAccelerations
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:388
moveit::core::RobotState::copyFrom
void copyFrom(const RobotState &other)
Definition: robot_state.cpp:201
moveit::core::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(RobotModel)
moveit::core::RobotState::dropDynamics
void dropDynamics()
Reduce RobotState to kinematic information (remove velocity, acceleration and effort,...
Definition: robot_state.cpp:334
moveit::core::RobotState::setFromIKSubgroups
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solv...
Definition: robot_state.cpp:1822
moveit::core::RobotState::hasVelocities
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:295
moveit::core::RobotState::updateLinkTransformsInternal
void updateLinkTransformsInternal(const JointModel *start)
Definition: robot_state.cpp:731
moveit::core::RobotState::getJointTransform
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1453
moveit::core::RobotState::markAcceleration
void markAcceleration()
Definition: robot_state.cpp:281
moveit::core::JointModel::getMimicFactor
double getMimicFactor() const
If mimicking a joint, this is the multiplicative factor for that joint's value.
Definition: joint_model.h:447
attached_body.h
moveit::core::RobotState::getAttachedBody
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
Definition: robot_state.cpp:974
moveit::core::RobotState::getVariableEffort
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:494
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::RobotState::copyJointGroupPositions
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:714
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1399
moveit::core::RobotState::setFromIK
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
If the group this state corresponds to is a chain and a solver is available, then the joint values ca...
Definition: robot_state.cpp:1473
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition: robot_state.cpp:341
moveit::core::RobotState::computeVariableVelocity
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip) const
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
Definition: robot_state.cpp:1413
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
moveit::core::RobotState::getJointAccelerations
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:643
moveit::core::RobotState::setAttachedBodyUpdateCallback
void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
Definition: robot_state.cpp:964
moveit::core::RobotState::setVariableVelocity
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:348
moveit::core::RobotState::setToIKSolverFrame
bool setToIKSolverFrame(Eigen::Isometry3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
Convert the frame of reference of the pose to that same frame as the IK solver expects.
Definition: robot_state.cpp:1536
moveit::core::RobotState::printStatePositions
void printStatePositions(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2096
moveit::core::RobotState::getMinDistanceToPositionBounds
std::pair< double, const JointModel * > getMinDistanceToPositionBounds() const
Get the minimm distance from this state to the bounds. The minimum distance and the joint for which t...
Definition: robot_state.cpp:866
moveit::core::RobotState::attachBody
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
Definition: robot_state.cpp:986
moveit::core::RobotState::getRobotMarkers
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) const
Get a MarkerArray that fully describes the robot markers for a given robot.
Definition: robot_state.cpp:1195
moveit::core::RobotState::checkCollisionTransforms
bool checkCollisionTransforms() const
This function is only called in debug mode.
Definition: robot_state.cpp:262
robot_model.h
moveit::core::RobotState::getVariableNames
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:182
moveit::core::RobotState::clearAttachedBodies
void clearAttachedBodies()
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
Definition: robot_state.cpp:1030
moveit::core::RobotState::getMissingKeys
void getMissingKeys(const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) const
Definition: robot_state.cpp:438
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
moveit::core::RobotState::copyJointGroupAccelerations
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:914
moveit::core::RobotState::harmonizePositions
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
Definition: robot_state.cpp:854
transforms.h
moveit::core::RobotState::getRandomNumberGenerator
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1729
moveit::core::RobotState::setToRandomPositionsNearBy
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance)
Set all joints in group to random values near the value in \near. distance is the maximum amount each...
Definition: robot_state.cpp:382
moveit::core::RobotState::memory_
void * memory_
Definition: robot_state.h:1899
moveit::core::RobotState::printDirtyInfo
void printDirtyInfo(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2155
moveit::core::RobotState::satisfiesPositionBounds
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1580
moveit::core::RobotState::RobotState
RobotState(const RobotModelConstPtr &robot_model)
A state can be constructed from a specified robot model. No values are initialized....
Definition: robot_state.cpp:122
moveit::core::RobotState::markDirtyJointTransforms
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1826
moveit::core::RobotState::markEffort
void markEffort()
Definition: robot_state.cpp:291
moveit::core::RobotState::printTransform
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
Definition: robot_state.cpp:2211
moveit::core::RobotState::dirtyLinkTransforms
bool dirtyLinkTransforms() const
Definition: robot_state.h:1486
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:676
moveit::core::RobotState::setVariablePosition
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:254
moveit::core::RobotState::dropVelocities
void dropVelocities()
Remove velocities from this state (this differs from setting them to zero)
Definition: robot_state.cpp:319
moveit::core::JointModel::getFirstVariableIndex
int getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
Definition: joint_model.h:267
moveit::core::RobotState::dirty_link_transforms_
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1909
moveit::core::RobotState::printTransforms
void printTransforms(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2220
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:170
moveit::core::JointModel::getMimicRequests
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:456
moveit::core::RobotState::getVariablePosition
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:273
moveit::core::RobotState::position_
double * position_
Definition: robot_state.h:1901
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:405
moveit::core::RobotState::setVariableEffort
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:511
moveit::core::RobotState::getJointEffort
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:653
moveit::core::RobotState::global_link_transforms_
Eigen::Isometry3d * global_link_transforms_
Transforms from model frame to link frame for each link.
Definition: robot_state.h:1916
moveit::core::RobotState::updateMimicJoints
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
Definition: robot_state.h:1872
moveit::core::RobotState::getStateTreeString
std::string getStateTreeString(const std::string &prefix="") const
Definition: robot_state.cpp:2249
moveit::core::RobotState::computeAABB
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx,...
Definition: robot_state.cpp:2061
moveit::core::RobotState::setJointEfforts
void setJointEfforts(const JointModel *joint, const double *effort)
Definition: robot_state.cpp:547
moveit::core::RobotState::allocMemory
void allocMemory()
Definition: robot_state.cpp:150
moveit::core::RobotState::rng_
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1932
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Definition: robot_state.cpp:1006
moveit::core::AttachedBodyCallback
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:115
moveit::core::RobotState::has_acceleration_
bool has_acceleration_
Definition: robot_state.h:1906
moveit::core::RobotState::setFromDiffIK
bool setFromDiffIK(const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Set the joint values from a Cartesian velocity applied during a time dt.
Definition: robot_state.cpp:1397
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:117
moveit::core::JointModel::getJointIndex
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:279
moveit::core::RobotState::enforcePositionBounds
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1550
moveit::core::RobotState::setFromIK
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:1048
moveit::core::RobotState::attached_body_map_
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:1921
moveit::core::RobotState::printStatePositionsWithJointLimits
void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup *jmg, std::ostream &out=std::cout) const
Output to console the current state of the robot's joint limits.
Definition: robot_state.cpp:2103
moveit::core::RobotState::global_collision_body_transforms_
Eigen::Isometry3d * global_collision_body_transforms_
Transforms from model frame to collision bodies.
Definition: robot_state.h:1917
moveit::core::RobotState::variable_joint_transforms_
Eigen::Isometry3d * variable_joint_transforms_
Local transforms of all joints.
Definition: robot_state.h:1915
moveit::core::RobotState::~RobotState
~RobotState()
Definition: robot_state.cpp:142
moveit::core::RobotState::effort_
double * effort_
Definition: robot_state.h:1904
random_numbers::RandomNumberGenerator
moveit::core::RobotState::has_velocity_
bool has_velocity_
Definition: robot_state.h:1905
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
Definition: robot_state.cpp:717
moveit::core::RobotState::integrateVariableVelocity
bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn())
Given the velocities for the variables in this group (qdot) and an amount of time (dt),...
Definition: robot_state.cpp:1454
moveit::core::RobotState::getVariablePositions
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
Definition: robot_state.h:213
moveit::core::RobotState::dirty
bool dirty() const
Returns true if anything in this state is dirty.
Definition: robot_state.h:1497
moveit::core::JointModel::harmonizePosition
virtual bool harmonizePosition(double *values, const Bounds &other_bounds) const
Definition: joint_model.cpp:159
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1093
moveit::core::RobotState::updateCollisionBodyTransforms
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
Definition: robot_state.cpp:689
moveit::core::RobotState::zeroEffort
void zeroEffort()
Set all effort values to 0.0.
Definition: robot_state.cpp:313
moveit::core::RobotState::zeroVelocities
void zeroVelocities()
Set all velocities to 0.0.
Definition: robot_state.cpp:301
moveit::core::RobotState::getVariableVelocities
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:302
moveit::core::RobotState::checkLinkTransforms
bool checkLinkTransforms() const
This function is only called in debug mode.
Definition: robot_state.cpp:252
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
moveit::core::JointModel::getMimic
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:435
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:107
moveit::core::RobotState::clearAttachedBody
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)....
Definition: robot_state.cpp:1078
moveit::core::RobotState::getJointPositions
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:623
moveit::core::RobotState::dirty_joint_transforms_
unsigned char * dirty_joint_transforms_
Definition: robot_state.h:1918
moveit::core::RobotState::invertVelocity
void invertVelocity()
Invert velocity if present.
Definition: robot_state.cpp:538
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
Definition: robot_state.cpp:822
moveit::core::RobotState::getCollisionBodyTransform
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1431
moveit::core::RobotState::copyJointGroupVelocities
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:814
moveit::core::RobotState::attached_body_update_callback_
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:1925
moveit::core::RobotState::interpolate
void interpolate(const RobotState &to, double t, RobotState &state) const
Interpolate from this state towards state to, at time t in [0,1]. The result is stored in state,...
Definition: robot_state.cpp:945
moveit::core::RobotState::initTransforms
void initTransforms()
Definition: robot_state.cpp:180
moveit::core::JointModel::distance
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)
moveit::core::RobotState::getVariableAcceleration
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:461
index
unsigned int index
moveit::core::RobotState::setJointVelocities
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:615
moveit::core::RobotState::setJointGroupAccelerations
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:871
moveit::core::RobotState::robot_model_
RobotModelConstPtr robot_model_
Definition: robot_state.h:1898
moveit::core::RobotState::setVariableAccelerations
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:414
moveit::core::RobotState::updateStateWithLinkAt
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:1383
moveit::core::RobotState::acceleration_
double * acceleration_
Definition: robot_state.h:1903
moveit::core::RobotState::enforceVelocityBounds
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1569
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition: joint_model_group.h:475
moveit::core::RobotState::hasEffort
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:486
moveit::core::RobotState::setJointGroupPositions
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:671
moveit::core::RobotState::has_effort_
bool has_effort_
Definition: robot_state.h:1907
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:200
moveit::core::RobotState::setVariableValues
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1315
moveit::core::RobotState::setVariableAcceleration
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:447
moveit::core::RobotState::printStateInfo
void printStateInfo(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2168
moveit::core::JointModel::satisfiesVelocityBounds
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:374
moveit::core::JointModel::satisfiesPositionBounds
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:343
moveit::core::RobotState::isValidVelocityMove
bool isValidVelocityMove(const RobotState &other, const JointModelGroup *group, double dt) const
Check that the time to move between two waypoints is sufficient given velocity limits and time step.
Definition: robot_state.cpp:913
moveit::core::RobotState::markVelocity
void markVelocity()
Definition: robot_state.cpp:272
ros::Duration
moveit::core::JointModel::computeVariablePositions
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values. Make sure the passed ...
moveit::core::RobotState::enforceBounds
void enforceBounds()
Definition: robot_state.cpp:840
moveit::core::RobotState::computeCartesianPath
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())
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular gr...
Definition: robot_state.cpp:2028
moveit::core::RobotState::dropEffort
void dropEffort()
Remove effort values from this state (this differs from setting them to zero)
Definition: robot_state.cpp:329
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
moveit::core::RobotState::updateMimicJoint
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1846
moveit::core::RobotState::dirtyCollisionBodyTransforms
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1491
moveit::core::RobotState::hasAttachedBody
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in this state.
Definition: robot_state.cpp:969
moveit::core::RobotState::satisfiesVelocityBounds
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1584
moveit::core::RobotState::getJointVelocities
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:633
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::RobotState::dirty_collision_body_transforms_
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1910


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Nov 24 2020 03:26:40