00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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 <geometry_msgs/Twist.h>
00046 #include <cassert>
00047
00048 #include <boost/assert.hpp>
00049
00050 namespace moveit
00051 {
00052 namespace core
00053 {
00054
00055 MOVEIT_CLASS_FORWARD(RobotState);
00056
00059 typedef boost::function<bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn;
00060
00078 class RobotState
00079 {
00080 public:
00081
00084 RobotState(const RobotModelConstPtr &robot_model);
00085 ~RobotState();
00086
00088 RobotState(const RobotState &other);
00089
00091 RobotState& operator=(const RobotState &other);
00092
00094 const RobotModelConstPtr& getRobotModel() const
00095 {
00096 return robot_model_;
00097 }
00098
00100 std::size_t getVariableCount() const
00101 {
00102 return robot_model_->getVariableCount();
00103 }
00104
00106 const std::vector<std::string>& getVariableNames() const
00107 {
00108 return robot_model_->getVariableNames();
00109 }
00110
00112 const LinkModel* getLinkModel(const std::string &link) const
00113 {
00114 return robot_model_->getLinkModel(link);
00115 }
00116
00118 const JointModel* getJointModel(const std::string &joint) const
00119 {
00120 return robot_model_->getJointModel(joint);
00121 }
00122
00124 const JointModelGroup* getJointModelGroup(const std::string &group) const
00125 {
00126 return robot_model_->getJointModelGroup(group);
00127 }
00128
00137 double* getVariablePositions()
00138 {
00139 return position_;
00140 }
00141
00144 const double* getVariablePositions() const
00145 {
00146 return position_;
00147 }
00148
00152 void setVariablePositions(const double *position);
00153
00157 void setVariablePositions(const std::vector<double> &position)
00158 {
00159 assert(robot_model_->getVariableCount() <= position.size());
00160 setVariablePositions(&position[0]);
00161 }
00162
00164 void setVariablePositions(const std::map<std::string, double> &variable_map);
00165
00168 void setVariablePositions(const std::map<std::string, double> &variable_map, std::vector<std::string> &missing_variables);
00169
00172 void setVariablePositions(const std::vector<std::string>& variable_names, const std::vector<double>& variable_position);
00173
00175 void setVariablePosition(const std::string &variable, double value)
00176 {
00177 setVariablePosition(robot_model_->getVariableIndex(variable), value);
00178 }
00179
00181 void setVariablePosition(int index, double value)
00182 {
00183 position_[index] = value;
00184 const JointModel *jm = robot_model_->getJointOfVariable(index);
00185 if (jm)
00186 {
00187 markDirtyJointTransforms(jm);
00188 updateMimicJoint(jm);
00189 }
00190 }
00191
00193 const double getVariablePosition(const std::string &variable) const
00194 {
00195 return position_[robot_model_->getVariableIndex(variable)];
00196 }
00197
00201 const double getVariablePosition(int index) const
00202 {
00203 return position_[index];
00204 }
00205
00215 bool hasVelocities() const
00216 {
00217 return has_velocity_;
00218 }
00219
00221 double* getVariableVelocities()
00222 {
00223 markVelocity();
00224 return velocity_;
00225 }
00226
00228 const double* getVariableVelocities() const
00229 {
00230 return velocity_;
00231 }
00232
00234 void setVariableVelocities(const double *velocity)
00235 {
00236 has_velocity_ = true;
00237
00238 memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
00239 }
00240
00242 void setVariableVelocities(const std::vector<double> &velocity)
00243 {
00244 assert(robot_model_->getVariableCount() <= velocity.size());
00245 setVariableVelocities(&velocity[0]);
00246 }
00247
00249 void setVariableVelocities(const std::map<std::string, double> &variable_map);
00250
00253 void setVariableVelocities(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables);
00254
00256 void setVariableVelocities(const std::vector<std::string>& variable_names, const std::vector<double>& variable_velocity);
00257
00259 void setVariableVelocity(const std::string &variable, double value)
00260 {
00261 setVariableVelocity(robot_model_->getVariableIndex(variable), value);
00262 }
00263
00265 void setVariableVelocity(int index, double value)
00266 {
00267 markVelocity();
00268 velocity_[index] = value;
00269 }
00270
00272 const double getVariableVelocity(const std::string &variable) const
00273 {
00274 return velocity_[robot_model_->getVariableIndex(variable)];
00275 }
00276
00280 const double getVariableVelocity(int index) const
00281 {
00282 return velocity_[index];
00283 }
00284
00294 bool hasAccelerations() const
00295 {
00296 return has_acceleration_;
00297 }
00298
00300 double* getVariableAccelerations()
00301 {
00302 markAcceleration();
00303 return acceleration_;
00304 }
00305
00307 const double* getVariableAccelerations() const
00308 {
00309 return acceleration_;
00310 }
00311
00313 void setVariableAccelerations(const double *acceleration)
00314 {
00315 has_acceleration_ = true;
00316 has_effort_ = false;
00317
00318
00319 memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
00320 }
00321
00323 void setVariableAccelerations(const std::vector<double> &acceleration)
00324 {
00325 assert(robot_model_->getVariableCount() <= acceleration.size());
00326 setVariableAccelerations(&acceleration[0]);
00327 }
00328
00330 void setVariableAccelerations(const std::map<std::string, double> &variable_map);
00331
00334 void setVariableAccelerations(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables);
00335
00337 void setVariableAccelerations(const std::vector<std::string>& variable_names, const std::vector<double>& variable_acceleration);
00338
00340 void setVariableAcceleration(const std::string &variable, double value)
00341 {
00342 setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
00343 }
00344
00346 void setVariableAcceleration(int index, double value)
00347 {
00348 markAcceleration();
00349 acceleration_[index] = value;
00350 }
00351
00353 const double getVariableAcceleration(const std::string &variable) const
00354 {
00355 return acceleration_[robot_model_->getVariableIndex(variable)];
00356 }
00357
00361 const double getVariableAcceleration(int index) const
00362 {
00363 return acceleration_[index];
00364 }
00365
00375 bool hasEffort() const
00376 {
00377 return has_effort_;
00378 }
00379
00381 double* getVariableEffort()
00382 {
00383 markEffort();
00384 return effort_;
00385 }
00386
00388 const double* getVariableEffort() const
00389 {
00390 return effort_;
00391 }
00392
00394 void setVariableEffort(const double *effort)
00395 {
00396 has_effort_ = true;
00397 has_acceleration_ = false;
00398
00399 memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
00400 }
00401
00403 void setVariableEffort(const std::vector<double> &effort)
00404 {
00405 assert(robot_model_->getVariableCount() <= effort.size());
00406 setVariableEffort(&effort[0]);
00407 }
00408
00410 void setVariableEffort(const std::map<std::string, double> &variable_map);
00411
00414 void setVariableEffort(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables);
00415
00417 void setVariableEffort(const std::vector<std::string>& variable_names, const std::vector<double>& variable_acceleration);
00418
00420 void setVariableEffort(const std::string &variable, double value)
00421 {
00422 setVariableEffort(robot_model_->getVariableIndex(variable), value);
00423 }
00424
00426 void setVariableEffort(int index, double value)
00427 {
00428 markEffort();
00429 effort_[index] = value;
00430 }
00431
00433 const double getVariableEffort(const std::string &variable) const
00434 {
00435 return effort_[robot_model_->getVariableIndex(variable)];
00436 }
00437
00441 const double getVariableEffort(int index) const
00442 {
00443 return effort_[index];
00444 }
00445
00451 void setJointPositions(const std::string &joint_name, const double *position)
00452 {
00453 setJointPositions(robot_model_->getJointModel(joint_name), position);
00454 }
00455
00456 void setJointPositions(const std::string &joint_name, const std::vector<double> &position)
00457 {
00458 setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
00459 }
00460
00461 void setJointPositions(const JointModel *joint, const std::vector<double> &position)
00462 {
00463 setJointPositions(joint, &position[0]);
00464 }
00465
00466 void setJointPositions(const JointModel *joint, const double *position)
00467 {
00468 memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
00469 markDirtyJointTransforms(joint);
00470 updateMimicJoint(joint);
00471 }
00472
00473 void setJointPositions(const std::string &joint_name, const Eigen::Affine3d& transform)
00474 {
00475 setJointPositions(robot_model_->getJointModel(joint_name), transform);
00476 }
00477
00478 void setJointPositions(const JointModel *joint, const Eigen::Affine3d& transform)
00479 {
00480 joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
00481 markDirtyJointTransforms(joint);
00482 updateMimicJoint(joint);
00483 }
00484
00485 const double* getJointPositions(const std::string &joint_name) const
00486 {
00487 return getJointPositions(robot_model_->getJointModel(joint_name));
00488 }
00489
00490 const double* getJointPositions(const JointModel *joint) const
00491 {
00492 return position_ + joint->getFirstVariableIndex();
00493 }
00494
00495 const double* getJointVelocities(const std::string &joint_name) const
00496 {
00497 return getJointVelocities(robot_model_->getJointModel(joint_name));
00498 }
00499
00500 const double* getJointVelocities(const JointModel *joint) const
00501 {
00502 return velocity_ + joint->getFirstVariableIndex();
00503 }
00504
00505 const double* getJointAccelerations(const std::string &joint_name) const
00506 {
00507 return getJointAccelerations(robot_model_->getJointModel(joint_name));
00508 }
00509
00510 const double* getJointAccelerations(const JointModel *joint) const
00511 {
00512 return acceleration_ + joint->getFirstVariableIndex();
00513 }
00514
00515 const double* getJointEffort(const std::string &joint_name) const
00516 {
00517 return getJointEffort(robot_model_->getJointModel(joint_name));
00518 }
00519
00520 const double* getJointEffort(const JointModel *joint) const
00521 {
00522 return effort_ + joint->getFirstVariableIndex();
00523 }
00524
00534 void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
00535 {
00536 const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
00537 if (jmg)
00538 setJointGroupPositions(jmg, gstate);
00539 }
00540
00543 void setJointGroupPositions(const std::string &joint_group_name, const std::vector<double> &gstate)
00544 {
00545 const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
00546 if (jmg)
00547 setJointGroupPositions(jmg, &gstate[0]);
00548 }
00549
00552 void setJointGroupPositions(const JointModelGroup *group, const std::vector<double> &gstate)
00553 {
00554 setJointGroupPositions(group, &gstate[0]);
00555 }
00556
00559 void setJointGroupPositions(const JointModelGroup *group, const double *gstate);
00560
00563 void setJointGroupPositions(const std::string &joint_group_name, const Eigen::VectorXd& values)
00564 {
00565 const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
00566 if (jmg)
00567 setJointGroupPositions(jmg, values);
00568 }
00569
00572 void setJointGroupPositions(const JointModelGroup *group, const Eigen::VectorXd& values);
00573
00575 void copyJointGroupPositions(const std::string &joint_group_name, std::vector<double> &gstate) const
00576 {
00577 const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
00578 if (jmg)
00579 {
00580 gstate.resize(jmg->getVariableCount());
00581 copyJointGroupPositions(jmg, &gstate[0]);
00582 }
00583 }
00584
00586 void copyJointGroupPositions(const std::string &joint_group_name, double *gstate) const
00587 {
00588 const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
00589 if (jmg)
00590 copyJointGroupPositions(jmg, gstate);
00591 }
00592
00594 void copyJointGroupPositions(const JointModelGroup *group, std::vector<double> &gstate) const
00595 {
00596 gstate.resize(group->getVariableCount());
00597 copyJointGroupPositions(group, &gstate[0]);
00598 }
00599
00601 void copyJointGroupPositions(const JointModelGroup *group, double *gstate) const;
00602
00604 void copyJointGroupPositions(const std::string &joint_group_name, Eigen::VectorXd& values) const
00605 {
00606 const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
00607 if (jmg)
00608 copyJointGroupPositions(jmg, values);
00609 }
00610
00612 void copyJointGroupPositions(const JointModelGroup *group, Eigen::VectorXd& values) const;
00613
00620 bool setToIKSolverFrame(Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr& solver);
00621
00628 bool setToIKSolverFrame(Eigen::Affine3d &pose, const std::string &ik_frame);
00629
00636 bool setFromIK(const JointModelGroup *group,
00637 const geometry_msgs::Pose &pose,
00638 unsigned int attempts = 0, double timeout = 0.0,
00639 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00640 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00641
00649 bool setFromIK(const JointModelGroup *group,
00650 const geometry_msgs::Pose &pose, const std::string &tip,
00651 unsigned int attempts = 0, double timeout = 0.0,
00652 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00653 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00654
00661 bool setFromIK(const JointModelGroup *group,
00662 const Eigen::Affine3d &pose,
00663 unsigned int attempts = 0, double timeout = 0.0,
00664 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00665 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00666
00673 bool setFromIK(const JointModelGroup *group,
00674 const Eigen::Affine3d &pose, const std::string &tip,
00675 unsigned int attempts = 0, double timeout = 0.0,
00676 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00677 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00678
00687 bool setFromIK(const JointModelGroup *group,
00688 const Eigen::Affine3d &pose, const std::string &tip,
00689 const std::vector<double> &consistency_limits,
00690 unsigned int attempts = 0, double timeout = 0.0,
00691 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00692 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00693
00704 bool setFromIK(const JointModelGroup *group,
00705 const EigenSTL::vector_Affine3d &poses,
00706 const std::vector<std::string> &tips,
00707 unsigned int attempts = 0, double timeout = 0.0,
00708 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00709 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00710
00722 bool setFromIK(const JointModelGroup *group,
00723 const EigenSTL::vector_Affine3d &poses,
00724 const std::vector<std::string> &tips,
00725 const std::vector<std::vector<double> > &consistency_limits,
00726 unsigned int attempts = 0, double timeout = 0.0,
00727 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00728 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00729
00739 bool setFromIKSubgroups(const JointModelGroup *group,
00740 const EigenSTL::vector_Affine3d &poses,
00741 const std::vector<std::string> &tips,
00742 const std::vector<std::vector<double> > &consistency_limits,
00743 unsigned int attempts = 0, double timeout = 0.0,
00744 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
00745 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00746
00754 bool setFromDiffIK(const JointModelGroup *group,
00755 const Eigen::VectorXd &twist, const std::string &tip, double dt,
00756 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn());
00757
00765 bool setFromDiffIK(const JointModelGroup *group,
00766 const geometry_msgs::Twist &twist, const std::string &tip, double dt,
00767 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn());
00768
00769
00789 double computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
00790 const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold,
00791 const GroupStateValidityCallbackFn &validCallback = GroupStateValidityCallbackFn(),
00792 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00793
00813 double computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
00814 const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold,
00815 const GroupStateValidityCallbackFn &validCallback = GroupStateValidityCallbackFn(),
00816 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00817
00836 double computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
00837 const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold,
00838 const GroupStateValidityCallbackFn &validCallback = GroupStateValidityCallbackFn(),
00839 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
00840
00849 bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position,
00850 Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;
00851
00860 bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position,
00861 Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
00862 {
00863 updateLinkTransforms();
00864 return const_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian, use_quaternion_representation);
00865 }
00866
00872 Eigen::MatrixXd getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;
00873
00879 Eigen::MatrixXd getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
00880 {
00881 updateLinkTransforms();
00882 return const_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
00883 }
00884
00886 void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot,
00887 const Eigen::VectorXd &twist, const LinkModel *tip) const;
00888
00890 void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot,
00891 const Eigen::VectorXd &twist, const LinkModel *tip)
00892 {
00893 updateLinkTransforms();
00894 const_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
00895 }
00896
00899 bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt,
00900 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn());
00901
00908 void setVariableValues(const sensor_msgs::JointState& msg)
00909 {
00910 if (!msg.position.empty())
00911 setVariablePositions(msg.name, msg.position);
00912 if (!msg.velocity.empty())
00913 setVariableVelocities(msg.name, msg.velocity);
00914 }
00915
00919 void setToDefaultValues();
00920
00922 bool setToDefaultValues(const JointModelGroup *group, const std::string &name);
00923
00925 void setToRandomPositions();
00926
00928 void setToRandomPositions(const JointModelGroup *group);
00929
00932 void setToRandomPositions(const JointModelGroup *group, random_numbers::RandomNumberGenerator &rng);
00933
00939 void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance);
00940
00948 void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, const std::vector<double> &distances);
00949
00958 void updateCollisionBodyTransforms();
00959
00961 void updateLinkTransforms();
00962
00964 void update(bool force = false);
00965
00967 void updateStateWithLinkAt(const std::string& link_name, const Eigen::Affine3d& transform, bool backward = false)
00968 {
00969 updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
00970 }
00971
00973 void updateStateWithLinkAt(const LinkModel *link, const Eigen::Affine3d& transform, bool backward = false);
00974
00975 const Eigen::Affine3d& getGlobalLinkTransform(const std::string &link_name)
00976 {
00977 return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
00978 }
00979
00980 const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel *link)
00981 {
00982 updateLinkTransforms();
00983 return global_link_transforms_[link->getLinkIndex()];
00984 }
00985
00986 const Eigen::Affine3d& getCollisionBodyTransforms(const std::string &link_name, std::size_t index)
00987 {
00988 return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
00989 }
00990
00991 const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel *link, std::size_t index)
00992 {
00993 updateCollisionBodyTransforms();
00994 return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
00995 }
00996
00997 const Eigen::Affine3d& getJointTransform(const std::string &joint_name)
00998 {
00999 return getJointTransform(robot_model_->getJointModel(joint_name));
01000 }
01001
01002 const Eigen::Affine3d& getJointTransform(const JointModel *joint)
01003 {
01004 const int idx = joint->getJointIndex();
01005 unsigned char &dirty = dirty_joint_transforms_[idx];
01006 if (dirty)
01007 {
01008 joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
01009 dirty = 0;
01010 }
01011 return variable_joint_transforms_[idx];
01012 }
01013
01014 const Eigen::Affine3d& getGlobalLinkTransform(const std::string &link_name) const
01015 {
01016 return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
01017 }
01018
01019 const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel *link) const
01020 {
01021 BOOST_VERIFY(checkLinkTransforms());
01022 return global_link_transforms_[link->getLinkIndex()];
01023 }
01024
01025 const Eigen::Affine3d& getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
01026 {
01027 return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
01028 }
01029
01030 const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
01031 {
01032 BOOST_VERIFY(checkCollisionTransforms());
01033 return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
01034 }
01035
01036 const Eigen::Affine3d& getJointTransform(const std::string &joint_name) const
01037 {
01038 return getJointTransform(robot_model_->getJointModel(joint_name));
01039 }
01040
01041 const Eigen::Affine3d& getJointTransform(const JointModel *joint) const
01042 {
01043 BOOST_VERIFY(checkJointTransforms(joint));
01044 return variable_joint_transforms_[joint->getJointIndex()];
01045 }
01046
01047 bool dirtyJointTransform(const JointModel *joint) const
01048 {
01049 return dirty_joint_transforms_[joint->getJointIndex()];
01050 }
01051
01052 bool dirtyLinkTransforms() const
01053 {
01054 return dirty_link_transforms_;
01055 }
01056
01057 bool dirtyCollisionBodyTransforms() const
01058 {
01059 return dirty_link_transforms_ || dirty_collision_body_transforms_;
01060 }
01061
01063 bool dirty() const
01064 {
01065 return dirtyCollisionBodyTransforms();
01066 }
01067
01074 double distance(const RobotState &other) const
01075 {
01076 return robot_model_->distance(position_, other.getVariablePositions());
01077 }
01078
01079 double distance(const RobotState &other, const JointModelGroup *joint_group) const;
01080
01081 double distance(const RobotState &other, const JointModel *joint) const
01082 {
01083 const int idx = joint->getFirstVariableIndex();
01084 return joint->distance(position_ + idx, other.position_ + idx);
01085 }
01086
01090 void interpolate(const RobotState &to, double t, RobotState &state) const;
01091
01095 void interpolate(const RobotState &to, double t, RobotState &state, const JointModelGroup *joint_group) const;
01096
01100 void interpolate(const RobotState &to, double t, RobotState &state, const JointModel *joint) const
01101 {
01102 const int idx = joint->getFirstVariableIndex();
01103 joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
01104 state.markDirtyJointTransforms(joint);
01105 state.updateMimicJoint(joint);
01106 }
01107
01108 void enforceBounds();
01109 void enforceBounds(const JointModelGroup *joint_group);
01110 void enforceBounds(const JointModel *joint)
01111 {
01112 enforcePositionBounds(joint);
01113 if (has_velocity_)
01114 enforceVelocityBounds(joint);
01115 }
01116 void enforcePositionBounds(const JointModel *joint)
01117 {
01118 if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
01119 {
01120 markDirtyJointTransforms(joint);
01121 updateMimicJoint(joint);
01122 }
01123 }
01124 void enforceVelocityBounds(const JointModel *joint)
01125 {
01126 joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
01127 }
01128
01129 bool satisfiesBounds(double margin = 0.0) const;
01130 bool satisfiesBounds(const JointModelGroup *joint_group, double margin = 0.0) const;
01131 bool satisfiesBounds(const JointModel *joint, double margin = 0.0) const
01132 {
01133 return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
01134 }
01135 bool satisfiesPositionBounds(const JointModel *joint, double margin = 0.0) const
01136 {
01137 return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
01138 }
01139 bool satisfiesVelocityBounds(const JointModel *joint, double margin = 0.0) const
01140 {
01141 return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
01142 }
01143
01146 std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
01147
01150 std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup *group) const;
01151
01154 std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const std::vector<const JointModel*> &joints) const;
01155
01182 void attachBody(AttachedBody *attached_body);
01183
01198 void attachBody(const std::string &id,
01199 const std::vector<shapes::ShapeConstPtr> &shapes,
01200 const EigenSTL::vector_Affine3d &attach_trans,
01201 const std::set<std::string> &touch_links,
01202 const std::string &link_name,
01203 const trajectory_msgs::JointTrajectory &detach_posture = trajectory_msgs::JointTrajectory());
01204
01219 void attachBody(const std::string &id,
01220 const std::vector<shapes::ShapeConstPtr> &shapes,
01221 const EigenSTL::vector_Affine3d &attach_trans,
01222 const std::vector<std::string> &touch_links,
01223 const std::string &link_name,
01224 const trajectory_msgs::JointTrajectory &detach_posture = trajectory_msgs::JointTrajectory())
01225 {
01226 std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
01227 attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
01228 }
01229
01231 void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies) const;
01232
01234 void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies, const JointModelGroup *lm) const;
01235
01237 void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies, const LinkModel *lm) const;
01238
01240 bool clearAttachedBody(const std::string &id);
01241
01243 void clearAttachedBodies(const LinkModel *link);
01244
01246 void clearAttachedBodies(const JointModelGroup *group);
01247
01249 void clearAttachedBodies();
01250
01252 const AttachedBody* getAttachedBody(const std::string &name) const;
01253
01255 bool hasAttachedBody(const std::string &id) const;
01256
01257 void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback);
01262 void computeAABB(std::vector<double> &aabb) const;
01263
01266 void computeAABB(std::vector<double> &aabb)
01267 {
01268 updateLinkTransforms();
01269 const_cast<const RobotState*>(this)->computeAABB(aabb);
01270 }
01271
01273 random_numbers::RandomNumberGenerator& getRandomNumberGenerator()
01274 {
01275 if (!rng_)
01276 rng_ = new random_numbers::RandomNumberGenerator();
01277 return *rng_;
01278 }
01279
01281 const Eigen::Affine3d& getFrameTransform(const std::string &id);
01282
01284 const Eigen::Affine3d& getFrameTransform(const std::string &id) const;
01285
01287 bool knowsFrameTransform(const std::string &id) const;
01288
01296 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
01297 const std::vector<std::string> &link_names,
01298 const std_msgs::ColorRGBA& color,
01299 const std::string& ns,
01300 const ros::Duration& dur,
01301 bool include_attached = false) const;
01302
01310 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
01311 const std::vector<std::string> &link_names,
01312 const std_msgs::ColorRGBA& color,
01313 const std::string& ns,
01314 const ros::Duration& dur,
01315 bool include_attached = false)
01316 {
01317 updateCollisionBodyTransforms();
01318 const_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
01319 }
01320
01325 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
01326 const std::vector<std::string> &link_names,
01327 bool include_attached = false) const;
01328
01333 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
01334 const std::vector<std::string> &link_names,
01335 bool include_attached = false)
01336 {
01337 updateCollisionBodyTransforms();
01338 const_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
01339 }
01340
01341 void printStatePositions(std::ostream &out = std::cout) const;
01342
01343 void printStateInfo(std::ostream &out = std::cout) const;
01344
01345 void printTransforms(std::ostream &out = std::cout) const;
01346
01347 void printTransform(const Eigen::Affine3d &transform, std::ostream &out = std::cout) const;
01348
01349 void printDirtyInfo(std::ostream &out = std::cout) const;
01350
01351 std::string getStateTreeString(const std::string& prefix = "") const;
01352
01353 private:
01354
01355 void allocMemory();
01356
01357 void copyFrom(const RobotState &other);
01358
01359 void markDirtyJointTransforms(const JointModel *joint)
01360 {
01361 dirty_joint_transforms_[joint->getJointIndex()] = 1;
01362 dirty_link_transforms_ = dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
01363 }
01364
01365 void markDirtyJointTransforms(const JointModelGroup *group)
01366 {
01367 const std::vector<const JointModel*> &jm = group->getActiveJointModels();
01368 for (std::size_t i = 0 ; i < jm.size() ; ++i)
01369 dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
01370 dirty_link_transforms_ = dirty_link_transforms_ == NULL ? group->getCommonRoot() : robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
01371 }
01372
01373 void markVelocity();
01374 void markAcceleration();
01375 void markEffort();
01376
01377 void updateMimicJoint(const JointModel *joint)
01378 {
01379 const std::vector<const JointModel*> &mim = joint->getMimicRequests();
01380 double v = position_[joint->getFirstVariableIndex()];
01381 for (std::size_t i = 0 ; i < mim.size() ; ++i)
01382 {
01383 position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
01384 dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
01385 }
01386 }
01387
01389 void updateMimicJoint(const std::vector<const JointModel*> &mim)
01390 {
01391 for (std::size_t i = 0 ; i < mim.size() ; ++i)
01392 {
01393 const int fvi = mim[i]->getFirstVariableIndex();
01394 position_[fvi] = mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
01395 dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
01396 }
01397 }
01398
01399 void updateLinkTransformsInternal(const JointModel *start);
01400
01401 void getMissingKeys(const std::map<std::string, double> &variable_map, std::vector<std::string> &missing_variables) const;
01402 void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;
01403
01405 bool checkJointTransforms(const JointModel *joint) const;
01406
01408 bool checkLinkTransforms() const;
01409
01411 bool checkCollisionTransforms() const;
01412
01413 RobotModelConstPtr robot_model_;
01414 void *memory_;
01415
01416 double *position_;
01417 double *velocity_;
01418 double *acceleration_;
01419 double *effort_;
01420 bool has_velocity_;
01421 bool has_acceleration_;
01422 bool has_effort_;
01423
01424 const JointModel *dirty_link_transforms_;
01425 const JointModel *dirty_collision_body_transforms_;
01426
01427 Eigen::Affine3d *variable_joint_transforms_;
01428 Eigen::Affine3d *global_link_transforms_;
01429 Eigen::Affine3d *global_collision_body_transforms_;
01430 unsigned char *dirty_joint_transforms_;
01431
01433 std::map<std::string, AttachedBody*> attached_body_map_;
01434
01437 AttachedBodyCallback attached_body_update_callback_;
01438
01443 random_numbers::RandomNumberGenerator *rng_;
01444 };
01445
01447 std::ostream& operator<<(std::ostream &out, const RobotState &s);
01448
01449 }
01450 }
01451
01452 #endif