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 
83 {
84 public:
87  RobotState(const RobotModelConstPtr& robot_model);
88  ~RobotState();
89 
91  RobotState(const RobotState& other);
92 
94  RobotState& operator=(const RobotState& other);
95 
97  const RobotModelConstPtr& getRobotModel() const
98  {
99  return robot_model_;
100  }
101 
103  std::size_t getVariableCount() const
104  {
105  return robot_model_->getVariableCount();
106  }
107 
109  const std::vector<std::string>& getVariableNames() const
110  {
111  return robot_model_->getVariableNames();
112  }
113 
115  const LinkModel* getLinkModel(const std::string& link) const
116  {
117  return robot_model_->getLinkModel(link);
118  }
119 
121  const JointModel* getJointModel(const std::string& joint) const
122  {
123  return robot_model_->getJointModel(joint);
124  }
125 
127  const JointModelGroup* getJointModelGroup(const std::string& group) const
128  {
129  return robot_model_->getJointModelGroup(group);
130  }
131 
141  {
142  return position_;
143  }
144 
147  const double* getVariablePositions() const
148  {
149  return position_;
150  }
151 
155  void setVariablePositions(const double* position);
156 
160  void setVariablePositions(const std::vector<double>& position)
161  {
162  assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
163  setVariablePositions(&position[0]);
164  }
165 
168  void setVariablePositions(const std::map<std::string, double>& variable_map);
169 
172  void setVariablePositions(const std::map<std::string, double>& variable_map,
173  std::vector<std::string>& missing_variables);
174 
177  void setVariablePositions(const std::vector<std::string>& variable_names,
178  const std::vector<double>& variable_position);
179 
181  void setVariablePosition(const std::string& variable, double value)
182  {
183  setVariablePosition(robot_model_->getVariableIndex(variable), value);
184  }
185 
188  void setVariablePosition(int index, double value)
189  {
190  position_[index] = value;
191  const JointModel* jm = robot_model_->getJointOfVariable(index);
192  if (jm)
193  {
195  updateMimicJoint(jm);
196  }
197  }
198 
200  double getVariablePosition(const std::string& variable) const
201  {
202  return position_[robot_model_->getVariableIndex(variable)];
203  }
204 
208  double getVariablePosition(int index) const
209  {
210  return position_[index];
211  }
212 
222  bool hasVelocities() const
223  {
224  return has_velocity_;
225  }
226 
230  {
231  markVelocity();
232  return velocity_;
233  }
234 
237  const double* getVariableVelocities() const
238  {
239  return velocity_;
240  }
241 
243  void setVariableVelocities(const double* velocity)
244  {
245  has_velocity_ = true;
246  // assume everything is in order in terms of array lengths (for efficiency reasons)
247  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
248  }
249 
251  void setVariableVelocities(const std::vector<double>& velocity)
252  {
253  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
254  setVariableVelocities(&velocity[0]);
255  }
256 
259  void setVariableVelocities(const std::map<std::string, double>& variable_map);
260 
263  void setVariableVelocities(const std::map<std::string, double>& variable_map,
264  std::vector<std::string>& missing_variables);
265 
268  void setVariableVelocities(const std::vector<std::string>& variable_names,
269  const std::vector<double>& variable_velocity);
270 
272  void setVariableVelocity(const std::string& variable, double value)
273  {
274  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
275  }
276 
279  void setVariableVelocity(int index, double value)
280  {
281  markVelocity();
282  velocity_[index] = value;
283  }
284 
286  double getVariableVelocity(const std::string& variable) const
287  {
288  return velocity_[robot_model_->getVariableIndex(variable)];
289  }
290 
294  double getVariableVelocity(int index) const
295  {
296  return velocity_[index];
297  }
298 
309  bool hasAccelerations() const
310  {
311  return has_acceleration_;
312  }
313 
318  {
320  return acceleration_;
321  }
322 
325  const double* getVariableAccelerations() const
326  {
327  return acceleration_;
328  }
329 
332  void setVariableAccelerations(const double* acceleration)
333  {
334  has_acceleration_ = true;
335  has_effort_ = false;
336 
337  // assume everything is in order in terms of array lengths (for efficiency reasons)
338  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
339  }
340 
343  void setVariableAccelerations(const std::vector<double>& acceleration)
344  {
345  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
346  setVariableAccelerations(&acceleration[0]);
347  }
348 
351  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
352 
356  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
357  std::vector<std::string>& missing_variables);
358 
361  void setVariableAccelerations(const std::vector<std::string>& variable_names,
362  const std::vector<double>& variable_acceleration);
363 
365  void setVariableAcceleration(const std::string& variable, double value)
366  {
367  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
368  }
369 
372  void setVariableAcceleration(int index, double value)
373  {
375  acceleration_[index] = value;
376  }
377 
379  double getVariableAcceleration(const std::string& variable) const
380  {
381  return acceleration_[robot_model_->getVariableIndex(variable)];
382  }
383 
387  double getVariableAcceleration(int index) const
388  {
389  return acceleration_[index];
390  }
391 
401  bool hasEffort() const
402  {
403  return has_effort_;
404  }
405 
410  {
411  markEffort();
412  return effort_;
413  }
414 
417  const double* getVariableEffort() const
418  {
419  return effort_;
420  }
421 
423  void setVariableEffort(const double* effort)
424  {
425  has_effort_ = true;
426  has_acceleration_ = false;
427  // assume everything is in order in terms of array lengths (for efficiency reasons)
428  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
429  }
430 
432  void setVariableEffort(const std::vector<double>& effort)
433  {
434  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
435  setVariableEffort(&effort[0]);
436  }
437 
439  void setVariableEffort(const std::map<std::string, double>& variable_map);
440 
443  void setVariableEffort(const std::map<std::string, double>& variable_map,
444  std::vector<std::string>& missing_variables);
445 
447  void setVariableEffort(const std::vector<std::string>& variable_names,
448  const std::vector<double>& variable_acceleration);
449 
451  void setVariableEffort(const std::string& variable, double value)
452  {
453  setVariableEffort(robot_model_->getVariableIndex(variable), value);
454  }
455 
458  void setVariableEffort(int index, double value)
459  {
460  markEffort();
461  effort_[index] = value;
462  }
463 
465  double getVariableEffort(const std::string& variable) const
466  {
467  return effort_[robot_model_->getVariableIndex(variable)];
468  }
469 
473  double getVariableEffort(int index) const
474  {
475  return effort_[index];
476  }
477 
483  void setJointPositions(const std::string& joint_name, const double* position)
484  {
485  setJointPositions(robot_model_->getJointModel(joint_name), position);
486  }
487 
488  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
489  {
490  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
491  }
492 
493  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
494  {
495  setJointPositions(joint, &position[0]);
496  }
497 
498  void setJointPositions(const JointModel* joint, const double* position)
499  {
500  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
502  updateMimicJoint(joint);
503  }
504 
505  void setJointPositions(const std::string& joint_name, const Eigen::Affine3d& transform)
506  {
507  setJointPositions(robot_model_->getJointModel(joint_name), transform);
508  }
509 
510  void setJointPositions(const JointModel* joint, const Eigen::Affine3d& transform)
511  {
512  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
514  updateMimicJoint(joint);
515  }
516 
517  void setJointVelocities(const JointModel* joint, const double* velocity)
518  {
519  has_velocity_ = true;
520  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
521  }
522 
523  void setJointEfforts(const JointModel* joint, const double* effort)
524  {
525  if (has_acceleration_)
526  {
527  logError("Unable to set joint efforts because array is being used for accelerations");
528  return;
529  }
530  has_effort_ = true;
531 
532  memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double));
533  }
534 
535  const double* getJointPositions(const std::string& joint_name) const
536  {
537  return getJointPositions(robot_model_->getJointModel(joint_name));
538  }
539 
540  const double* getJointPositions(const JointModel* joint) const
541  {
542  return position_ + joint->getFirstVariableIndex();
543  }
544 
545  const double* getJointVelocities(const std::string& joint_name) const
546  {
547  return getJointVelocities(robot_model_->getJointModel(joint_name));
548  }
549 
550  const double* getJointVelocities(const JointModel* joint) const
551  {
552  return velocity_ + joint->getFirstVariableIndex();
553  }
554 
555  const double* getJointAccelerations(const std::string& joint_name) const
556  {
557  return getJointAccelerations(robot_model_->getJointModel(joint_name));
558  }
559 
560  const double* getJointAccelerations(const JointModel* joint) const
561  {
562  return acceleration_ + joint->getFirstVariableIndex();
563  }
564 
565  const double* getJointEffort(const std::string& joint_name) const
566  {
567  return getJointEffort(robot_model_->getJointModel(joint_name));
568  }
569 
570  const double* getJointEffort(const JointModel* joint) const
571  {
572  return effort_ + joint->getFirstVariableIndex();
573  }
574 
584  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
585  {
586  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
587  if (jmg)
588  setJointGroupPositions(jmg, gstate);
589  }
590 
594  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
595  {
596  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
597  if (jmg)
598  setJointGroupPositions(jmg, &gstate[0]);
599  }
600 
604  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
605  {
606  setJointGroupPositions(group, &gstate[0]);
607  }
608 
612  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
613 
617  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
618  {
619  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
620  if (jmg)
621  setJointGroupPositions(jmg, values);
622  }
623 
627  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
628 
632  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
633  {
634  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
635  if (jmg)
636  {
637  gstate.resize(jmg->getVariableCount());
638  copyJointGroupPositions(jmg, &gstate[0]);
639  }
640  }
641 
645  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
646  {
647  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
648  if (jmg)
649  copyJointGroupPositions(jmg, gstate);
650  }
651 
655  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
656  {
657  gstate.resize(group->getVariableCount());
658  copyJointGroupPositions(group, &gstate[0]);
659  }
660 
664  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
665 
669  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
670  {
671  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
672  if (jmg)
673  copyJointGroupPositions(jmg, values);
674  }
675 
679  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
680 
690  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
691  {
692  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
693  if (jmg)
694  setJointGroupVelocities(jmg, gstate);
695  }
696 
700  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
701  {
702  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
703  if (jmg)
704  setJointGroupVelocities(jmg, &gstate[0]);
705  }
706 
710  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
711  {
712  setJointGroupVelocities(group, &gstate[0]);
713  }
714 
718  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
719 
723  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
724  {
725  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
726  if (jmg)
727  setJointGroupVelocities(jmg, values);
728  }
729 
733  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
734 
738  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
739  {
740  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
741  if (jmg)
742  {
743  gstate.resize(jmg->getVariableCount());
744  copyJointGroupVelocities(jmg, &gstate[0]);
745  }
746  }
747 
751  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
752  {
753  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
754  if (jmg)
755  copyJointGroupVelocities(jmg, gstate);
756  }
757 
761  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
762  {
763  gstate.resize(group->getVariableCount());
764  copyJointGroupVelocities(group, &gstate[0]);
765  }
766 
770  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
771 
775  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
776  {
777  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
778  if (jmg)
779  copyJointGroupVelocities(jmg, values);
780  }
781 
785  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
786 
796  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
797  {
798  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
799  if (jmg)
800  setJointGroupAccelerations(jmg, gstate);
801  }
802 
806  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
807  {
808  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
809  if (jmg)
810  setJointGroupAccelerations(jmg, &gstate[0]);
811  }
812 
816  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
817  {
818  setJointGroupAccelerations(group, &gstate[0]);
819  }
820 
824  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
825 
829  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
830  {
831  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
832  if (jmg)
833  setJointGroupAccelerations(jmg, values);
834  }
835 
839  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
840 
844  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
845  {
846  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
847  if (jmg)
848  {
849  gstate.resize(jmg->getVariableCount());
850  copyJointGroupAccelerations(jmg, &gstate[0]);
851  }
852  }
853 
857  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
858  {
859  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
860  if (jmg)
861  copyJointGroupAccelerations(jmg, gstate);
862  }
863 
867  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
868  {
869  gstate.resize(group->getVariableCount());
870  copyJointGroupAccelerations(group, &gstate[0]);
871  }
872 
876  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
877 
881  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
882  {
883  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
884  if (jmg)
885  copyJointGroupAccelerations(jmg, values);
886  }
887 
891  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
892 
905  bool setToIKSolverFrame(Eigen::Affine3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
906 
913  bool setToIKSolverFrame(Eigen::Affine3d& pose, const std::string& ik_frame);
914 
922  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts = 0,
923  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
925 
934  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
935  unsigned int attempts = 0, double timeout = 0.0,
938 
946  bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, unsigned int attempts = 0,
947  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
949 
957  bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, const std::string& tip,
958  unsigned int attempts = 0, double timeout = 0.0,
961 
971  bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, const std::string& tip,
972  const std::vector<double>& consistency_limits, unsigned int attempts = 0, double timeout = 0.0,
975 
986  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
987  const std::vector<std::string>& tips, unsigned int attempts = 0, double timeout = 0.0,
990 
1002  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
1003  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1004  unsigned int attempts = 0, double timeout = 0.0,
1007 
1017  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
1018  const std::vector<std::string>& tips,
1019  const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts = 0,
1020  double timeout = 0.0,
1023 
1031  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1033 
1041  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1043 
1078  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1079  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1080  double max_step, double jump_threshold,
1083 
1117  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1118  const Eigen::Affine3d& target, bool global_reference_frame, double max_step,
1119  double jump_threshold,
1122 
1156  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1157  const EigenSTL::vector_Affine3d& waypoints, bool global_reference_frame, double max_step,
1158  double jump_threshold,
1161 
1171  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1172  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1173 
1183  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1184  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1185  {
1187  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1188  use_quaternion_representation);
1189  }
1190 
1197  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1198  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1199 
1206  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1207  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1208  {
1210  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1211  }
1212 
1215  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1216  const LinkModel* tip) const;
1217 
1220  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1221  const LinkModel* tip)
1222  {
1224  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1225  }
1226 
1230  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1232 
1239  void setVariableValues(const sensor_msgs::JointState& msg)
1240  {
1241  if (!msg.position.empty())
1242  setVariablePositions(msg.name, msg.position);
1243  if (!msg.velocity.empty())
1244  setVariableVelocities(msg.name, msg.velocity);
1245  }
1246 
1250  void setToDefaultValues();
1251 
1253  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1254 
1256  void setToRandomPositions();
1257 
1259  void setToRandomPositions(const JointModelGroup* group);
1260 
1264 
1270  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1271 
1279  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1280  const std::vector<double>& distances);
1281 
1291 
1294  void updateLinkTransforms();
1295 
1297  void update(bool force = false);
1298 
1300  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Affine3d& transform, bool backward = false)
1301  {
1302  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1303  }
1304 
1306  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Affine3d& transform, bool backward = false);
1307 
1308  const Eigen::Affine3d& getGlobalLinkTransform(const std::string& link_name)
1309  {
1310  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1311  }
1312 
1313  const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel* link)
1314  {
1316  return global_link_transforms_[link->getLinkIndex()];
1317  }
1318 
1319  const Eigen::Affine3d& getCollisionBodyTransforms(const std::string& link_name, std::size_t index)
1320  {
1321  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1322  }
1323 
1324  const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1325  {
1328  }
1329 
1330  const Eigen::Affine3d& getJointTransform(const std::string& joint_name)
1331  {
1332  return getJointTransform(robot_model_->getJointModel(joint_name));
1333  }
1334 
1335  const Eigen::Affine3d& getJointTransform(const JointModel* joint)
1336  {
1337  const int idx = joint->getJointIndex();
1338  unsigned char& dirty = dirty_joint_transforms_[idx];
1339  if (dirty)
1340  {
1342  dirty = 0;
1343  }
1344  return variable_joint_transforms_[idx];
1345  }
1346 
1347  const Eigen::Affine3d& getGlobalLinkTransform(const std::string& link_name) const
1348  {
1349  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1350  }
1351 
1352  const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel* link) const
1353  {
1354  BOOST_VERIFY(checkLinkTransforms());
1355  return global_link_transforms_[link->getLinkIndex()];
1356  }
1357 
1358  const Eigen::Affine3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1359  {
1360  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1361  }
1362 
1363  const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1364  {
1365  BOOST_VERIFY(checkCollisionTransforms());
1367  }
1368 
1369  const Eigen::Affine3d& getJointTransform(const std::string& joint_name) const
1370  {
1371  return getJointTransform(robot_model_->getJointModel(joint_name));
1372  }
1373 
1374  const Eigen::Affine3d& getJointTransform(const JointModel* joint) const
1375  {
1376  BOOST_VERIFY(checkJointTransforms(joint));
1377  return variable_joint_transforms_[joint->getJointIndex()];
1378  }
1379 
1380  bool dirtyJointTransform(const JointModel* joint) const
1381  {
1382  return dirty_joint_transforms_[joint->getJointIndex()];
1383  }
1384 
1385  bool dirtyLinkTransforms() const
1386  {
1387  return dirty_link_transforms_;
1388  }
1389 
1391  {
1393  }
1394 
1396  bool dirty() const
1397  {
1399  }
1400 
1407  double distance(const RobotState& other) const
1408  {
1409  return robot_model_->distance(position_, other.getVariablePositions());
1410  }
1411 
1412  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1413 
1414  double distance(const RobotState& other, const JointModel* joint) const
1415  {
1416  const int idx = joint->getFirstVariableIndex();
1417  return joint->distance(position_ + idx, other.position_ + idx);
1418  }
1419 
1423  void interpolate(const RobotState& to, double t, RobotState& state) const;
1424 
1428  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1429 
1433  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1434  {
1435  const int idx = joint->getFirstVariableIndex();
1436  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1437  state.markDirtyJointTransforms(joint);
1438  state.updateMimicJoint(joint);
1439  }
1440 
1441  void enforceBounds();
1442  void enforceBounds(const JointModelGroup* joint_group);
1443  void enforceBounds(const JointModel* joint)
1444  {
1445  enforcePositionBounds(joint);
1446  if (has_velocity_)
1447  enforceVelocityBounds(joint);
1448  }
1450  {
1451  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1452  {
1453  markDirtyJointTransforms(joint);
1454  updateMimicJoint(joint);
1455  }
1456  }
1458  {
1460  }
1461 
1462  bool satisfiesBounds(double margin = 0.0) const;
1463  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1464  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1465  {
1466  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1467  }
1468  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1469  {
1470  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1471  }
1472  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1473  {
1474  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1475  }
1476 
1479  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1480 
1483  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1484 
1487  std::pair<double, const JointModel*>
1488  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1489 
1496  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1497 
1523  void attachBody(AttachedBody* attached_body);
1524 
1539  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1540  const EigenSTL::vector_Affine3d& attach_trans, const std::set<std::string>& touch_links,
1541  const std::string& link_name,
1542  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1543 
1558  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1559  const EigenSTL::vector_Affine3d& attach_trans, const std::vector<std::string>& touch_links,
1560  const std::string& link_name,
1561  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1562  {
1563  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1564  attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1565  }
1566 
1568  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1569 
1571  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
1572 
1574  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
1575 
1578  bool clearAttachedBody(const std::string& id);
1579 
1581  void clearAttachedBodies(const LinkModel* link);
1582 
1584  void clearAttachedBodies(const JointModelGroup* group);
1585 
1587  void clearAttachedBodies();
1588 
1590  const AttachedBody* getAttachedBody(const std::string& name) const;
1591 
1593  bool hasAttachedBody(const std::string& id) const;
1594 
1600  void computeAABB(std::vector<double>& aabb) const;
1601 
1604  void computeAABB(std::vector<double>& aabb)
1605  {
1607  static_cast<const RobotState*>(this)->computeAABB(aabb);
1608  }
1609 
1612  {
1613  if (!rng_)
1615  return *rng_;
1616  }
1617 
1619  const Eigen::Affine3d& getFrameTransform(const std::string& id);
1620 
1622  const Eigen::Affine3d& getFrameTransform(const std::string& id) const;
1623 
1625  bool knowsFrameTransform(const std::string& id) const;
1626 
1634  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1635  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1636  bool include_attached = false) const;
1637 
1645  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1646  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1647  bool include_attached = false)
1648  {
1650  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1651  }
1652 
1657  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1658  bool include_attached = false) const;
1659 
1664  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1665  bool include_attached = false)
1666  {
1668  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1669  }
1670 
1671  void printStatePositions(std::ostream& out = std::cout) const;
1672 
1673  void printStateInfo(std::ostream& out = std::cout) const;
1674 
1675  void printTransforms(std::ostream& out = std::cout) const;
1676 
1677  void printTransform(const Eigen::Affine3d& transform, std::ostream& out = std::cout) const;
1678 
1679  void printDirtyInfo(std::ostream& out = std::cout) const;
1680 
1681  std::string getStateTreeString(const std::string& prefix = "") const;
1682 
1683 private:
1684  void allocMemory();
1685 
1686  void copyFrom(const RobotState& other);
1687 
1689  {
1690  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1692  dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1693  }
1694 
1696  {
1697  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
1698  for (std::size_t i = 0; i < jm.size(); ++i)
1699  dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1701  group->getCommonRoot() :
1702  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1703  }
1704 
1705  void markVelocity();
1706  void markAcceleration();
1707  void markEffort();
1708 
1709  void updateMimicJoint(const JointModel* joint)
1710  {
1711  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
1712  double v = position_[joint->getFirstVariableIndex()];
1713  for (std::size_t i = 0; i < mim.size(); ++i)
1714  {
1715  position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1716  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1717  }
1718  }
1719 
1721  void updateMimicJoint(const std::vector<const JointModel*>& mim)
1722  {
1723  for (std::size_t i = 0; i < mim.size(); ++i)
1724  {
1725  const int fvi = mim[i]->getFirstVariableIndex();
1726  position_[fvi] =
1727  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1728  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1729  }
1730  }
1731 
1733 
1734  void getMissingKeys(const std::map<std::string, double>& variable_map,
1735  std::vector<std::string>& missing_variables) const;
1736  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1737 
1747  double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, double jump_threshold);
1748 
1750  bool checkJointTransforms(const JointModel* joint) const;
1751 
1753  bool checkLinkTransforms() const;
1754 
1756  bool checkCollisionTransforms() const;
1757 
1758  RobotModelConstPtr robot_model_;
1759  void* memory_;
1760 
1761  double* position_;
1762  double* velocity_;
1763  double* acceleration_;
1764  double* effort_;
1768 
1771 
1772  Eigen::Affine3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned
1773  Eigen::Affine3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned
1774  Eigen::Affine3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned
1775  unsigned char* dirty_joint_transforms_;
1776 
1778  std::map<std::string, AttachedBody*> attached_body_map_;
1779 
1783 
1790 };
1791 
1793 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1794 }
1795 }
1796 
1797 #endif
void attachBody(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &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:1558
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:483
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:309
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:881
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:751
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1611
A set of options for the kinematics solver.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
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:1604
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1709
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:775
Core components of MoveIt!
const Eigen::Affine3d & getGlobalLinkTransform(const LinkModel *link) const
Definition: robot_state.h:1352
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:1664
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:594
Eigen::Affine3d * global_collision_body_transforms_
Definition: robot_state.h:1774
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:796
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:857
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 ...
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...
void printTransforms(std::ostream &out=std::cout) const
ROSCPP_DECL void start()
double distance(const RobotState &other) const
Definition: robot_state.h:1407
void printStatePositions(std::ostream &out=std::cout) const
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:645
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:867
const Eigen::Affine3d & getJointTransform(const JointModel *joint)
Definition: robot_state.h:1335
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
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:379
void setJointEfforts(const JointModel *joint, const double *effort)
Definition: robot_state.h:523
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:294
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name) const
Definition: robot_state.h:1347
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:409
bool knowsFrameTransform(const std::string &id) const
Check if a transformation matrix from the model frame to frame id is known.
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1308
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1449
const Eigen::Affine3d & getJointTransform(const std::string &joint_name) const
Definition: robot_state.h:1369
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:383
const Eigen::Affine3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
Definition: robot_state.h:1363
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:844
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:545
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:127
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:243
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...
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1770
bool checkCollisionTransforms() const
This function is only called in debug mode.
const Eigen::Affine3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
Definition: robot_state.h:1358
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, 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...
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:222
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:761
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1457
void clearAttachedBodies()
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
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:160
const Eigen::Affine3d & getFrameTransform(const std::string &id)
Get the transformation matrix from the model frame to the frame identified by id. ...
void updateLinkTransformsInternal(const JointModel *start)
bool checkLinkTransforms() const
This function is only called in debug mode.
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 setJointPositions(const JointModel *joint, const double *position)
Definition: robot_state.h:498
Eigen::Affine3d * global_link_transforms_
Definition: robot_state.h:1773
const double * getJointEffort(const JointModel *joint) const
Definition: robot_state.h:570
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:690
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:310
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:669
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:140
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...
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:372
bool satisfiesBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1464
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
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
void getMissingKeys(const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) const
bool setToIKSolverFrame(Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr &solver)
Convert the frame of reference of the pose to that same frame as the IK solver expects.
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1688
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:632
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:458
int getFirstCollisionBodyTransformIndex() const
Definition: link_model.h:97
void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback)
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:604
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
Definition: robot_state.h:488
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...
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1472
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1468
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:1206
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void copyFrom(const RobotState &other)
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:565
const Eigen::Affine3d & getJointTransform(const JointModel *joint) const
Definition: robot_state.h:1374
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:710
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Definition: link_model.h:87
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:451
bool hasAttachedBody(const std::string &id) const
Check if an attached body named id exists in this state.
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:700
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
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:806
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:432
const Eigen::Affine3d & getCollisionBodyTransforms(const std::string &link_name, std::size_t index)
Definition: robot_state.h:1319
void enforceBounds(const JointModel *joint)
Definition: robot_state.h:1443
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
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...
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_CLASS_FORWARD(RobotModel)
void update(bool force=false)
Update all transforms.
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
Definition: robot_state.h:493
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
double distance(const RobotState &other, const JointModel *joint) const
Definition: robot_state.h:1414
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:343
const Eigen::Affine3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index)
Definition: robot_state.h:1324
void getStateTreeJointString(std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
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:723
double testJointSpaceJump(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double jump_threshold)
Tests joint space jumps of a trajectory. First, the average distance between adjacent trajectory poin...
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:1183
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance)
Set all joints in group to random values near the value in . distance is the maximum amount each join...
unsigned char * dirty_joint_transforms_
Definition: robot_state.h:1775
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:1721
bool dirtyLinkTransforms() const
Definition: robot_state.h:1385
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
Definition: robot_state.h:1300
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:1782
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:829
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
Definition: robot_state.h:147
std::string getStateTreeString(const std::string &prefix="") const
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)...
bool satisfiesBounds(double margin=0.0) const
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:617
void printStateInfo(std::ostream &out=std::cout) const
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts=0, 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...
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:1645
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:97
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:208
RobotState(const RobotModelConstPtr &robot_model)
A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information.
Definition: robot_state.cpp:47
void markDirtyJointTransforms(const JointModelGroup *group)
Definition: robot_state.h:1695
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:229
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.
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:121
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
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:279
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:1220
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:738
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.
virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform.
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1769
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
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:387
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:317
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:115
RobotState & operator=(const RobotState &other)
Copy operator.
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:584
const double * getJointAccelerations(const JointModel *joint) const
Definition: robot_state.h:560
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1789
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:401
void setJointPositions(const JointModel *joint, const Eigen::Affine3d &transform)
Definition: robot_state.h:510
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1239
RobotModelConstPtr robot_model_
Definition: robot_state.h:1758
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:332
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:1433
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:237
void updateCollisionBodyTransforms()
Update the transforms for the collision bodies. This call is needed before calling collision checking...
const double * getJointVelocities(const JointModel *joint) const
Definition: robot_state.h:550
const Eigen::Affine3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1330
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:82
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 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:365
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
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:200
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
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:417
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:423
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:325
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:109
bool dirty() const
Returns true if anything in this state is dirty.
Definition: robot_state.h:1396
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1380
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:816
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:535
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:555
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:181
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...
Main namespace for MoveIt!
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:655
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int attempts=0, 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...
const Eigen::Affine3d & getGlobalLinkTransform(const LinkModel *link)
Definition: robot_state.h:1313
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:473
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:301
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:103
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:286
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
Eigen::Affine3d * variable_joint_transforms_
Definition: robot_state.h:1772
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
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:188
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1390
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:251
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:272
virtual void computeVariablePositions(const Eigen::Affine3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values.
const double * getJointPositions(const JointModel *joint) const
Definition: robot_state.h:540
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 printDirtyInfo(std::ostream &out=std::cout) const
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:1778
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:465
void setJointPositions(const std::string &joint_name, const Eigen::Affine3d &transform)
Definition: robot_state.h:505
bool checkJointTransforms(const JointModel *joint) const
This function is only called in debug mode.
void printTransform(const Eigen::Affine3d &transform, std::ostream &out=std::cout) const
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:517


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44