robot_state.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Ioan A. Sucan
00005 *  Copyright (c) 2013, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
00037 
00038 #ifndef MOVEIT_CORE_ROBOT_STATE_
00039 #define MOVEIT_CORE_ROBOT_STATE_
00040 
00041 #include <moveit/robot_model/robot_model.h>
00042 #include <moveit/robot_state/attached_body.h>
00043 #include <sensor_msgs/JointState.h>
00044 #include <visualization_msgs/MarkerArray.h>
00045 #include <std_msgs/ColorRGBA.h>
00046 #include <geometry_msgs/Twist.h>
00047 #include <cassert>
00048 
00049 #include <boost/assert.hpp>
00050 
00051 namespace moveit
00052 {
00053 namespace core
00054 {
00055 MOVEIT_CLASS_FORWARD(RobotState);
00056 
00061 typedef boost::function<bool(RobotState* robot_state, const JointModelGroup* joint_group,
00062                              const double* joint_group_variable_values)>
00063     GroupStateValidityCallbackFn;
00064 
00082 class RobotState
00083 {
00084 public:
00087   RobotState(const RobotModelConstPtr& robot_model);
00088   ~RobotState();
00089 
00091   RobotState(const RobotState& other);
00092 
00094   RobotState& operator=(const RobotState& other);
00095 
00097   const RobotModelConstPtr& getRobotModel() const
00098   {
00099     return robot_model_;
00100   }
00101 
00103   std::size_t getVariableCount() const
00104   {
00105     return robot_model_->getVariableCount();
00106   }
00107 
00109   const std::vector<std::string>& getVariableNames() const
00110   {
00111     return robot_model_->getVariableNames();
00112   }
00113 
00115   const LinkModel* getLinkModel(const std::string& link) const
00116   {
00117     return robot_model_->getLinkModel(link);
00118   }
00119 
00121   const JointModel* getJointModel(const std::string& joint) const
00122   {
00123     return robot_model_->getJointModel(joint);
00124   }
00125 
00127   const JointModelGroup* getJointModelGroup(const std::string& group) const
00128   {
00129     return robot_model_->getJointModelGroup(group);
00130   }
00131 
00140   double* getVariablePositions()
00141   {
00142     return position_;
00143   }
00144 
00147   const double* getVariablePositions() const
00148   {
00149     return position_;
00150   }
00151 
00155   void setVariablePositions(const double* position);
00156 
00160   void setVariablePositions(const std::vector<double>& position)
00161   {
00162     assert(robot_model_->getVariableCount() <= position.size());  // checked only in debug mode
00163     setVariablePositions(&position[0]);
00164   }
00165 
00168   void setVariablePositions(const std::map<std::string, double>& variable_map);
00169 
00172   void setVariablePositions(const std::map<std::string, double>& variable_map,
00173                             std::vector<std::string>& missing_variables);
00174 
00177   void setVariablePositions(const std::vector<std::string>& variable_names,
00178                             const std::vector<double>& variable_position);
00179 
00181   void setVariablePosition(const std::string& variable, double value)
00182   {
00183     setVariablePosition(robot_model_->getVariableIndex(variable), value);
00184   }
00185 
00188   void setVariablePosition(int index, double value)
00189   {
00190     position_[index] = value;
00191     const JointModel* jm = robot_model_->getJointOfVariable(index);
00192     if (jm)
00193     {
00194       markDirtyJointTransforms(jm);
00195       updateMimicJoint(jm);
00196     }
00197   }
00198 
00200   double getVariablePosition(const std::string& variable) const
00201   {
00202     return position_[robot_model_->getVariableIndex(variable)];
00203   }
00204 
00208   double getVariablePosition(int index) const
00209   {
00210     return position_[index];
00211   }
00212 
00222   bool hasVelocities() const
00223   {
00224     return has_velocity_;
00225   }
00226 
00229   double* getVariableVelocities()
00230   {
00231     markVelocity();
00232     return velocity_;
00233   }
00234 
00237   const double* getVariableVelocities() const
00238   {
00239     return velocity_;
00240   }
00241 
00243   void setVariableVelocities(const double* velocity)
00244   {
00245     has_velocity_ = true;
00246     // assume everything is in order in terms of array lengths (for efficiency reasons)
00247     memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
00248   }
00249 
00251   void setVariableVelocities(const std::vector<double>& velocity)
00252   {
00253     assert(robot_model_->getVariableCount() <= velocity.size());  // checked only in debug mode
00254     setVariableVelocities(&velocity[0]);
00255   }
00256 
00259   void setVariableVelocities(const std::map<std::string, double>& variable_map);
00260 
00263   void setVariableVelocities(const std::map<std::string, double>& variable_map,
00264                              std::vector<std::string>& missing_variables);
00265 
00268   void setVariableVelocities(const std::vector<std::string>& variable_names,
00269                              const std::vector<double>& variable_velocity);
00270 
00272   void setVariableVelocity(const std::string& variable, double value)
00273   {
00274     setVariableVelocity(robot_model_->getVariableIndex(variable), value);
00275   }
00276 
00279   void setVariableVelocity(int index, double value)
00280   {
00281     markVelocity();
00282     velocity_[index] = value;
00283   }
00284 
00286   double getVariableVelocity(const std::string& variable) const
00287   {
00288     return velocity_[robot_model_->getVariableIndex(variable)];
00289   }
00290 
00294   double getVariableVelocity(int index) const
00295   {
00296     return velocity_[index];
00297   }
00298 
00309   bool hasAccelerations() const
00310   {
00311     return has_acceleration_;
00312   }
00313 
00317   double* getVariableAccelerations()
00318   {
00319     markAcceleration();
00320     return acceleration_;
00321   }
00322 
00325   const double* getVariableAccelerations() const
00326   {
00327     return acceleration_;
00328   }
00329 
00332   void setVariableAccelerations(const double* acceleration)
00333   {
00334     has_acceleration_ = true;
00335     has_effort_ = false;
00336 
00337     // assume everything is in order in terms of array lengths (for efficiency reasons)
00338     memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
00339   }
00340 
00343   void setVariableAccelerations(const std::vector<double>& acceleration)
00344   {
00345     assert(robot_model_->getVariableCount() <= acceleration.size());  // checked only in debug mode
00346     setVariableAccelerations(&acceleration[0]);
00347   }
00348 
00351   void setVariableAccelerations(const std::map<std::string, double>& variable_map);
00352 
00356   void setVariableAccelerations(const std::map<std::string, double>& variable_map,
00357                                 std::vector<std::string>& missing_variables);
00358 
00361   void setVariableAccelerations(const std::vector<std::string>& variable_names,
00362                                 const std::vector<double>& variable_acceleration);
00363 
00365   void setVariableAcceleration(const std::string& variable, double value)
00366   {
00367     setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
00368   }
00369 
00372   void setVariableAcceleration(int index, double value)
00373   {
00374     markAcceleration();
00375     acceleration_[index] = value;
00376   }
00377 
00379   double getVariableAcceleration(const std::string& variable) const
00380   {
00381     return acceleration_[robot_model_->getVariableIndex(variable)];
00382   }
00383 
00387   double getVariableAcceleration(int index) const
00388   {
00389     return acceleration_[index];
00390   }
00391 
00401   bool hasEffort() const
00402   {
00403     return has_effort_;
00404   }
00405 
00409   double* getVariableEffort()
00410   {
00411     markEffort();
00412     return effort_;
00413   }
00414 
00417   const double* getVariableEffort() const
00418   {
00419     return effort_;
00420   }
00421 
00423   void setVariableEffort(const double* effort)
00424   {
00425     has_effort_ = true;
00426     has_acceleration_ = false;
00427     // assume everything is in order in terms of array lengths (for efficiency reasons)
00428     memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
00429   }
00430 
00432   void setVariableEffort(const std::vector<double>& effort)
00433   {
00434     assert(robot_model_->getVariableCount() <= effort.size());  // checked only in debug mode
00435     setVariableEffort(&effort[0]);
00436   }
00437 
00439   void setVariableEffort(const std::map<std::string, double>& variable_map);
00440 
00443   void setVariableEffort(const std::map<std::string, double>& variable_map,
00444                          std::vector<std::string>& missing_variables);
00445 
00447   void setVariableEffort(const std::vector<std::string>& variable_names,
00448                          const std::vector<double>& variable_acceleration);
00449 
00451   void setVariableEffort(const std::string& variable, double value)
00452   {
00453     setVariableEffort(robot_model_->getVariableIndex(variable), value);
00454   }
00455 
00458   void setVariableEffort(int index, double value)
00459   {
00460     markEffort();
00461     effort_[index] = value;
00462   }
00463 
00465   double getVariableEffort(const std::string& variable) const
00466   {
00467     return effort_[robot_model_->getVariableIndex(variable)];
00468   }
00469 
00473   double getVariableEffort(int index) const
00474   {
00475     return effort_[index];
00476   }
00477 
00483   void setJointPositions(const std::string& joint_name, const double* position)
00484   {
00485     setJointPositions(robot_model_->getJointModel(joint_name), position);
00486   }
00487 
00488   void setJointPositions(const std::string& joint_name, const std::vector<double>& position)
00489   {
00490     setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
00491   }
00492 
00493   void setJointPositions(const JointModel* joint, const std::vector<double>& position)
00494   {
00495     setJointPositions(joint, &position[0]);
00496   }
00497 
00498   void setJointPositions(const JointModel* joint, const double* position)
00499   {
00500     memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
00501     markDirtyJointTransforms(joint);
00502     updateMimicJoint(joint);
00503   }
00504 
00505   void setJointPositions(const std::string& joint_name, const Eigen::Affine3d& transform)
00506   {
00507     setJointPositions(robot_model_->getJointModel(joint_name), transform);
00508   }
00509 
00510   void setJointPositions(const JointModel* joint, const Eigen::Affine3d& transform)
00511   {
00512     joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
00513     markDirtyJointTransforms(joint);
00514     updateMimicJoint(joint);
00515   }
00516 
00517   void setJointVelocities(const JointModel* joint, const double* velocity)
00518   {
00519     has_velocity_ = true;
00520     memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
00521   }
00522 
00523   void setJointEfforts(const JointModel* joint, const double* effort)
00524   {
00525     if (has_acceleration_)
00526     {
00527       logError("Unable to set joint efforts because array is being used for accelerations");
00528       return;
00529     }
00530     has_effort_ = true;
00531 
00532     memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double));
00533   }
00534 
00535   const double* getJointPositions(const std::string& joint_name) const
00536   {
00537     return getJointPositions(robot_model_->getJointModel(joint_name));
00538   }
00539 
00540   const double* getJointPositions(const JointModel* joint) const
00541   {
00542     return position_ + joint->getFirstVariableIndex();
00543   }
00544 
00545   const double* getJointVelocities(const std::string& joint_name) const
00546   {
00547     return getJointVelocities(robot_model_->getJointModel(joint_name));
00548   }
00549 
00550   const double* getJointVelocities(const JointModel* joint) const
00551   {
00552     return velocity_ + joint->getFirstVariableIndex();
00553   }
00554 
00555   const double* getJointAccelerations(const std::string& joint_name) const
00556   {
00557     return getJointAccelerations(robot_model_->getJointModel(joint_name));
00558   }
00559 
00560   const double* getJointAccelerations(const JointModel* joint) const
00561   {
00562     return acceleration_ + joint->getFirstVariableIndex();
00563   }
00564 
00565   const double* getJointEffort(const std::string& joint_name) const
00566   {
00567     return getJointEffort(robot_model_->getJointModel(joint_name));
00568   }
00569 
00570   const double* getJointEffort(const JointModel* joint) const
00571   {
00572     return effort_ + joint->getFirstVariableIndex();
00573   }
00574 
00584   void setJointGroupPositions(const std::string& joint_group_name, const double* gstate)
00585   {
00586     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00587     if (jmg)
00588       setJointGroupPositions(jmg, gstate);
00589   }
00590 
00594   void setJointGroupPositions(const std::string& joint_group_name, const std::vector<double>& gstate)
00595   {
00596     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00597     if (jmg)
00598       setJointGroupPositions(jmg, &gstate[0]);
00599   }
00600 
00604   void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
00605   {
00606     setJointGroupPositions(group, &gstate[0]);
00607   }
00608 
00612   void setJointGroupPositions(const JointModelGroup* group, const double* gstate);
00613 
00617   void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
00618   {
00619     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00620     if (jmg)
00621       setJointGroupPositions(jmg, values);
00622   }
00623 
00627   void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
00628 
00632   void copyJointGroupPositions(const std::string& joint_group_name, std::vector<double>& gstate) const
00633   {
00634     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00635     if (jmg)
00636     {
00637       gstate.resize(jmg->getVariableCount());
00638       copyJointGroupPositions(jmg, &gstate[0]);
00639     }
00640   }
00641 
00645   void copyJointGroupPositions(const std::string& joint_group_name, double* gstate) const
00646   {
00647     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00648     if (jmg)
00649       copyJointGroupPositions(jmg, gstate);
00650   }
00651 
00655   void copyJointGroupPositions(const JointModelGroup* group, std::vector<double>& gstate) const
00656   {
00657     gstate.resize(group->getVariableCount());
00658     copyJointGroupPositions(group, &gstate[0]);
00659   }
00660 
00664   void copyJointGroupPositions(const JointModelGroup* group, double* gstate) const;
00665 
00669   void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
00670   {
00671     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00672     if (jmg)
00673       copyJointGroupPositions(jmg, values);
00674   }
00675 
00679   void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
00680 
00690   void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
00691   {
00692     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00693     if (jmg)
00694       setJointGroupVelocities(jmg, gstate);
00695   }
00696 
00700   void setJointGroupVelocities(const std::string& joint_group_name, const std::vector<double>& gstate)
00701   {
00702     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00703     if (jmg)
00704       setJointGroupVelocities(jmg, &gstate[0]);
00705   }
00706 
00710   void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
00711   {
00712     setJointGroupVelocities(group, &gstate[0]);
00713   }
00714 
00718   void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);
00719 
00723   void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
00724   {
00725     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00726     if (jmg)
00727       setJointGroupVelocities(jmg, values);
00728   }
00729 
00733   void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
00734 
00738   void copyJointGroupVelocities(const std::string& joint_group_name, std::vector<double>& gstate) const
00739   {
00740     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00741     if (jmg)
00742     {
00743       gstate.resize(jmg->getVariableCount());
00744       copyJointGroupVelocities(jmg, &gstate[0]);
00745     }
00746   }
00747 
00751   void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
00752   {
00753     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00754     if (jmg)
00755       copyJointGroupVelocities(jmg, gstate);
00756   }
00757 
00761   void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
00762   {
00763     gstate.resize(group->getVariableCount());
00764     copyJointGroupVelocities(group, &gstate[0]);
00765   }
00766 
00770   void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;
00771 
00775   void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
00776   {
00777     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00778     if (jmg)
00779       copyJointGroupVelocities(jmg, values);
00780   }
00781 
00785   void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
00786 
00796   void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
00797   {
00798     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00799     if (jmg)
00800       setJointGroupAccelerations(jmg, gstate);
00801   }
00802 
00806   void setJointGroupAccelerations(const std::string& joint_group_name, const std::vector<double>& gstate)
00807   {
00808     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00809     if (jmg)
00810       setJointGroupAccelerations(jmg, &gstate[0]);
00811   }
00812 
00816   void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
00817   {
00818     setJointGroupAccelerations(group, &gstate[0]);
00819   }
00820 
00824   void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);
00825 
00829   void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
00830   {
00831     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00832     if (jmg)
00833       setJointGroupAccelerations(jmg, values);
00834   }
00835 
00839   void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
00840 
00844   void copyJointGroupAccelerations(const std::string& joint_group_name, std::vector<double>& gstate) const
00845   {
00846     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00847     if (jmg)
00848     {
00849       gstate.resize(jmg->getVariableCount());
00850       copyJointGroupAccelerations(jmg, &gstate[0]);
00851     }
00852   }
00853 
00857   void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
00858   {
00859     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00860     if (jmg)
00861       copyJointGroupAccelerations(jmg, gstate);
00862   }
00863 
00867   void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
00868   {
00869     gstate.resize(group->getVariableCount());
00870     copyJointGroupAccelerations(group, &gstate[0]);
00871   }
00872 
00876   void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;
00877 
00881   void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
00882   {
00883     const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
00884     if (jmg)
00885       copyJointGroupAccelerations(jmg, values);
00886   }
00887 
00891   void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
00892 
00905   bool setToIKSolverFrame(Eigen::Affine3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
00906 
00913   bool setToIKSolverFrame(Eigen::Affine3d& pose, const std::string& ik_frame);
00914 
00922   bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, unsigned int attempts = 0,
00923                  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
00924                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
00925 
00934   bool setFromIK(const JointModelGroup* group, const geometry_msgs::Pose& pose, const std::string& tip,
00935                  unsigned int attempts = 0, double timeout = 0.0,
00936                  const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
00937                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
00938 
00946   bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, unsigned int attempts = 0,
00947                  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
00948                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
00949 
00957   bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, const std::string& tip,
00958                  unsigned int attempts = 0, double timeout = 0.0,
00959                  const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
00960                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
00961 
00971   bool setFromIK(const JointModelGroup* group, const Eigen::Affine3d& pose, const std::string& tip,
00972                  const std::vector<double>& consistency_limits, unsigned int attempts = 0, double timeout = 0.0,
00973                  const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
00974                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
00975 
00986   bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
00987                  const std::vector<std::string>& tips, unsigned int attempts = 0, double timeout = 0.0,
00988                  const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
00989                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
00990 
01002   bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
01003                  const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
01004                  unsigned int attempts = 0, double timeout = 0.0,
01005                  const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
01006                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
01007 
01017   bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Affine3d& poses,
01018                           const std::vector<std::string>& tips,
01019                           const std::vector<std::vector<double> >& consistency_limits, unsigned int attempts = 0,
01020                           double timeout = 0.0,
01021                           const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
01022                           const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
01023 
01031   bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
01032                      const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());
01033 
01041   bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt,
01042                      const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());
01043 
01078   double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
01079                               const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
01080                               double max_step, double jump_threshold,
01081                               const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
01082                               const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
01083 
01117   double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
01118                               const Eigen::Affine3d& target, bool global_reference_frame, double max_step,
01119                               double jump_threshold,
01120                               const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
01121                               const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
01122 
01156   double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
01157                               const EigenSTL::vector_Affine3d& waypoints, bool global_reference_frame, double max_step,
01158                               double jump_threshold,
01159                               const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
01160                               const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
01161 
01171   bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
01172                    Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
01173 
01183   bool getJacobian(const JointModelGroup* group, const LinkModel* link, const Eigen::Vector3d& reference_point_position,
01184                    Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
01185   {
01186     updateLinkTransforms();
01187     return static_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian,
01188                                                              use_quaternion_representation);
01189   }
01190 
01197   Eigen::MatrixXd getJacobian(const JointModelGroup* group,
01198                               const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
01199 
01206   Eigen::MatrixXd getJacobian(const JointModelGroup* group,
01207                               const Eigen::Vector3d& reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
01208   {
01209     updateLinkTransforms();
01210     return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
01211   }
01212 
01215   void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
01216                                const LinkModel* tip) const;
01217 
01220   void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
01221                                const LinkModel* tip)
01222   {
01223     updateLinkTransforms();
01224     static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
01225   }
01226 
01230   bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
01231                                  const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());
01232 
01239   void setVariableValues(const sensor_msgs::JointState& msg)
01240   {
01241     if (!msg.position.empty())
01242       setVariablePositions(msg.name, msg.position);
01243     if (!msg.velocity.empty())
01244       setVariableVelocities(msg.name, msg.velocity);
01245   }
01246 
01250   void setToDefaultValues();
01251 
01253   bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
01254 
01256   void setToRandomPositions();
01257 
01259   void setToRandomPositions(const JointModelGroup* group);
01260 
01263   void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
01264 
01270   void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near, double distance);
01271 
01279   void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& near,
01280                                   const std::vector<double>& distances);
01281 
01290   void updateCollisionBodyTransforms();
01291 
01294   void updateLinkTransforms();
01295 
01297   void update(bool force = false);
01298 
01300   void updateStateWithLinkAt(const std::string& link_name, const Eigen::Affine3d& transform, bool backward = false)
01301   {
01302     updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
01303   }
01304 
01306   void updateStateWithLinkAt(const LinkModel* link, const Eigen::Affine3d& transform, bool backward = false);
01307 
01308   const Eigen::Affine3d& getGlobalLinkTransform(const std::string& link_name)
01309   {
01310     return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
01311   }
01312 
01313   const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel* link)
01314   {
01315     updateLinkTransforms();
01316     return global_link_transforms_[link->getLinkIndex()];
01317   }
01318 
01319   const Eigen::Affine3d& getCollisionBodyTransforms(const std::string& link_name, std::size_t index)
01320   {
01321     return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
01322   }
01323 
01324   const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
01325   {
01326     updateCollisionBodyTransforms();
01327     return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
01328   }
01329 
01330   const Eigen::Affine3d& getJointTransform(const std::string& joint_name)
01331   {
01332     return getJointTransform(robot_model_->getJointModel(joint_name));
01333   }
01334 
01335   const Eigen::Affine3d& getJointTransform(const JointModel* joint)
01336   {
01337     const int idx = joint->getJointIndex();
01338     unsigned char& dirty = dirty_joint_transforms_[idx];
01339     if (dirty)
01340     {
01341       joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
01342       dirty = 0;
01343     }
01344     return variable_joint_transforms_[idx];
01345   }
01346 
01347   const Eigen::Affine3d& getGlobalLinkTransform(const std::string& link_name) const
01348   {
01349     return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
01350   }
01351 
01352   const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel* link) const
01353   {
01354     BOOST_VERIFY(checkLinkTransforms());
01355     return global_link_transforms_[link->getLinkIndex()];
01356   }
01357 
01358   const Eigen::Affine3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
01359   {
01360     return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
01361   }
01362 
01363   const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
01364   {
01365     BOOST_VERIFY(checkCollisionTransforms());
01366     return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
01367   }
01368 
01369   const Eigen::Affine3d& getJointTransform(const std::string& joint_name) const
01370   {
01371     return getJointTransform(robot_model_->getJointModel(joint_name));
01372   }
01373 
01374   const Eigen::Affine3d& getJointTransform(const JointModel* joint) const
01375   {
01376     BOOST_VERIFY(checkJointTransforms(joint));
01377     return variable_joint_transforms_[joint->getJointIndex()];
01378   }
01379 
01380   bool dirtyJointTransform(const JointModel* joint) const
01381   {
01382     return dirty_joint_transforms_[joint->getJointIndex()];
01383   }
01384 
01385   bool dirtyLinkTransforms() const
01386   {
01387     return dirty_link_transforms_;
01388   }
01389 
01390   bool dirtyCollisionBodyTransforms() const
01391   {
01392     return dirty_link_transforms_ || dirty_collision_body_transforms_;
01393   }
01394 
01396   bool dirty() const
01397   {
01398     return dirtyCollisionBodyTransforms();
01399   }
01400 
01407   double distance(const RobotState& other) const
01408   {
01409     return robot_model_->distance(position_, other.getVariablePositions());
01410   }
01411 
01412   double distance(const RobotState& other, const JointModelGroup* joint_group) const;
01413 
01414   double distance(const RobotState& other, const JointModel* joint) const
01415   {
01416     const int idx = joint->getFirstVariableIndex();
01417     return joint->distance(position_ + idx, other.position_ + idx);
01418   }
01419 
01423   void interpolate(const RobotState& to, double t, RobotState& state) const;
01424 
01428   void interpolate(const RobotState& to, double t, RobotState& state, const JointModelGroup* joint_group) const;
01429 
01433   void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
01434   {
01435     const int idx = joint->getFirstVariableIndex();
01436     joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
01437     state.markDirtyJointTransforms(joint);
01438     state.updateMimicJoint(joint);
01439   }
01440 
01441   void enforceBounds();
01442   void enforceBounds(const JointModelGroup* joint_group);
01443   void enforceBounds(const JointModel* joint)
01444   {
01445     enforcePositionBounds(joint);
01446     if (has_velocity_)
01447       enforceVelocityBounds(joint);
01448   }
01449   void enforcePositionBounds(const JointModel* joint)
01450   {
01451     if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
01452     {
01453       markDirtyJointTransforms(joint);
01454       updateMimicJoint(joint);
01455     }
01456   }
01457   void enforceVelocityBounds(const JointModel* joint)
01458   {
01459     joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
01460   }
01461 
01462   bool satisfiesBounds(double margin = 0.0) const;
01463   bool satisfiesBounds(const JointModelGroup* joint_group, double margin = 0.0) const;
01464   bool satisfiesBounds(const JointModel* joint, double margin = 0.0) const
01465   {
01466     return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
01467   }
01468   bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
01469   {
01470     return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
01471   }
01472   bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
01473   {
01474     return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
01475   }
01476 
01479   std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
01480 
01483   std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup* group) const;
01484 
01487   std::pair<double, const JointModel*>
01488   getMinDistanceToPositionBounds(const std::vector<const JointModel*>& joints) const;
01489 
01515   void attachBody(AttachedBody* attached_body);
01516 
01531   void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
01532                   const EigenSTL::vector_Affine3d& attach_trans, const std::set<std::string>& touch_links,
01533                   const std::string& link_name,
01534                   const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
01535 
01550   void attachBody(const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
01551                   const EigenSTL::vector_Affine3d& attach_trans, const std::vector<std::string>& touch_links,
01552                   const std::string& link_name,
01553                   const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
01554   {
01555     std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
01556     attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
01557   }
01558 
01560   void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
01561 
01563   void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* lm) const;
01564 
01566   void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* lm) const;
01567 
01570   bool clearAttachedBody(const std::string& id);
01571 
01573   void clearAttachedBodies(const LinkModel* link);
01574 
01576   void clearAttachedBodies(const JointModelGroup* group);
01577 
01579   void clearAttachedBodies();
01580 
01582   const AttachedBody* getAttachedBody(const std::string& name) const;
01583 
01585   bool hasAttachedBody(const std::string& id) const;
01586 
01587   void setAttachedBodyUpdateCallback(const AttachedBodyCallback& callback);
01592   void computeAABB(std::vector<double>& aabb) const;
01593 
01596   void computeAABB(std::vector<double>& aabb)
01597   {
01598     updateLinkTransforms();
01599     static_cast<const RobotState*>(this)->computeAABB(aabb);
01600   }
01601 
01603   random_numbers::RandomNumberGenerator& getRandomNumberGenerator()
01604   {
01605     if (!rng_)
01606       rng_ = new random_numbers::RandomNumberGenerator();
01607     return *rng_;
01608   }
01609 
01611   const Eigen::Affine3d& getFrameTransform(const std::string& id);
01612 
01614   const Eigen::Affine3d& getFrameTransform(const std::string& id) const;
01615 
01617   bool knowsFrameTransform(const std::string& id) const;
01618 
01626   void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
01627                        const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
01628                        bool include_attached = false) const;
01629 
01637   void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
01638                        const std_msgs::ColorRGBA& color, const std::string& ns, const ros::Duration& dur,
01639                        bool include_attached = false)
01640   {
01641     updateCollisionBodyTransforms();
01642     static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
01643   }
01644 
01649   void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
01650                        bool include_attached = false) const;
01651 
01656   void getRobotMarkers(visualization_msgs::MarkerArray& arr, const std::vector<std::string>& link_names,
01657                        bool include_attached = false)
01658   {
01659     updateCollisionBodyTransforms();
01660     static_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
01661   }
01662 
01663   void printStatePositions(std::ostream& out = std::cout) const;
01664 
01665   void printStateInfo(std::ostream& out = std::cout) const;
01666 
01667   void printTransforms(std::ostream& out = std::cout) const;
01668 
01669   void printTransform(const Eigen::Affine3d& transform, std::ostream& out = std::cout) const;
01670 
01671   void printDirtyInfo(std::ostream& out = std::cout) const;
01672 
01673   std::string getStateTreeString(const std::string& prefix = "") const;
01674 
01675 private:
01676   void allocMemory();
01677 
01678   void copyFrom(const RobotState& other);
01679 
01680   void markDirtyJointTransforms(const JointModel* joint)
01681   {
01682     dirty_joint_transforms_[joint->getJointIndex()] = 1;
01683     dirty_link_transforms_ =
01684         dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
01685   }
01686 
01687   void markDirtyJointTransforms(const JointModelGroup* group)
01688   {
01689     const std::vector<const JointModel*>& jm = group->getActiveJointModels();
01690     for (std::size_t i = 0; i < jm.size(); ++i)
01691       dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
01692     dirty_link_transforms_ = dirty_link_transforms_ == NULL ?
01693                                  group->getCommonRoot() :
01694                                  robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
01695   }
01696 
01697   void markVelocity();
01698   void markAcceleration();
01699   void markEffort();
01700 
01701   void updateMimicJoint(const JointModel* joint)
01702   {
01703     const std::vector<const JointModel*>& mim = joint->getMimicRequests();
01704     double v = position_[joint->getFirstVariableIndex()];
01705     for (std::size_t i = 0; i < mim.size(); ++i)
01706     {
01707       position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
01708       dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
01709     }
01710   }
01711 
01713   void updateMimicJoint(const std::vector<const JointModel*>& mim)
01714   {
01715     for (std::size_t i = 0; i < mim.size(); ++i)
01716     {
01717       const int fvi = mim[i]->getFirstVariableIndex();
01718       position_[fvi] =
01719           mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
01720       dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
01721     }
01722   }
01723 
01724   void updateLinkTransformsInternal(const JointModel* start);
01725 
01726   void getMissingKeys(const std::map<std::string, double>& variable_map,
01727                       std::vector<std::string>& missing_variables) const;
01728   void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
01729 
01731   bool checkJointTransforms(const JointModel* joint) const;
01732 
01734   bool checkLinkTransforms() const;
01735 
01737   bool checkCollisionTransforms() const;
01738 
01739   RobotModelConstPtr robot_model_;
01740   void* memory_;
01741 
01742   double* position_;
01743   double* velocity_;
01744   double* acceleration_;
01745   double* effort_;
01746   bool has_velocity_;
01747   bool has_acceleration_;
01748   bool has_effort_;
01749 
01750   const JointModel* dirty_link_transforms_;
01751   const JointModel* dirty_collision_body_transforms_;
01752 
01753   Eigen::Affine3d* variable_joint_transforms_;         // this points to an element in transforms_, so it is aligned
01754   Eigen::Affine3d* global_link_transforms_;            // this points to an element in transforms_, so it is aligned
01755   Eigen::Affine3d* global_collision_body_transforms_;  // this points to an element in transforms_, so it is aligned
01756   unsigned char* dirty_joint_transforms_;
01757 
01759   std::map<std::string, AttachedBody*> attached_body_map_;
01760 
01763   AttachedBodyCallback attached_body_update_callback_;
01764 
01770   random_numbers::RandomNumberGenerator* rng_;
01771 };
01772 
01774 std::ostream& operator<<(std::ostream& out, const RobotState& s);
01775 }
01776 }
01777 
01778 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44