robot_state.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Ioan A. Sucan
5 * Copyright (c) 2013, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35 
36 /* Author: Ioan Sucan */
37 
38 #ifndef MOVEIT_CORE_ROBOT_STATE_
39 #define MOVEIT_CORE_ROBOT_STATE_
40 
44 #include <sensor_msgs/JointState.h>
45 #include <visualization_msgs/MarkerArray.h>
46 #include <std_msgs/ColorRGBA.h>
47 #include <geometry_msgs/Twist.h>
48 #include <cassert>
49 
50 #include <boost/assert.hpp>
51 
52 namespace moveit
53 {
54 namespace core
55 {
56 MOVEIT_CLASS_FORWARD(RobotState);
57 
62 typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
63  const double* joint_group_variable_values)>
65 
72 {
73  double factor;
74  double revolute;
75  double prismatic;
76 
77  explicit JumpThreshold() : factor(0.0), revolute(0.0), prismatic(0.0)
78  {
79  }
80 
81  explicit JumpThreshold(double jt_factor) : JumpThreshold()
82  {
83  factor = jt_factor;
84  }
85 
86  explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold()
87  {
88  revolute = jt_revolute;
89  prismatic = jt_prismatic;
90  }
91 };
92 
111 {
112 public:
115  RobotState(const RobotModelConstPtr& robot_model);
116  ~RobotState();
117 
119  RobotState(const RobotState& other);
120 
122  RobotState& operator=(const RobotState& other);
123 
125  const RobotModelConstPtr& getRobotModel() const
126  {
127  return robot_model_;
128  }
129 
131  std::size_t getVariableCount() const
132  {
133  return robot_model_->getVariableCount();
134  }
135 
137  const std::vector<std::string>& getVariableNames() const
138  {
139  return robot_model_->getVariableNames();
140  }
141 
143  const LinkModel* getLinkModel(const std::string& link) const
144  {
145  return robot_model_->getLinkModel(link);
146  }
147 
149  const JointModel* getJointModel(const std::string& joint) const
150  {
151  return robot_model_->getJointModel(joint);
152  }
153 
155  const JointModelGroup* getJointModelGroup(const std::string& group) const
156  {
157  return robot_model_->getJointModelGroup(group);
158  }
159 
169  {
170  return position_;
171  }
172 
175  const double* getVariablePositions() const
176  {
177  return position_;
178  }
179 
183  void setVariablePositions(const double* position);
184 
188  void setVariablePositions(const std::vector<double>& position)
189  {
190  assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
191  setVariablePositions(&position[0]);
192  }
193 
196  void setVariablePositions(const std::map<std::string, double>& variable_map);
197 
200  void setVariablePositions(const std::map<std::string, double>& variable_map,
201  std::vector<std::string>& missing_variables);
202 
205  void setVariablePositions(const std::vector<std::string>& variable_names,
206  const std::vector<double>& variable_position);
207 
209  void setVariablePosition(const std::string& variable, double value)
210  {
211  setVariablePosition(robot_model_->getVariableIndex(variable), value);
212  }
213 
216  void setVariablePosition(int index, double value)
217  {
218  position_[index] = value;
219  const JointModel* jm = robot_model_->getJointOfVariable(index);
220  if (jm)
221  {
222  markDirtyJointTransforms(jm);
223  updateMimicJoint(jm);
224  }
225  }
226 
228  double getVariablePosition(const std::string& variable) const
229  {
230  return position_[robot_model_->getVariableIndex(variable)];
231  }
232 
236  double getVariablePosition(int index) const
237  {
238  return position_[index];
239  }
240 
250  bool hasVelocities() const
251  {
252  return has_velocity_;
253  }
254 
258  {
259  markVelocity();
260  return velocity_;
261  }
262 
265  const double* getVariableVelocities() const
266  {
267  return velocity_;
268  }
269 
271  void setVariableVelocities(const double* velocity)
272  {
273  has_velocity_ = true;
274  // assume everything is in order in terms of array lengths (for efficiency reasons)
275  memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
276  }
277 
279  void setVariableVelocities(const std::vector<double>& velocity)
280  {
281  assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
282  setVariableVelocities(&velocity[0]);
283  }
284 
287  void setVariableVelocities(const std::map<std::string, double>& variable_map);
288 
291  void setVariableVelocities(const std::map<std::string, double>& variable_map,
292  std::vector<std::string>& missing_variables);
293 
296  void setVariableVelocities(const std::vector<std::string>& variable_names,
297  const std::vector<double>& variable_velocity);
298 
300  void setVariableVelocity(const std::string& variable, double value)
301  {
302  setVariableVelocity(robot_model_->getVariableIndex(variable), value);
303  }
304 
307  void setVariableVelocity(int index, double value)
308  {
309  markVelocity();
310  velocity_[index] = value;
311  }
312 
314  double getVariableVelocity(const std::string& variable) const
315  {
316  return velocity_[robot_model_->getVariableIndex(variable)];
317  }
318 
322  double getVariableVelocity(int index) const
323  {
324  return velocity_[index];
325  }
326 
337  bool hasAccelerations() const
338  {
339  return has_acceleration_;
340  }
341 
346  {
347  markAcceleration();
348  return acceleration_;
349  }
350 
353  const double* getVariableAccelerations() const
354  {
355  return acceleration_;
356  }
357 
360  void setVariableAccelerations(const double* acceleration)
361  {
362  has_acceleration_ = true;
363  has_effort_ = false;
364 
365  // assume everything is in order in terms of array lengths (for efficiency reasons)
366  memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
367  }
368 
371  void setVariableAccelerations(const std::vector<double>& acceleration)
372  {
373  assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
374  setVariableAccelerations(&acceleration[0]);
375  }
376 
379  void setVariableAccelerations(const std::map<std::string, double>& variable_map);
380 
384  void setVariableAccelerations(const std::map<std::string, double>& variable_map,
385  std::vector<std::string>& missing_variables);
386 
389  void setVariableAccelerations(const std::vector<std::string>& variable_names,
390  const std::vector<double>& variable_acceleration);
391 
393  void setVariableAcceleration(const std::string& variable, double value)
394  {
395  setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
396  }
397 
400  void setVariableAcceleration(int index, double value)
401  {
402  markAcceleration();
403  acceleration_[index] = value;
404  }
405 
407  double getVariableAcceleration(const std::string& variable) const
408  {
409  return acceleration_[robot_model_->getVariableIndex(variable)];
410  }
411 
415  double getVariableAcceleration(int index) const
416  {
417  return acceleration_[index];
418  }
419 
429  bool hasEffort() const
430  {
431  return has_effort_;
432  }
433 
438  {
439  markEffort();
440  return effort_;
441  }
442 
445  const double* getVariableEffort() const
446  {
447  return effort_;
448  }
449 
451  void setVariableEffort(const double* effort)
452  {
453  has_effort_ = true;
454  has_acceleration_ = false;
455  // assume everything is in order in terms of array lengths (for efficiency reasons)
456  memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
457  }
458 
460  void setVariableEffort(const std::vector<double>& effort)
461  {
462  assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
463  setVariableEffort(&effort[0]);
464  }
465 
467  void setVariableEffort(const std::map<std::string, double>& variable_map);
468 
471  void setVariableEffort(const std::map<std::string, double>& variable_map,
472  std::vector<std::string>& missing_variables);
473 
475  void setVariableEffort(const std::vector<std::string>& variable_names,
476  const std::vector<double>& variable_acceleration);
477 
479  void setVariableEffort(const std::string& variable, double value)
480  {
481  setVariableEffort(robot_model_->getVariableIndex(variable), value);
482  }
483 
486  void setVariableEffort(int index, double value)
487  {
488  markEffort();
489  effort_[index] = value;
490  }
491 
493  double getVariableEffort(const std::string& variable) const
494  {
495  return effort_[robot_model_->getVariableIndex(variable)];
496  }
497 
501  double getVariableEffort(int index) const
502  {
503  return effort_[index];
504  }
505 
511  void setJointPositions(const std::string& joint_name, const double* position)
512  {
513  setJointPositions(robot_model_->getJointModel(joint_name), position);
514  }
515 
516  void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
517  {
518  setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
519  }
520 
521  void setJointPositions(const JointModel* joint, const std::vector<double>& position)
522  {
523  setJointPositions(joint, &position[0]);
524  }
525 
526  void setJointPositions(const JointModel* joint, const double* position)
527  {
528  memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
529  markDirtyJointTransforms(joint);
530  updateMimicJoint(joint);
531  }
532 
533  void setJointPositions(const std::string& joint_name, const Eigen::Affine3d& transform)
534  {
535  setJointPositions(robot_model_->getJointModel(joint_name), transform);
536  }
537 
538  void setJointPositions(const JointModel* joint, const Eigen::Affine3d& transform)
539  {
540  joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
541  markDirtyJointTransforms(joint);
542  updateMimicJoint(joint);
543  }
544 
545  void setJointVelocities(const JointModel* joint, const double* velocity)
546  {
547  has_velocity_ = true;
548  memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
549  }
550 
551  void setJointEfforts(const JointModel* joint, const double* effort)
552  {
553  if (has_acceleration_)
554  {
555  ROS_ERROR_NAMED("robot_state", "Unable to set joint efforts because array is being used for accelerations");
556  return;
557  }
558  has_effort_ = true;
559 
560  memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double));
561  }
562 
563  const double* getJointPositions(const std::string& joint_name) const
564  {
565  return getJointPositions(robot_model_->getJointModel(joint_name));
566  }
567 
568  const double* getJointPositions(const JointModel* joint) const
569  {
570  return position_ + joint->getFirstVariableIndex();
571  }
572 
573  const double* getJointVelocities(const std::string& joint_name) const
574  {
575  return getJointVelocities(robot_model_->getJointModel(joint_name));
576  }
577 
578  const double* getJointVelocities(const JointModel* joint) const
579  {
580  return velocity_ + joint->getFirstVariableIndex();
581  }
582 
583  const double* getJointAccelerations(const std::string& joint_name) const
584  {
585  return getJointAccelerations(robot_model_->getJointModel(joint_name));
586  }
587 
588  const double* getJointAccelerations(const JointModel* joint) const
589  {
590  return acceleration_ + joint->getFirstVariableIndex();
591  }
592 
593  const double* getJointEffort(const std::string& joint_name) const
594  {
595  return getJointEffort(robot_model_->getJointModel(joint_name));
596  }
597 
598  const double* getJointEffort(const JointModel* joint) const
599  {
600  return effort_ + joint->getFirstVariableIndex();
601  }
602 
612  void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
613  {
614  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
615  if (jmg)
616  setJointGroupPositions(jmg, gstate);
617  }
618 
622  void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
623  {
624  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
625  if (jmg)
626  setJointGroupPositions(jmg, &gstate[0]);
627  }
628 
632  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
633  {
634  setJointGroupPositions(group, &gstate[0]);
635  }
636 
640  void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
641 
645  void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
646  {
647  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
648  if (jmg)
649  setJointGroupPositions(jmg, values);
650  }
651 
655  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
656 
660  void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
661  {
662  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
663  if (jmg)
664  {
665  gstate.resize(jmg->getVariableCount());
666  copyJointGroupPositions(jmg, &gstate[0]);
667  }
668  }
669 
673  void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
674  {
675  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
676  if (jmg)
677  copyJointGroupPositions(jmg, gstate);
678  }
679 
683  void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
684  {
685  gstate.resize(group->getVariableCount());
686  copyJointGroupPositions(group, &gstate[0]);
687  }
688 
692  void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
693 
697  void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
698  {
699  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
700  if (jmg)
701  copyJointGroupPositions(jmg, values);
702  }
703 
707  void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
708 
718  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
719  {
720  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
721  if (jmg)
722  setJointGroupVelocities(jmg, gstate);
723  }
724 
728  void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
729  {
730  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
731  if (jmg)
732  setJointGroupVelocities(jmg, &gstate[0]);
733  }
734 
738  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
739  {
740  setJointGroupVelocities(group, &gstate[0]);
741  }
742 
746  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
747 
751  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
752  {
753  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
754  if (jmg)
755  setJointGroupVelocities(jmg, values);
756  }
757 
761  void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
762 
766  void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
767  {
768  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
769  if (jmg)
770  {
771  gstate.resize(jmg->getVariableCount());
772  copyJointGroupVelocities(jmg, &gstate[0]);
773  }
774  }
775 
779  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
780  {
781  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
782  if (jmg)
783  copyJointGroupVelocities(jmg, gstate);
784  }
785 
789  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
790  {
791  gstate.resize(group->getVariableCount());
792  copyJointGroupVelocities(group, &gstate[0]);
793  }
794 
798  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
799 
803  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
804  {
805  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
806  if (jmg)
807  copyJointGroupVelocities(jmg, values);
808  }
809 
813  void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
814 
824  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
825  {
826  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
827  if (jmg)
828  setJointGroupAccelerations(jmg, gstate);
829  }
830 
834  void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
835  {
836  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
837  if (jmg)
838  setJointGroupAccelerations(jmg, &gstate[0]);
839  }
840 
844  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
845  {
846  setJointGroupAccelerations(group, &gstate[0]);
847  }
848 
852  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
853 
857  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
858  {
859  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
860  if (jmg)
861  setJointGroupAccelerations(jmg, values);
862  }
863 
867  void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
868 
872  void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
873  {
874  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
875  if (jmg)
876  {
877  gstate.resize(jmg->getVariableCount());
878  copyJointGroupAccelerations(jmg, &gstate[0]);
879  }
880  }
881 
885  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
886  {
887  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
888  if (jmg)
889  copyJointGroupAccelerations(jmg, gstate);
890  }
891 
895  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
896  {
897  gstate.resize(group->getVariableCount());
898  copyJointGroupAccelerations(group, &gstate[0]);
899  }
900 
904  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
905 
909  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
910  {
911  const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
912  if (jmg)
913  copyJointGroupAccelerations(jmg, values);
914  }
915 
919  void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
920 
933  bool setToIKSolverFrame(Eigen::Affine3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
934 
941  bool setToIKSolverFrame(Eigen::Affine3d& pose, const std::string& ik_frame);
942 
950  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts = 0,
951  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
953 
962  bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
963  unsigned int attempts = 0, double timeout = 0.0,
966 
974  bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, unsigned int attempts = 0,
975  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
977 
985  bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, const std::string& tip,
986  unsigned int attempts = 0, double timeout = 0.0,
989 
999  bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, const std::string& tip,
1000  const std::vector<double>& consistency_limits, unsigned int attempts = 0, double timeout = 0.0,
1003 
1014  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
1015  const std::vector<std::string>& tips, unsigned int attempts = 0, double timeout = 0.0,
1018 
1030  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
1031  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
1032  unsigned int attempts = 0, double timeout = 0.0,
1035 
1045  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
1046  const std::vector<std::string>& tips,
1047  const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts = 0,
1048  double timeout = 0.0,
1051 
1059  bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
1061 
1069  bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
1071 
1097  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1098  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1099  double max_step, const JumpThreshold& jump_threshold,
1102 
1103  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1104  const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
1105  double max_step, double jump_threshold_factor,
1108  {
1109  return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance, max_step,
1110  JumpThreshold(jump_threshold_factor), validCallback, options);
1111  }
1112 
1120  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1121  const Eigen::Affine3d& target, bool global_reference_frame, double max_step,
1122  const JumpThreshold& jump_threshold,
1125 
1126  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1127  const Eigen::Affine3d& target, bool global_reference_frame, double max_step,
1128  double jump_threshold_factor,
1131  {
1132  return computeCartesianPath(group, traj, link, target, global_reference_frame, max_step,
1133  JumpThreshold(jump_threshold_factor), validCallback, options);
1134  }
1135 
1142  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1143  const EigenSTL::vector_Affine3d& waypoints, bool global_reference_frame, double max_step,
1144  const JumpThreshold& jump_threshold,
1147 
1148  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
1149  const EigenSTL::vector_Affine3d& waypoints, bool global_reference_frame, double max_step,
1150  double jump_threshold_factor,
1153  {
1154  return computeCartesianPath(group, traj, link, waypoints, global_reference_frame, max_step,
1155  JumpThreshold(jump_threshold_factor), validCallback, options);
1156  }
1157 
1172  static double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1173  const JumpThreshold& jump_threshold);
1174 
1184  static double testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1185  double jump_threshold_factor);
1186 
1198  static double testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1199  double revolute_jump_threshold, double prismatic_jump_threshold);
1200 
1210  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1211  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
1212 
1222  bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
1223  Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
1224  {
1225  updateLinkTransforms();
1226  return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
1227  use_quaternion_representation);
1228  }
1229 
1236  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1237  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
1238 
1245  Eigen::MatrixXd getJacobian(const JointModelGroup* group,
1246  const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
1247  {
1248  updateLinkTransforms();
1249  return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
1250  }
1251 
1254  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1255  const LinkModel* tip) const;
1256 
1259  void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
1260  const LinkModel* tip)
1261  {
1262  updateLinkTransforms();
1263  static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
1264  }
1265 
1269  bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
1271 
1278  void setVariableValues(const sensor_msgs::JointState& msg)
1279  {
1280  if (!msg.position.empty())
1281  setVariablePositions(msg.name, msg.position);
1282  if (!msg.velocity.empty())
1283  setVariableVelocities(msg.name, msg.velocity);
1284  }
1285 
1289  void setToDefaultValues();
1290 
1292  bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
1293 
1295  void setToRandomPositions();
1296 
1298  void setToRandomPositions(const JointModelGroup* group);
1299 
1302  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
1303 
1309  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
1310 
1318  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
1319  const std::vector<double>& distances);
1320 
1329  void updateCollisionBodyTransforms();
1330 
1333  void updateLinkTransforms();
1334 
1336  void update(bool force = false);
1337 
1346  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Affine3d& transform, bool backward = false)
1347  {
1348  updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1349  }
1350 
1352  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Affine3d& transform, bool backward = false);
1353 
1354  const Eigen::Affine3d& getGlobalLinkTransform(const std::string& link_name)
1355  {
1356  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1357  }
1358 
1359  const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel* link)
1360  {
1361  updateLinkTransforms();
1362  return global_link_transforms_[link->getLinkIndex()];
1363  }
1364 
1365  const Eigen::Affine3d& getCollisionBodyTransforms(const std::string& link_name, std::size_t index)
1366  {
1367  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1368  }
1369 
1370  const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
1371  {
1372  updateCollisionBodyTransforms();
1373  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1374  }
1375 
1376  const Eigen::Affine3d& getJointTransform(const std::string& joint_name)
1377  {
1378  return getJointTransform(robot_model_->getJointModel(joint_name));
1379  }
1380 
1381  const Eigen::Affine3d& getJointTransform(const JointModel* joint)
1382  {
1383  const int idx = joint->getJointIndex();
1384  unsigned char& dirty = dirty_joint_transforms_[idx];
1385  if (dirty)
1386  {
1387  joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
1388  dirty = 0;
1389  }
1390  return variable_joint_transforms_[idx];
1391  }
1392 
1393  const Eigen::Affine3d& getGlobalLinkTransform(const std::string& link_name) const
1394  {
1395  return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1396  }
1397 
1398  const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel* link) const
1399  {
1400  BOOST_VERIFY(checkLinkTransforms());
1401  return global_link_transforms_[link->getLinkIndex()];
1402  }
1403 
1404  const Eigen::Affine3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
1405  {
1406  return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1407  }
1408 
1409  const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
1410  {
1411  BOOST_VERIFY(checkCollisionTransforms());
1412  return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
1413  }
1414 
1415  const Eigen::Affine3d& getJointTransform(const std::string& joint_name) const
1416  {
1417  return getJointTransform(robot_model_->getJointModel(joint_name));
1418  }
1419 
1420  const Eigen::Affine3d& getJointTransform(const JointModel* joint) const
1421  {
1422  BOOST_VERIFY(checkJointTransforms(joint));
1423  return variable_joint_transforms_[joint->getJointIndex()];
1424  }
1425 
1426  bool dirtyJointTransform(const JointModel* joint) const
1427  {
1428  return dirty_joint_transforms_[joint->getJointIndex()];
1429  }
1430 
1431  bool dirtyLinkTransforms() const
1432  {
1433  return dirty_link_transforms_;
1434  }
1435 
1437  {
1438  return dirty_link_transforms_ || dirty_collision_body_transforms_;
1439  }
1440 
1442  bool dirty() const
1443  {
1444  return dirtyCollisionBodyTransforms();
1445  }
1446 
1453  double distance(const RobotState& other) const
1454  {
1455  return robot_model_->distance(position_, other.getVariablePositions());
1456  }
1457 
1458  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
1459 
1460  double distance(const RobotState& other, const JointModel* joint) const
1461  {
1462  const int idx = joint->getFirstVariableIndex();
1463  return joint->distance(position_ + idx, other.position_ + idx);
1464  }
1465 
1469  void interpolate(const RobotState& to, double t, RobotState& state) const;
1470 
1474  void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
1475 
1479  void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
1480  {
1481  const int idx = joint->getFirstVariableIndex();
1482  joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
1483  state.markDirtyJointTransforms(joint);
1484  state.updateMimicJoint(joint);
1485  }
1486 
1487  void enforceBounds();
1488  void enforceBounds(const JointModelGroup* joint_group);
1489  void enforceBounds(const JointModel* joint)
1490  {
1491  enforcePositionBounds(joint);
1492  if (has_velocity_)
1493  enforceVelocityBounds(joint);
1494  }
1496  {
1497  if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
1498  {
1499  markDirtyJointTransforms(joint);
1500  updateMimicJoint(joint);
1501  }
1502  }
1504  {
1505  joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
1506  }
1507 
1508  bool satisfiesBounds(double margin = 0.0) const;
1509  bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
1510  bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
1511  {
1512  return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1513  }
1514  bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
1515  {
1516  return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
1517  }
1518  bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
1519  {
1520  return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
1521  }
1522 
1525  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
1526 
1529  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
1530 
1533  std::pair<double, const JointModel*>
1534  getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
1535 
1542  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const;
1543 
1569  void attachBody(AttachedBody* attached_body);
1570 
1585  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1586  const EigenSTL::vector_Affine3d& attach_trans, const std::set<std::string>& touch_links,
1587  const std::string& link_name,
1588  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1589 
1604  void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
1605  const EigenSTL::vector_Affine3d& attach_trans, const std::vector<std::string>& touch_links,
1606  const std::string& link_name,
1607  const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1608  {
1609  std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1610  attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1611  }
1612 
1614  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
1615 
1617  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
1618 
1620  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
1621 
1624  bool clearAttachedBody(const std::string& id);
1625 
1627  void clearAttachedBodies(const LinkModel* link);
1628 
1630  void clearAttachedBodies(const JointModelGroup* group);
1631 
1633  void clearAttachedBodies();
1634 
1636  const AttachedBody* getAttachedBody(const std::string& name) const;
1637 
1639  bool hasAttachedBody(const std::string& id) const;
1640 
1641  void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback);
1646  void computeAABB(std::vector<double>& aabb) const;
1647 
1650  void computeAABB(std::vector<double>& aabb)
1651  {
1652  updateLinkTransforms();
1653  static_cast<const RobotState*>(this)->computeAABB(aabb);
1654  }
1655 
1658  {
1659  if (!rng_)
1661  return *rng_;
1662  }
1663 
1665  const Eigen::Affine3d& getFrameTransform(const std::string& id);
1666 
1668  const Eigen::Affine3d& getFrameTransform(const std::string& id) const;
1669 
1671  bool knowsFrameTransform(const std::string& id) const;
1672 
1680  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1681  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1682  bool include_attached = false) const;
1683 
1691  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1692  const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
1693  bool include_attached = false)
1694  {
1695  updateCollisionBodyTransforms();
1696  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1697  }
1698 
1703  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1704  bool include_attached = false) const;
1705 
1710  void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
1711  bool include_attached = false)
1712  {
1713  updateCollisionBodyTransforms();
1714  static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
1715  }
1716 
1717  void printStatePositions(std::ostream& out = std::cout) const;
1718 
1719  void printStateInfo(std::ostream& out = std::cout) const;
1720 
1721  void printTransforms(std::ostream& out = std::cout) const;
1722 
1723  void printTransform(const Eigen::Affine3d& transform, std::ostream& out = std::cout) const;
1724 
1725  void printDirtyInfo(std::ostream& out = std::cout) const;
1726 
1727  std::string getStateTreeString(const std::string& prefix = "") const;
1728 
1729 private:
1730  void allocMemory();
1731 
1732  void copyFrom(const RobotState& other);
1733 
1735  {
1736  dirty_joint_transforms_[joint->getJointIndex()] = 1;
1737  dirty_link_transforms_ =
1738  dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1739  }
1740 
1742  {
1743  const std::vector<const JointModel*>& jm = group->getActiveJointModels();
1744  for (std::size_t i = 0; i < jm.size(); ++i)
1745  dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1746  dirty_link_transforms_ = dirty_link_transforms_ == NULL ?
1747  group->getCommonRoot() :
1748  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
1749  }
1750 
1751  void markVelocity();
1752  void markAcceleration();
1753  void markEffort();
1754 
1755  void updateMimicJoint(const JointModel* joint)
1756  {
1757  const std::vector<const JointModel*>& mim = joint->getMimicRequests();
1758  double v = position_[joint->getFirstVariableIndex()];
1759  for (std::size_t i = 0; i < mim.size(); ++i)
1760  {
1761  position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1762  markDirtyJointTransforms(mim[i]);
1763  }
1764  }
1765 
1767  /* use updateMimicJoints() instead, which also marks joints dirty */
1768  MOVEIT_DEPRECATED void updateMimicJoint(const std::vector<const JointModel*>& mim)
1769  {
1770  for (std::size_t i = 0; i < mim.size(); ++i)
1771  {
1772  const int fvi = mim[i]->getFirstVariableIndex();
1773  position_[fvi] =
1774  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1775  // Only mark joint transform dirty, but not the associated link transform
1776  // as this function is always used in combination of
1777  // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
1778  dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1779  }
1780  }
1781 
1784  {
1785  for (const JointModel* jm : group->getMimicJointModels())
1786  {
1787  const int fvi = jm->getFirstVariableIndex();
1788  position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
1789  markDirtyJointTransforms(jm);
1790  }
1791  markDirtyJointTransforms(group);
1792  }
1793 
1794  void updateLinkTransformsInternal(const JointModel* start);
1795 
1796  void getMissingKeys(const std::map<std::string, double>& variable_map,
1797  std::vector<std::string>& missing_variables) const;
1798  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
1799 
1801  bool checkJointTransforms(const JointModel* joint) const;
1802 
1804  bool checkLinkTransforms() const;
1805 
1807  bool checkCollisionTransforms() const;
1808 
1809  RobotModelConstPtr robot_model_;
1810  void* memory_;
1811 
1812  double* position_;
1813  double* velocity_;
1814  double* acceleration_;
1815  double* effort_;
1819 
1822 
1823  Eigen::Affine3d* variable_joint_transforms_; // this points to an element in transforms_, so it is aligned
1824  Eigen::Affine3d* global_link_transforms_; // this points to an element in transforms_, so it is aligned
1825  Eigen::Affine3d* global_collision_body_transforms_; // this points to an element in transforms_, so it is aligned
1826  unsigned char* dirty_joint_transforms_;
1827 
1829  std::map<std::string, AttachedBody*> attached_body_map_;
1830 
1834 
1841 };
1842 
1844 std::ostream& operator<<(std::ostream& out, const RobotState& s);
1845 }
1846 }
1847 
1848 #endif
void attachBody(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &attach_trans, const std::vector< std::string > &touch_links, const std::string &link_name, const trajectory_msgs::JointTrajectory &detach_posture=trajectory_msgs::JointTrajectory())
Add an attached body to a link.
Definition: robot_state.h:1604
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:511
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
Definition: robot_state.h:337
void copyJointGroupAccelerations(const std::string &joint_group_name, Eigen::VectorXd &values) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
Definition: robot_state.h:909
void copyJointGroupVelocities(const std::string &joint_group_name, double *gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
Definition: robot_state.h:779
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
Definition: robot_state.h:1657
JumpThreshold(double jt_factor)
Definition: robot_state.h:81
A set of options for the kinematics solver.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
void computeAABB(std::vector< double > &aabb)
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
Definition: robot_state.h:1650
void updateMimicJoint(const JointModel *joint)
Definition: robot_state.h:1755
void copyJointGroupVelocities(const std::string &joint_group_name, Eigen::VectorXd &values) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
Definition: robot_state.h:803
Core components of MoveIt!
const Eigen::Affine3d & getGlobalLinkTransform(const LinkModel *link) const
Definition: robot_state.h:1398
void getRobotMarkers(visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, bool include_attached=false)
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first...
Definition: robot_state.h:1710
void setJointGroupPositions(const std::string &joint_group_name, const std::vector< double > &gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:622
Eigen::Affine3d * global_collision_body_transforms_
Definition: robot_state.h:1825
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:824
void copyJointGroupAccelerations(const std::string &joint_group_name, double *gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
Definition: robot_state.h:885
ROSCPP_DECL void start()
double distance(const RobotState &other) const
Definition: robot_state.h:1453
void copyJointGroupPositions(const std::string &joint_group_name, double *gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:673
void copyJointGroupAccelerations(const JointModelGroup *group, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
Definition: robot_state.h:895
const Eigen::Affine3d & getJointTransform(const JointModel *joint)
Definition: robot_state.h:1381
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:407
void setJointEfforts(const JointModel *joint, const double *effort)
Definition: robot_state.h:551
double getVariableVelocity(int index) const
Get the velocity of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition: robot_state.h:322
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
Definition: robot_state.h:1783
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name) const
Definition: robot_state.h:1393
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:437
const Eigen::Affine3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1354
void enforcePositionBounds(const JointModel *joint)
Definition: robot_state.h:1495
const Eigen::Affine3d & getJointTransform(const std::string &joint_name) const
Definition: robot_state.h:1415
const std::vector< const JointModel * > & getMimicRequests() const
The joint models whose values would be modified if the value of this joint changed.
Definition: joint_model.h:383
const Eigen::Affine3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
Definition: robot_state.h:1409
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:872
const double * getJointVelocities(const std::string &joint_name) const
Definition: robot_state.h:573
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:155
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:271
const JointModel * dirty_collision_body_transforms_
Definition: robot_state.h:1821
const Eigen::Affine3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
Definition: robot_state.h:1404
#define MOVEIT_DEPRECATED
Definition: deprecation.h:48
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
Definition: robot_state.h:250
void copyJointGroupVelocities(const JointModelGroup *group, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
Definition: robot_state.h:789
void enforceVelocityBounds(const JointModel *joint)
Definition: robot_state.h:1503
void setVariablePositions(const std::vector< double > &position)
It is assumed positions is an array containing the new positions for all variables in this state...
Definition: robot_state.h:188
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1126
bool satisfiesPositionBounds(const double *values, double margin=0.0) const
Check if the set of values for the variables of this joint are within bounds.
Definition: joint_model.h:278
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
void setJointPositions(const JointModel *joint, const double *position)
Definition: robot_state.h:526
Eigen::Affine3d * global_link_transforms_
Definition: robot_state.h:1824
const double * getJointEffort(const JointModel *joint) const
Definition: robot_state.h:598
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:718
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
Definition: joint_model.h:310
void copyJointGroupPositions(const std::string &joint_group_name, Eigen::VectorXd &values) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:697
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition: robot_state.h:168
void setVariableAcceleration(int index, double value)
Set the acceleration of a single variable. The variable is specified by its index (a value associated...
Definition: robot_state.h:400
bool satisfiesBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1510
bool enforcePositionBounds(double *values) const
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous joints are made between -Pi and Pi. Returns true if changes were made.
Definition: joint_model.h:290
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1148
void markDirtyJointTransforms(const JointModel *joint)
Definition: robot_state.h:1734
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:660
void setVariableEffort(int index, double value)
Set the effort of a single variable. The variable is specified by its index (a value associated by th...
Definition: robot_state.h:486
int getFirstCollisionBodyTransformIndex() const
Definition: link_model.h:97
void setJointGroupPositions(const JointModelGroup *group, const std::vector< double > &gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:632
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
Definition: robot_state.h:516
virtual void interpolate(const double *from, const double *to, const double t, double *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1518
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
Definition: robot_state.h:1514
Eigen::MatrixXd getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position=Eigen::Vector3d(0.0, 0.0, 0.0))
Compute the Jacobian with reference to the last link of a specified group. If the group is not a chai...
Definition: robot_state.h:1245
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const double * getJointEffort(const std::string &joint_name) const
Definition: robot_state.h:593
const Eigen::Affine3d & getJointTransform(const JointModel *joint) const
Definition: robot_state.h:1420
void setJointGroupVelocities(const JointModelGroup *group, const std::vector< double > &gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
Definition: robot_state.h:738
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
Definition: link_model.h:87
void setVariableEffort(const std::string &variable, double value)
Set the effort of a variable. If an unknown variable name is specified, an exception is thrown...
Definition: robot_state.h:479
void setJointGroupVelocities(const std::string &joint_group_name, const std::vector< double > &gstate)
Given velocities for the variables that make up a group, in the order found in the group (including v...
Definition: robot_state.h:728
void setJointGroupAccelerations(const std::string &joint_group_name, const std::vector< double > &gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
Definition: robot_state.h:834
void setVariableEffort(const std::vector< double > &effort)
Given an array with effort values for all variables, set those values as the effort in this state...
Definition: robot_state.h:460
const Eigen::Affine3d & getCollisionBodyTransforms(const std::string &link_name, std::size_t index)
Definition: robot_state.h:1365
void enforceBounds(const JointModel *joint)
Definition: robot_state.h:1489
int getJointIndex() const
Get the index of this joint within the robot model.
Definition: joint_model.h:214
Struct for containing jump_threshold.
Definition: robot_state.h:71
virtual double distance(const double *value1, const double *value2) const =0
Compute the distance between two joint states of the same model (represented by the variable values) ...
MOVEIT_CLASS_FORWARD(RobotModel)
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
Definition: robot_state.h:521
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
double distance(const RobotState &other, const JointModel *joint) const
Definition: robot_state.h:1460
void setVariableAccelerations(const std::vector< double > &acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Definition: robot_state.h:371
const Eigen::Affine3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index)
Definition: robot_state.h:1370
void setJointGroupVelocities(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given velocities for the variables that make up a group, in the order found in the group (including v...
Definition: robot_state.h:751
bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false)
Compute the Jacobian with reference to a particular point on a given link, for a specified group...
Definition: robot_state.h:1222
unsigned char * dirty_joint_transforms_
Definition: robot_state.h:1826
bool dirtyLinkTransforms() const
Definition: robot_state.h:1431
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
Definition: robot_state.h:1346
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:1833
void setJointGroupAccelerations(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
Definition: robot_state.h:857
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
Definition: robot_state.h:175
void setJointGroupPositions(const std::string &joint_group_name, const Eigen::VectorXd &values)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:645
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
void getRobotMarkers(visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false)
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first...
Definition: robot_state.h:1691
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:125
double getVariablePosition(int index) const
Get the position of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition: robot_state.h:236
void markDirtyJointTransforms(const JointModelGroup *group)
Definition: robot_state.h:1741
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:257
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
Definition: robot_state.h:149
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
void setVariableVelocity(int index, double value)
Set the velocity of a single variable. The variable is specified by its index (a value associated by ...
Definition: robot_state.h:307
void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip)
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and ...
Definition: robot_state.h:1259
JumpThreshold(double jt_revolute, double jt_prismatic)
Definition: robot_state.h:86
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:766
virtual void computeTransform(const double *joint_values, Eigen::Affine3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform.
const JointModel * dirty_link_transforms_
Definition: robot_state.h:1820
int getFirstVariableIndex() const
Get the index of this joint&#39;s first variable within the full robot state.
Definition: joint_model.h:202
double getVariableAcceleration(int index) const
Get the acceleration of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition: robot_state.h:415
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:345
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:143
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:612
const double * getJointAccelerations(const JointModel *joint) const
Definition: robot_state.h:588
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
Definition: robot_state.h:1840
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:429
void setJointPositions(const JointModel *joint, const Eigen::Affine3d &transform)
Definition: robot_state.h:538
RobotModelConstPtr robot_model_
Definition: robot_state.h:1809
void setVariableValues(const sensor_msgs::JointState &msg)
Definition: robot_state.h:1278
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:360
#define ROS_ERROR_NAMED(name,...)
void interpolate(const RobotState &to, double t, RobotState &state, const JointModel *joint) const
Update state by interpolating form this state towards to, at time t in [0,1] but only for the joint j...
Definition: robot_state.h:1479
const double * getVariableVelocities() const
Get const access to the velocities of the variables that make up this state. The values are in the sa...
Definition: robot_state.h:265
const double * getJointVelocities(const JointModel *joint) const
Definition: robot_state.h:578
const Eigen::Affine3d & getJointTransform(const std::string &joint_name)
Definition: robot_state.h:1376
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:110
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
void setVariableAcceleration(const std::string &variable, double value)
Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown...
Definition: robot_state.h:393
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:228
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
const double * getVariableEffort() const
Get const raw access to the effort of the variables that make up this state. The values are in the sa...
Definition: robot_state.h:445
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:451
const double * getVariableAccelerations() const
Get const raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames()
Definition: robot_state.h:353
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:137
bool dirty() const
Returns true if anything in this state is dirty.
Definition: robot_state.h:1442
bool dirtyJointTransform(const JointModel *joint) const
Definition: robot_state.h:1426
void setJointGroupAccelerations(const JointModelGroup *group, const std::vector< double > &gstate)
Given accelerations for the variables that make up a group, in the order found in the group (includin...
Definition: robot_state.h:844
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:563
const double * getJointAccelerations(const std::string &joint_name) const
Definition: robot_state.h:583
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:209
Main namespace for MoveIt!
void copyJointGroupPositions(const JointModelGroup *group, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:683
const Eigen::Affine3d & getGlobalLinkTransform(const LinkModel *link)
Definition: robot_state.h:1359
double getVariableEffort(int index) const
Get the effort of a particular variable. The variable is specified by its index. No checks are perfor...
Definition: robot_state.h:501
bool satisfiesVelocityBounds(const double *values, double margin=0.0) const
Check if the set of velocities for the variables of this joint are within bounds. ...
Definition: joint_model.h:301
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:131
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:314
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
Eigen::Affine3d * variable_joint_transforms_
Definition: robot_state.h:1823
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
void setVariablePosition(int index, double value)
Set the position of a single variable. The variable is specified by its index (a value associated by ...
Definition: robot_state.h:216
bool dirtyCollisionBodyTransforms() const
Definition: robot_state.h:1436
void setVariableVelocities(const std::vector< double > &velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
Definition: robot_state.h:279
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:300
virtual void computeVariablePositions(const Eigen::Affine3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values.
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
const double * getJointPositions(const JointModel *joint) const
Definition: robot_state.h:568
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:64
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Definition: robot_state.h:1103
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:1829
double getVariableEffort(const std::string &variable) const
Get the effort of a particular variable. An exception is thrown if the variable is not known...
Definition: robot_state.h:493
void setJointPositions(const std::string &joint_name, const Eigen::Affine3d &transform)
Definition: robot_state.h:533
MOVEIT_DEPRECATED void updateMimicJoint(const std::vector< const JointModel * > &mim)
Update a set of joints that are certain to be mimicking other joints.
Definition: robot_state.h:1768
void setJointVelocities(const JointModel *joint, const double *velocity)
Definition: robot_state.h:545


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu May 24 2018 02:48:36