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  {
619  assert(gstate.size() == jmg->getVariableCount());
620  setJointGroupPositions(jmg, &gstate[0]);
621  }
622  }
623 
626  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
627  {
628  assert(gstate.size() == group->getVariableCount());
629  setJointGroupPositions(group, &gstate[0]);
630  }
631 
634  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
635 
638  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
639  {
640  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
641  if (jmg)
642  {
643  assert(values.size() == jmg->getVariableCount());
644  setJointGroupPositions(jmg, values);
645  }
646  }
647 
650  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
651 
655  void setJointGroupActivePositions(const JointModelGroup* group, const std::vector<double>& gstate);
656 
660  void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector<double>& gstate)
661  {
662  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
663  if (jmg)
664  {
665  assert(gstate.size() == jmg->getActiveVariableCount());
666  setJointGroupActivePositions(jmg, gstate);
667  }
668  }
669 
673  void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values);
674 
678  void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
679  {
680  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
681  if (jmg)
682  {
683  assert(values.size() == jmg->getActiveVariableCount());
685  }
686  }
687 
691  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
692  {
693  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
694  if (jmg)
695  {
696  gstate.resize(jmg->getVariableCount());
697  copyJointGroupPositions(jmg, &gstate[0]);
698  }
699  }
700 
704  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
705  {
706  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
707  if (jmg)
708  copyJointGroupPositions(jmg, gstate);
709  }
710 
714  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
715  {
716  gstate.resize(group->getVariableCount());
717  copyJointGroupPositions(group, &gstate[0]);
718  }
719 
723  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
724 
728  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
729  {
730  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
731  if (jmg)
732  copyJointGroupPositions(jmg, values);
733  }
734 
738  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
739 
748  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
749  {
750  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
751  if (jmg)
752  setJointGroupVelocities(jmg, gstate);
753  }
754 
757  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
758  {
759  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
760  if (jmg)
761  setJointGroupVelocities(jmg, &gstate[0]);
762  }
763 
766  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
767  {
768  setJointGroupVelocities(group, &gstate[0]);
769  }
770 
773  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
774 
777  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
778  {
779  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
780  if (jmg)
781  setJointGroupVelocities(jmg, values);
782  }
783 
786  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
787 
791  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
792  {
793  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
794  if (jmg)
795  {
796  gstate.resize(jmg->getVariableCount());
797  copyJointGroupVelocities(jmg, &gstate[0]);
798  }
799  }
800 
804  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
805  {
806  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
807  if (jmg)
808  copyJointGroupVelocities(jmg, gstate);
809  }
810 
814  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
815  {
816  gstate.resize(group->getVariableCount());
817  copyJointGroupVelocities(group, &gstate[0]);
818  }
819 
823  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
824 
828  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
829  {
830  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
831  if (jmg)
833  }
834 
838  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
839 
848  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
849  {
850  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
851  if (jmg)
852  setJointGroupAccelerations(jmg, gstate);
853  }
854 
857  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
858  {
859  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
860  if (jmg)
861  setJointGroupAccelerations(jmg, &gstate[0]);
862  }
863 
866  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
867  {
868  setJointGroupAccelerations(group, &gstate[0]);
869  }
870 
873  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
874 
877  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
878  {
879  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
880  if (jmg)
881  setJointGroupAccelerations(jmg, values);
882  }
883 
886  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
887 
891  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
892  {
893  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
894  if (jmg)
895  {
896  gstate.resize(jmg->getVariableCount());
897  copyJointGroupAccelerations(jmg, &gstate[0]);
898  }
899  }
900 
904  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
905  {
906  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
907  if (jmg)
908  copyJointGroupAccelerations(jmg, gstate);
909  }
910 
914  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
915  {
916  gstate.resize(group->getVariableCount());
917  copyJointGroupAccelerations(group, &gstate[0]);
918  }
919 
923  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
924 
928  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
929  {
930  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
931  if (jmg)
933  }
934 
938  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
939 
952  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, double timeout = 0.0,
955  [[deprecated("The attempts argument is not supported anymore.")]] bool
956  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int /* attempts */,
957  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
959  {
960  return setFromIK(group, pose, timeout, constraint, options);
961  }
962 
970  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
971  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
973  [[deprecated("The attempts argument is not supported anymore.")]] bool
974  setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
975  unsigned int /* attempts */, double timeout = 0.0,
978  {
979  return setFromIK(group, pose, tip, timeout, constraint, options);
980  }
981 
988  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
991  [[deprecated("The attempts argument is not supported anymore.")]] bool
992  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, unsigned int /* attempts */,
993  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
995  {
996  return setFromIK(group, pose, timeout, constraint, options);
997  }
998 
1005  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1006  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1008  [[deprecated("The attempts argument is not supported anymore.")]] bool
1009  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1010  unsigned int /* attempts */, double timeout = 0.0,
1013  {
1014  return setFromIK(group, pose, tip, timeout, constraint, options);
1015  }
1016 
1025  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1026  const std::vector<double>& consistency_limits, double timeout = 0.0,
1029  [[deprecated("The attempts argument is not supported anymore.")]] bool
1030  setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
1031  const std::vector<double>& consistency_limits, unsigned int /* attempts */, double timeout = 0.0,
1034  {
1035  return setFromIK(group, pose, tip, consistency_limits, timeout, constraint, options);
1036  }
1037 
1047  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1048  const std::vector<std::string>& tips, double timeout = 0.0,
1051  [[deprecated("The attempts argument is not supported anymore.")]] bool
1052  setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1053  const std::vector<std::string>& tips, unsigned int /* attempts */, double timeout = 0.0,
1056  {
1057  return setFromIK(group, poses, tips, timeout, constraint, options);
1058  }
1059 
1070  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1071  const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
1072  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
1074  [[deprecated("The attempts argument is not supported anymore.")]] bool
1076  const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
1077  unsigned int /* attempts */, double timeout = 0.0,
1080  {
1081  return setFromIK(group, poses, tips, consistency_limits, timeout, constraint, options);
1082  }
1083 
1092  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
1093  const std::vector<std::string>& tips,
1094  const std::vector<std::vector<double>>& consistency_limits, double timeout = 0.0,
1097  [[deprecated("The attempts argument is not supported anymore.")]] bool
1099  const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
1100  unsigned int /* attempts */, double timeout = 0.0,
1103  {
1104  return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint, options);
1105  }
1106 
1113  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1115 
1122  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1124 
1158  [[deprecated]] double
1159  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1160  const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step,
1161  double jump_threshold_factor,
1174  [[deprecated]] double
1175  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1176  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1177  double jump_threshold_factor,
1180 
1190  [[deprecated]] double
1191  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1192  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step,
1193  double jump_threshold_factor,
1196 
1206  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1207  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1208 
1218  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1219  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1220  {
1222  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1223  use_quaternion_representation);
1224  }
1225 
1232  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1233  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1234 
1241  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1242  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1243  {
1245  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1246  }
1247 
1250  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1251  const LinkModel* tip) const;
1252 
1255  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1256  const LinkModel* tip)
1257  {
1259  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1260  }
1261 
1265  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1267 
1274  void setVariableValues(const sensor_msgs::JointState& msg)
1275  {
1276  if (!msg.position.empty())
1277  setVariablePositions(msg.name, msg.position);
1278  if (!msg.velocity.empty())
1279  setVariableVelocities(msg.name, msg.velocity);
1280  }
1281 
1285  void setToDefaultValues();
1286 
1288  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1289 
1290  bool setToDefaultValues(const std::string& group_name, const std::string& state_name)
1291  {
1292  const JointModelGroup* jmg = getJointModelGroup(group_name);
1293  if (jmg)
1294  return setToDefaultValues(jmg, state_name);
1295  else
1296  return false;
1297  }
1298 
1300  void setToRandomPositions();
1301 
1305 
1308 
1312 
1318  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance);
1319 
1325  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance,
1327 
1335  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
1336  const std::vector<double>& distances);
1337 
1345  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
1346  const std::vector<double>& distances, random_numbers::RandomNumberGenerator& rng);
1347 
1357 
1360  void updateLinkTransforms();
1361 
1363  void update(bool force = false);
1364 
1373  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1374  {
1375  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1376  }
1377 
1379  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1380 
1389  getRigidlyConnectedParentLinkModel(const std::string& frame, Eigen::Isometry3d* transform = nullptr,
1390  const moveit::core::JointModelGroup* jmg = nullptr) const;
1391 
1400  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1401  {
1402  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1403  }
1404 
1405  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1406  {
1408  return global_link_transforms_[link->getLinkIndex()];
1409  }
1410 
1411  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1412  {
1413  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1414  }
1415 
1416  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1417  {
1418  BOOST_VERIFY(checkLinkTransforms());
1419  return global_link_transforms_[link->getLinkIndex()];
1420  }
1421 
1432  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1433  {
1434  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1435  }
1436 
1437  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1438  {
1440  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1441  }
1442 
1443  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1444  {
1445  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1446  }
1447 
1448  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1449  {
1450  BOOST_VERIFY(checkCollisionTransforms());
1451  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1452  }
1453 
1454  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1455  {
1456  return getJointTransform(robot_model_->getJointModel(joint_name));
1457  }
1458 
1459  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1460  {
1461  const int idx = joint->getJointIndex();
1462  unsigned char& dirty = dirty_joint_transforms_[idx];
1463  if (dirty)
1464  {
1465  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1466  dirty = 0;
1467  }
1468  return variable_joint_transforms_[idx];
1469  }
1470 
1471  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1472  {
1473  return getJointTransform(robot_model_->getJointModel(joint_name));
1474  }
1475 
1476  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1477  {
1478  BOOST_VERIFY(checkJointTransforms(joint));
1479  return variable_joint_transforms_[joint->getJointIndex()];
1480  }
1481 
1482  bool dirtyJointTransform(const JointModel* joint) const
1483  {
1484  return dirty_joint_transforms_[joint->getJointIndex()];
1485  }
1486 
1487  bool dirtyLinkTransforms() const
1488  {
1489  return dirty_link_transforms_;
1490  }
1491 
1492  bool dirtyCollisionBodyTransforms() const
1493  {
1495  }
1496 
1498  bool dirty() const
1499  {
1501  }
1502 
1510  double distance(const RobotState& other) const
1511  {
1512  return robot_model_->distance(position_, other.getVariablePositions());
1513  }
1516  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1517 
1519  double distance(const RobotState& other, const JointModel* joint) const
1520  {
1521  const int idx = joint->getFirstVariableIndex();
1522  return joint->distance(position_ + idx, other.position_ + idx);
1523  }
1524 
1533  void interpolate(const RobotState& to, double t, RobotState& state) const;
1534 
1544  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1545 
1555  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1556  {
1557  const int idx = joint->getFirstVariableIndex();
1558  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1559  state.markDirtyJointTransforms(joint);
1560  state.updateMimicJoint(joint);
1561  }
1562 
1563  void enforceBounds();
1564  void enforceBounds(const JointModelGroup* joint_group);
1565  void enforceBounds(const JointModel* joint)
1566  {
1567  enforcePositionBounds(joint);
1568  if (has_velocity_)
1569  enforceVelocityBounds(joint);
1570  }
1571  void enforcePositionBounds(const JointModel* joint)
1572  {
1573  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1574  {
1575  markDirtyJointTransforms(joint);
1577  }
1578  }
1579 
1581  void harmonizePositions();
1582  void harmonizePositions(const JointModelGroup* joint_group);
1583  void harmonizePosition(const JointModel* joint)
1584  {
1585  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1586  // no need to mark transforms dirty, as the transform hasn't changed
1587  updateMimicJoint(joint);
1588  }
1589 
1590  void enforceVelocityBounds(const JointModel* joint)
1591  {
1593  }
1594 
1595  bool satisfiesBounds(double margin = 0.0) const;
1596  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1597  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1598  {
1599  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1600  }
1601  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1602  {
1603  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1604  }
1605  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1606  {
1607  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1608  }
1609 
1612  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1613 
1616  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1617 
1620  std::pair<double, const JointModel*>
1621  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1622 
1629  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1630 
1647  void attachBody(std::unique_ptr<AttachedBody> attached_body);
1648 
1668  [[deprecated("Deprecated. Pass a unique_ptr instead")]] void attachBody(AttachedBody* attached_body);
1669 
1687  void attachBody(const std::string& id, const Eigen::Isometry3d& pose,
1688  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
1689  const std::set<std::string>& touch_links, const std::string& link_name,
1690  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
1692 
1710  void attachBody(const std::string& id, const Eigen::Isometry3d& pose,
1711  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
1712  const std::vector<std::string>& touch_links, const std::string& link_name,
1713  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
1715  {
1716  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1717  attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses);
1718  }
1719 
1721  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1722 
1724  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;
1725 
1727  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) const;
1728 
1731  bool clearAttachedBody(const std::string& id);
1732 
1734  void clearAttachedBodies(const LinkModel* link);
1735 
1737  void clearAttachedBodies(const JointModelGroup* group);
1738 
1740  void clearAttachedBodies();
1741 
1743  const AttachedBody* getAttachedBody(const std::string& name) const;
1744 
1746  bool hasAttachedBody(const std::string& id) const;
1747 
1753  void computeAABB(std::vector<double>& aabb) const;
1754 
1757  void computeAABB(std::vector<double>& aabb)
1758  {
1760  static_cast<const RobotState*>(this)->computeAABB(aabb);
1761  }
1762 
1765  {
1766  if (!rng_)
1768  return *rng_;
1769  }
1770 
1776  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr);
1777 
1783  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const;
1784 
1791  const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
1792  bool& frame_found) const;
1793 
1795  bool knowsFrameTransform(const std::string& frame_id) const;
1796 
1804  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1805  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1806  bool include_attached = false) const;
1807 
1815  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1816  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1817  bool include_attached = false)
1818  {
1820  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1821  }
1822 
1827  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1828  bool include_attached = false) const;
1829 
1834  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1835  bool include_attached = false)
1836  {
1838  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1839  }
1840 
1841  void printStatePositions(std::ostream& out = std::cout) const;
1842 
1844  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const;
1845 
1846  void printStateInfo(std::ostream& out = std::cout) const;
1847 
1848  void printTransforms(std::ostream& out = std::cout) const;
1849 
1850  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1851 
1852  void printDirtyInfo(std::ostream& out = std::cout) const;
1853 
1854  std::string getStateTreeString() const;
1855 
1862  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
1863 
1870  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
1871 
1872 private:
1873  void allocMemory();
1874  void initTransforms();
1875  void copyFrom(const RobotState& other);
1876 
1877  void markDirtyJointTransforms(const JointModel* joint)
1878  {
1879  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1881  dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1882  }
1883 
1884  void markDirtyJointTransforms(const JointModelGroup* group)
1885  {
1886  for (const JointModel* jm : group->getActiveJointModels())
1889  group->getCommonRoot() :
1890  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1891  }
1892 
1893  void markVelocity();
1894  void markAcceleration();
1895  void markEffort();
1896 
1897  void updateMimicJoint(const JointModel* joint)
1898  {
1899  double v = position_[joint->getFirstVariableIndex()];
1900  for (const JointModel* jm : joint->getMimicRequests())
1901  {
1902  position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset();
1904  }
1905  }
1906 
1908  /* use updateMimicJoints() instead, which also marks joints dirty */
1909  [[deprecated]] void updateMimicJoint(const std::vector<const JointModel*>& mim)
1910  {
1911  for (const JointModel* jm : mim)
1912  {
1913  const int fvi = jm->getFirstVariableIndex();
1915  // Only mark joint transform dirty, but not the associated link transform
1916  // as this function is always used in combination of
1917  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1919  }
1920  }
1921 
1923  void updateMimicJoints(const JointModelGroup* group)
1924  {
1925  for (const JointModel* jm : group->getMimicJointModels())
1926  {
1927  const int fvi = jm->getFirstVariableIndex();
1930  }
1931  markDirtyJointTransforms(group);
1932  }
1933 
1934  void updateLinkTransformsInternal(const JointModel* start);
1935 
1936  void getMissingKeys(const std::map<std::string, double>& variable_map,
1937  std::vector<std::string>& missing_variables) const;
1938  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1939 
1941  bool checkJointTransforms(const JointModel* joint) const;
1942 
1944  bool checkLinkTransforms() const;
1945 
1947  bool checkCollisionTransforms() const;
1948 
1949  RobotModelConstPtr robot_model_;
1950  void* memory_;
1951 
1952  double* position_;
1953  double* velocity_;
1954  double* acceleration_;
1955  double* effort_;
1956  bool has_velocity_;
1957  bool has_acceleration_;
1958  bool has_effort_;
1959 
1962 
1963  // All the following transform variables point into aligned memory in memory_
1964  // They are updated lazily, based on the flags in dirty_joint_transforms_
1965  // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_
1966  Eigen::Isometry3d* variable_joint_transforms_;
1967  Eigen::Isometry3d* global_link_transforms_;
1968  Eigen::Isometry3d* global_collision_body_transforms_;
1969  unsigned char* dirty_joint_transforms_;
1970 
1972  std::map<std::string, std::unique_ptr<AttachedBody>> attached_body_map_;
1973 
1977 
1984 };
1985 
1987 bool haveSameAttachedObjects(const RobotState& left, const RobotState& right, const std::string& logprefix = "");
1988 
1990 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1991 } // namespace core
1992 } // namespace moveit
moveit::core::RobotState::dropAccelerations
void dropAccelerations()
Remove accelerations from this state (this differs from setting them to zero)
Definition: robot_state.cpp:336
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:83
moveit::core::RobotState::checkJointTransforms
bool checkJointTransforms(const JointModel *joint) const
This function is only called in debug mode.
Definition: robot_state.cpp:254
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::JointModelGroup::getActiveJointModels
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
Definition: joint_model_group.h:222
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:208
moveit::core::RobotState::distance
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. Only considers active joints.
Definition: robot_state.h:1576
moveit::core::RobotState::dirtyJointTransform
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1548
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:443
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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:309
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:1649
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:450
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:814
moveit::core::JointModelGroup::getActiveVariableCount
unsigned int getActiveVariableCount() const
Get the number of variables that describe the active joints in this joint group. This excludes variab...
Definition: joint_model_group.h:482
moveit::core::RobotState::velocity_
double * velocity_
Definition: robot_state.h:2019
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
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...
moveit::core::RobotState::getStateTreeJointString
void getStateTreeJointString(std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
Definition: robot_state.cpp:2354
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:1211
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:1259
moveit::core::RobotState::zeroAccelerations
void zeroAccelerations()
Set all accelerations to 0.0.
Definition: robot_state.cpp:319
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:1374
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:215
moveit::core::RobotState::dropDynamics
void dropDynamics()
Reduce RobotState to kinematic information (remove velocity, acceleration and effort,...
Definition: robot_state.cpp:346
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:784
moveit::core::RobotState::getJointTransform
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1520
moveit::core::RobotState::markAcceleration
void markAcceleration()
Definition: robot_state.cpp:293
moveit::core::RobotState::setJointGroupActivePositions
void setJointGroupActivePositions(const JointModelGroup *group, const std::vector< double > &gstate)
Given positions for the variables of active joints that make up a group, in the order found in the gr...
Definition: robot_state.cpp:609
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:456
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:1075
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:757
moveit::core::RobotState::attached_body_map_
std::map< std::string, std::unique_ptr< AttachedBody > > attached_body_map_
All attached bodies that are part of this state, indexed by their name.
Definition: robot_state.h:2038
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:1466
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:1558
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition: robot_state.cpp:353
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:1498
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:1065
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)
Transform pose from the robot model's base frame to the reference frame of the IK solver.
Definition: robot_state.cpp:1620
moveit::core::RobotState::printStatePositions
void printStatePositions(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2164
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:965
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:1280
moveit::core::RobotState::checkCollisionTransforms
bool checkCollisionTransforms() const
This function is only called in debug mode.
Definition: robot_state.cpp:274
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:1137
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:467
moveit::core::RobotState::getStateTreeString
std::string getStateTreeString() const
Definition: robot_state.cpp:2329
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:957
moveit::core::RobotState::getRigidlyConnectedParentLinkModel
const moveit::core::LinkModel * getRigidlyConnectedParentLinkModel(const std::string &frame, Eigen::Isometry3d *transform=nullptr, const moveit::core::JointModelGroup *jmg=nullptr) const
Get the latest link upwards the kinematic tree which is only connected via fixed joints.
Definition: robot_state.cpp:875
moveit::core::RobotState::harmonizePositions
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
Definition: robot_state.cpp:953
transforms.h
moveit::core::RobotState::getRandomNumberGenerator
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1830
moveit::core::RobotState::memory_
void * memory_
Definition: robot_state.h:2016
moveit::core::RobotState::printDirtyInfo
void printDirtyInfo(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2223
moveit::core::RobotState::satisfiesPositionBounds
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1667
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:125
moveit::core::RobotState::markDirtyJointTransforms
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1943
moveit::core::RobotState::markEffort
void markEffort()
Definition: robot_state.cpp:303
moveit::core::RobotState::printTransform
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
Definition: robot_state.cpp:2279
moveit::core::RobotState::dirtyLinkTransforms
bool dirtyLinkTransforms() const
Definition: robot_state.h:1553
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:729
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:331
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:2026
moveit::core::RobotState::printTransforms
void printTransforms(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2297
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:465
moveit::core::RobotState::attachBody
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
Definition: robot_state.cpp:1087
moveit::core::JointModelGroup::getCommonRoot
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
Definition: joint_model_group.h:272
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:2018
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:434
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::setToRandomPositionsNearBy
void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &seed, double distance)
Set all joints in group to random values near the value in seed. distance is the maximum amount each ...
Definition: robot_state.cpp:405
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:2033
moveit::core::RobotState::updateMimicJoints
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
Definition: robot_state.h:1989
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:2129
moveit::core::RobotState::setJointEfforts
void setJointEfforts(const JointModel *joint, const double *effort)
Definition: robot_state.cpp:576
moveit::core::RobotState::allocMemory
void allocMemory()
Definition: robot_state.cpp:159
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:2049
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:1113
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:2023
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:1482
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:1637
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:1075
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:2171
moveit::core::RobotState::global_collision_body_transforms_
Eigen::Isometry3d * global_collision_body_transforms_
Transforms from model frame to collision bodies.
Definition: robot_state.h:2034
moveit::core::RobotState::variable_joint_transforms_
Eigen::Isometry3d * variable_joint_transforms_
Local transforms of all joints.
Definition: robot_state.h:2032
moveit::core::RobotState::~RobotState
~RobotState()
Definition: robot_state.cpp:151
moveit::core::RobotState::effort_
double * effort_
Definition: robot_state.h:2021
random_numbers::RandomNumberGenerator
moveit::core::RobotState::has_velocity_
bool has_velocity_
Definition: robot_state.h:2022
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:770
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:1539
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:1564
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:1191
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:742
moveit::core::RobotState::zeroEffort
void zeroEffort()
Set all effort values to 0.0.
Definition: robot_state.cpp:325
moveit::core::RobotState::zeroVelocities
void zeroVelocities()
Set all velocities to 0.0.
Definition: robot_state.cpp:313
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:264
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
moveit::core::JointModel::getMimic
const JointModel * getMimic() const
Get the joint this one is mimicking.
Definition: joint_model.h:444
kinematics::KinematicsQueryOptions
A set of options for the kinematics solver.
Definition: kinematics_base.h:109
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:1177
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:2035
moveit::core::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(JointModelGroup)
moveit::core::RobotState::invertVelocity
void invertVelocity()
Invert velocity if present.
Definition: robot_state.cpp:567
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
Definition: robot_state.cpp:921
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:1498
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:857
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:2042
moveit::core::RobotState::interpolate
void interpolate(const RobotState &to, double t, RobotState &state) const
Definition: robot_state.cpp:1044
moveit::core::RobotState::initTransforms
void initTransforms()
Definition: robot_state.cpp:189
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:914
moveit::core::RobotState::robot_model_
RobotModelConstPtr robot_model_
Definition: robot_state.h:2015
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:1439
moveit::core::RobotState::acceleration_
double * acceleration_
Definition: robot_state.h:2020
moveit::core::RobotState::enforceVelocityBounds
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1656
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:2024
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::haveSameAttachedObjects
bool haveSameAttachedObjects(const RobotState &left, const RobotState &right, const std::string &logprefix="")
Definition: robot_state.cpp:2390
moveit::core::RobotState::setVariableValues
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1340
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:2236
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:1012
moveit::core::RobotState::markVelocity
void markVelocity()
Definition: robot_state.cpp:284
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:939
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:2096
moveit::core::RobotState::dropEffort
void dropEffort()
Remove effort values from this state (this differs from setting them to zero)
Definition: robot_state.cpp:341
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:1963
moveit::core::RobotState::dirtyCollisionBodyTransforms
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1558
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:1070
moveit::core::RobotState::satisfiesVelocityBounds
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1671
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:2027


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Mar 3 2024 03:23:35