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
1075  setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
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
1098  setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
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 
1114  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1116 
1124  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1126 
1160  [[deprecated]] double
1161  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1162  const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step,
1163  double jump_threshold_factor,
1166 
1176  [[deprecated]] double
1177  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1178  const Eigen::Isometry3d& target, bool global_reference_frame, double max_step,
1179  double jump_threshold_factor,
1182 
1192  [[deprecated]] double
1193  computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1194  const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step,
1195  double jump_threshold_factor,
1198 
1208  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1209  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1210 
1220  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1221  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1222  {
1224  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1225  use_quaternion_representation);
1226  }
1227 
1234  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1235  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1236 
1243  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1244  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1245  {
1247  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1248  }
1249 
1252  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1253  const LinkModel* tip) const;
1254 
1257  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1258  const LinkModel* tip)
1259  {
1261  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1262  }
1263 
1267  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1269 
1276  void setVariableValues(const sensor_msgs::JointState& msg)
1277  {
1278  if (!msg.position.empty())
1279  setVariablePositions(msg.name, msg.position);
1280  if (!msg.velocity.empty())
1281  setVariableVelocities(msg.name, msg.velocity);
1282  }
1283 
1287  void setToDefaultValues();
1288 
1290  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1291 
1293  void setToRandomPositions();
1294 
1296  void setToRandomPositions(const JointModelGroup* group);
1297 
1300  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1301 
1307  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance);
1308 
1316  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
1317  const std::vector<double>& distances);
1318 
1328 
1331  void updateLinkTransforms();
1332 
1334  void update(bool force = false);
1335 
1344  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
1345  {
1346  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1347  }
1348 
1350  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
1351 
1360  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
1361  {
1362  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1363  }
1364 
1365  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
1366  {
1368  return global_link_transforms_[link->getLinkIndex()];
1369  }
1370 
1371  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
1372  {
1373  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1374  }
1375 
1376  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
1377  {
1378  BOOST_VERIFY(checkLinkTransforms());
1379  return global_link_transforms_[link->getLinkIndex()];
1380  }
1381 
1392  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
1393  {
1394  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1395  }
1396 
1397  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1398  {
1400  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1401  }
1402 
1403  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1404  {
1405  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1406  }
1407 
1408  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1409  {
1410  BOOST_VERIFY(checkCollisionTransforms());
1411  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1412  }
1413 
1414  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
1415  {
1416  return getJointTransform(robot_model_->getJointModel(joint_name));
1417  }
1418 
1419  const Eigen::Isometry3d& getJointTransform(const JointModel* joint)
1420  {
1421  const int idx = joint->getJointIndex();
1422  unsigned char& dirty = dirty_joint_transforms_[idx];
1423  if (dirty)
1424  {
1425  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1426  dirty = 0;
1427  }
1428  return variable_joint_transforms_[idx];
1429  }
1430 
1431  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
1432  {
1433  return getJointTransform(robot_model_->getJointModel(joint_name));
1434  }
1435 
1436  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
1437  {
1438  BOOST_VERIFY(checkJointTransforms(joint));
1439  return variable_joint_transforms_[joint->getJointIndex()];
1440  }
1441 
1442  bool dirtyJointTransform(const JointModel* joint) const
1443  {
1444  return dirty_joint_transforms_[joint->getJointIndex()];
1445  }
1446 
1447  bool dirtyLinkTransforms() const
1448  {
1449  return dirty_link_transforms_;
1450  }
1451 
1452  bool dirtyCollisionBodyTransforms() const
1453  {
1455  }
1456 
1458  bool dirty() const
1459  {
1461  }
1462 
1470  double distance(const RobotState& other) const
1471  {
1472  return robot_model_->distance(position_, other.getVariablePositions());
1473  }
1476  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1477 
1479  double distance(const RobotState& other, const JointModel* joint) const
1480  {
1481  const int idx = joint->getFirstVariableIndex();
1482  return joint->distance(position_ + idx, other.position_ + idx);
1483  }
1484 
1493  void interpolate(const RobotState& to, double t, RobotState& state) const;
1494 
1504  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1505 
1515  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1516  {
1517  const int idx = joint->getFirstVariableIndex();
1518  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1519  state.markDirtyJointTransforms(joint);
1520  state.updateMimicJoint(joint);
1521  }
1522 
1523  void enforceBounds();
1524  void enforceBounds(const JointModelGroup* joint_group);
1525  void enforceBounds(const JointModel* joint)
1526  {
1527  enforcePositionBounds(joint);
1528  if (has_velocity_)
1529  enforceVelocityBounds(joint);
1530  }
1531  void enforcePositionBounds(const JointModel* joint)
1532  {
1533  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1534  {
1535  markDirtyJointTransforms(joint);
1537  }
1538  }
1539 
1541  void harmonizePositions();
1542  void harmonizePositions(const JointModelGroup* joint_group);
1543  void harmonizePosition(const JointModel* joint)
1544  {
1545  if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
1546  // no need to mark transforms dirty, as the transform hasn't changed
1547  updateMimicJoint(joint);
1548  }
1549 
1550  void enforceVelocityBounds(const JointModel* joint)
1551  {
1553  }
1554 
1555  bool satisfiesBounds(double margin = 0.0) const;
1556  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1557  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1558  {
1559  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1560  }
1561  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1562  {
1563  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1564  }
1565  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1566  {
1567  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1568  }
1569 
1572  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1573 
1576  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1577 
1580  std::pair<double, const JointModel*>
1581  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1582 
1589  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1590 
1616  void attachBody(AttachedBody* attached_body);
1617 
1634  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1635  const EigenSTL::vector_Isometry3d& shape_poses, const std::set<std::string>& touch_links,
1636  const std::string& link_name,
1637  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
1639 
1656  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1657  const EigenSTL::vector_Isometry3d& shape_poses, const std::vector<std::string>& touch_links,
1658  const std::string& link_name,
1659  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory(),
1661  {
1662  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1663  attachBody(id, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses);
1664  }
1665 
1667  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1668 
1670  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;
1671 
1673  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) const;
1674 
1677  bool clearAttachedBody(const std::string& id);
1678 
1680  void clearAttachedBodies(const LinkModel* link);
1681 
1683  void clearAttachedBodies(const JointModelGroup* group);
1684 
1686  void clearAttachedBodies();
1687 
1689  const AttachedBody* getAttachedBody(const std::string& name) const;
1690 
1692  bool hasAttachedBody(const std::string& id) const;
1693 
1699  void computeAABB(std::vector<double>& aabb) const;
1700 
1703  void computeAABB(std::vector<double>& aabb)
1704  {
1706  static_cast<const RobotState*>(this)->computeAABB(aabb);
1707  }
1708 
1711  {
1712  if (!rng_)
1714  return *rng_;
1715  }
1716 
1722  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr);
1723 
1729  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const;
1730 
1737  const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
1738  bool& frame_found) const;
1739 
1741  bool knowsFrameTransform(const std::string& frame_id) const;
1742 
1750  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1751  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1752  bool include_attached = false) const;
1753 
1761  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1762  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1763  bool include_attached = false)
1764  {
1766  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1767  }
1768 
1773  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1774  bool include_attached = false) const;
1775 
1780  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1781  bool include_attached = false)
1782  {
1784  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1785  }
1786 
1787  void printStatePositions(std::ostream& out = std::cout) const;
1788 
1790  void printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, std::ostream& out = std::cout) const;
1791 
1792  void printStateInfo(std::ostream& out = std::cout) const;
1793 
1794  void printTransforms(std::ostream& out = std::cout) const;
1795 
1796  void printTransform(const Eigen::Isometry3d& transform, std::ostream& out = std::cout) const;
1797 
1798  void printDirtyInfo(std::ostream& out = std::cout) const;
1799 
1800  std::string getStateTreeString(const std::string& prefix = "") const;
1801 
1808  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
1809 
1816  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
1817 
1818 private:
1819  void allocMemory();
1820  void initTransforms();
1821  void copyFrom(const RobotState& other);
1822 
1823  void markDirtyJointTransforms(const JointModel* joint)
1824  {
1825  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1827  dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1828  }
1829 
1830  void markDirtyJointTransforms(const JointModelGroup* group)
1831  {
1832  for (const JointModel* jm : group->getActiveJointModels())
1835  group->getCommonRoot() :
1836  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1837  }
1838 
1839  void markVelocity();
1840  void markAcceleration();
1841  void markEffort();
1842 
1843  void updateMimicJoint(const JointModel* joint)
1844  {
1845  double v = position_[joint->getFirstVariableIndex()];
1846  for (const JointModel* jm : joint->getMimicRequests())
1847  {
1848  position_[jm->getFirstVariableIndex()] = jm->getMimicFactor() * v + jm->getMimicOffset();
1850  }
1851  }
1852 
1854  /* use updateMimicJoints() instead, which also marks joints dirty */
1855  [[deprecated]] void updateMimicJoint(const std::vector<const JointModel*>& mim)
1856  {
1857  for (const JointModel* jm : mim)
1858  {
1859  const int fvi = jm->getFirstVariableIndex();
1861  // Only mark joint transform dirty, but not the associated link transform
1862  // as this function is always used in combination of
1863  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1865  }
1866  }
1867 
1869  void updateMimicJoints(const JointModelGroup* group)
1870  {
1871  for (const JointModel* jm : group->getMimicJointModels())
1872  {
1873  const int fvi = jm->getFirstVariableIndex();
1876  }
1877  markDirtyJointTransforms(group);
1878  }
1879 
1880  void updateLinkTransformsInternal(const JointModel* start);
1881 
1882  void getMissingKeys(const std::map<std::string, double>& variable_map,
1883  std::vector<std::string>& missing_variables) const;
1884  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1885 
1887  bool checkJointTransforms(const JointModel* joint) const;
1888 
1890  bool checkLinkTransforms() const;
1891 
1893  bool checkCollisionTransforms() const;
1894 
1895  RobotModelConstPtr robot_model_;
1896  void* memory_;
1897 
1898  double* position_;
1899  double* velocity_;
1900  double* acceleration_;
1901  double* effort_;
1902  bool has_velocity_;
1903  bool has_acceleration_;
1904  bool has_effort_;
1905 
1908 
1909  // All the following transform variables point into aligned memory in memory_
1910  // They are updated lazily, based on the flags in dirty_joint_transforms_
1911  // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_
1912  Eigen::Isometry3d* variable_joint_transforms_;
1913  Eigen::Isometry3d* global_link_transforms_;
1914  Eigen::Isometry3d* global_collision_body_transforms_;
1915  unsigned char* dirty_joint_transforms_;
1916 
1918  std::map<std::string, AttachedBody*> attached_body_map_;
1919 
1923 
1930 };
1931 
1933 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1934 } // namespace core
1935 } // namespace moveit
moveit::core::RobotState::dropAccelerations
void dropAccelerations()
Remove accelerations from this state (this differs from setting them to zero)
Definition: robot_state.cpp:327
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:245
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:197
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:1536
moveit::core::RobotState::dirtyJointTransform
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1508
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:417
shapes
moveit::core::operator<<
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
Definition: joint_model.cpp:287
moveit::core::RobotState::getVariableVelocity
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:362
moveit::core::RobotState::harmonizePosition
void harmonizePosition(const JointModel *joint)
Definition: robot_state.h:1609
moveit::core::JointModel::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:261
moveit::core::JointModel::getMimicOffset
double getMimicOffset() const
If mimicking a joint, this is the offset added to that joint's value.
Definition: joint_model.h:441
moveit::core::RobotState::getJointModel
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:194
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:176
moveit::core::JointModel::enforceVelocityBounds
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:383
moveit::core::RobotState::setJointGroupVelocities
void setJointGroupVelocities(const std::string &joint_group_name, const double *gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
Definition: robot_state.h: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:1965
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::RobotState::getStateTreeJointString
void getStateTreeJointString(std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const
Definition: robot_state.cpp:2303
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:1142
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:1203
moveit::core::RobotState::zeroAccelerations
void zeroAccelerations()
Set all accelerations to 0.0.
Definition: robot_state.cpp:310
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:1318
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:204
moveit::core::RobotState::dropDynamics
void dropDynamics()
Reduce RobotState to kinematic information (remove velocity, acceleration and effort,...
Definition: robot_state.cpp:337
moveit::core::RobotState::setFromIKSubgroups
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solv...
Definition: robot_state.cpp:1851
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:758
moveit::core::RobotState::getJointTransform
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1480
moveit::core::RobotState::markAcceleration
void markAcceleration()
Definition: robot_state.cpp:284
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:583
moveit::core::JointModel::getMimicFactor
double getMimicFactor() const
If mimicking a joint, this is the multiplicative factor for that joint's value.
Definition: joint_model.h:447
attached_body.h
moveit::core::RobotState::getAttachedBody
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
Definition: robot_state.cpp:1003
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::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:1426
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:1502
moveit::core::RobotState::setToRandomPositions
void setToRandomPositions()
Set all joints to random values. Values will be within default bounds.
Definition: robot_state.cpp:344
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:1442
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:993
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:1565
moveit::core::RobotState::printStatePositions
void printStatePositions(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2125
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:893
moveit::core::RobotState::attachBody
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
Definition: robot_state.cpp:1015
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:1224
moveit::core::RobotState::checkCollisionTransforms
bool checkCollisionTransforms() const
This function is only called in debug mode.
Definition: robot_state.cpp:265
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:1059
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:441
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::harmonizePositions
void harmonizePositions()
Call harmonizePosition() for all joints / all joints in group / given joint.
Definition: robot_state.cpp:881
transforms.h
moveit::core::RobotState::getRandomNumberGenerator
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1776
moveit::core::RobotState::memory_
void * memory_
Definition: robot_state.h:1962
moveit::core::RobotState::printDirtyInfo
void printDirtyInfo(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2184
moveit::core::RobotState::satisfiesPositionBounds
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1627
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:1889
moveit::core::RobotState::markEffort
void markEffort()
Definition: robot_state.cpp:294
moveit::core::RobotState::printTransform
void printTransform(const Eigen::Isometry3d &transform, std::ostream &out=std::cout) const
Definition: robot_state.cpp:2240
moveit::core::RobotState::dirtyLinkTransforms
bool dirtyLinkTransforms() const
Definition: robot_state.h:1513
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:703
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:322
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:1972
moveit::core::RobotState::printTransforms
void printTransforms(std::ostream &out=std::cout) const
Definition: robot_state.cpp:2249
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:170
moveit::core::JointModel::getMimicRequests
const std::vector< const JointModel * > & getMimicRequests() const
The joint models whose values would be modified if the value of this joint changed.
Definition: joint_model.h:456
moveit::core::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:1964
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:408
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 \near. distance is the maximum amount each...
Definition: robot_state.cpp:385
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:1979
moveit::core::RobotState::updateMimicJoints
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
Definition: robot_state.h:1935
moveit::core::RobotState::getStateTreeString
std::string getStateTreeString(const std::string &prefix="") const
Definition: robot_state.cpp:2278
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:2090
moveit::core::RobotState::setJointEfforts
void setJointEfforts(const JointModel *joint, const double *effort)
Definition: robot_state.cpp:550
moveit::core::RobotState::allocMemory
void allocMemory()
Definition: robot_state.cpp:153
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:1995
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:1035
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:1969
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:1426
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:1597
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::attached_body_map_
std::map< std::string, AttachedBody * > attached_body_map_
All attached bodies that are part of this state, indexed by their name.
Definition: robot_state.h:1984
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:2132
moveit::core::RobotState::global_collision_body_transforms_
Eigen::Isometry3d * global_collision_body_transforms_
Transforms from model frame to collision bodies.
Definition: robot_state.h:1980
moveit::core::RobotState::variable_joint_transforms_
Eigen::Isometry3d * variable_joint_transforms_
Local transforms of all joints.
Definition: robot_state.h:1978
moveit::core::RobotState::~RobotState
~RobotState()
Definition: robot_state.cpp:145
moveit::core::RobotState::effort_
double * effort_
Definition: robot_state.h:1967
random_numbers::RandomNumberGenerator
moveit::core::RobotState::has_velocity_
bool has_velocity_
Definition: robot_state.h:1968
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:744
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:1483
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:1524
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:1122
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:716
moveit::core::RobotState::zeroEffort
void zeroEffort()
Set all effort values to 0.0.
Definition: robot_state.cpp:316
moveit::core::RobotState::zeroVelocities
void zeroVelocities()
Set all velocities to 0.0.
Definition: robot_state.cpp:304
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:255
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:435
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:1107
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:1981
moveit::core::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(JointModelGroup)
moveit::core::RobotState::invertVelocity
void invertVelocity()
Invert velocity if present.
Definition: robot_state.cpp:541
moveit::core::RobotState::satisfiesBounds
bool satisfiesBounds(double margin=0.0) const
Definition: robot_state.cpp:849
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:1458
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:1988
moveit::core::RobotState::interpolate
void interpolate(const RobotState &to, double t, RobotState &state) const
Definition: robot_state.cpp:972
moveit::core::RobotState::initTransforms
void initTransforms()
Definition: robot_state.cpp:183
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:1961
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:1410
moveit::core::RobotState::acceleration_
double * acceleration_
Definition: robot_state.h:1966
moveit::core::RobotState::enforceVelocityBounds
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1616
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:1970
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:200
moveit::core::RobotState::setVariableValues
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1342
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:2197
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:940
moveit::core::RobotState::markVelocity
void markVelocity()
Definition: robot_state.cpp:275
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:867
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:2057
moveit::core::RobotState::dropEffort
void dropEffort()
Remove effort values from this state (this differs from setting them to zero)
Definition: robot_state.cpp:332
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:1909
moveit::core::RobotState::dirtyCollisionBodyTransforms
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1518
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:998
moveit::core::RobotState::satisfiesVelocityBounds
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1631
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:1973


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 21 2021 02:23:42