38 #ifndef MOVEIT_CORE_ROBOT_STATE_ 39 #define MOVEIT_CORE_ROBOT_STATE_ 43 #include <sensor_msgs/JointState.h> 44 #include <visualization_msgs/MarkerArray.h> 45 #include <std_msgs/ColorRGBA.h> 46 #include <geometry_msgs/Twist.h> 49 #include <boost/assert.hpp> 61 typedef boost::function<bool(RobotState*
robot_state,
const JointModelGroup* joint_group,
62 const double* joint_group_variable_values)>
87 revolute = jt_revolute;
88 prismatic = jt_prismatic;
97 MaxEEFStep(
double translation = 0.0,
double rotation = 0.0) : translation(translation), rotation(rotation)
145 return robot_model_->getVariableCount();
151 return robot_model_->getVariableNames();
157 return robot_model_->getLinkModel(link);
163 return robot_model_->getJointModel(joint);
169 return robot_model_->getJointModelGroup(group);
195 void setVariablePositions(
const double* position);
202 assert(robot_model_->getVariableCount() <= position.size());
203 setVariablePositions(&position[0]);
208 void setVariablePositions(
const std::map<std::string, double>& variable_map);
212 void setVariablePositions(
const std::map<std::string, double>& variable_map,
213 std::vector<std::string>& missing_variables);
217 void setVariablePositions(
const std::vector<std::string>& variable_names,
218 const std::vector<double>& variable_position);
223 setVariablePosition(robot_model_->getVariableIndex(variable), value);
230 position_[index] = value;
231 const JointModel* jm = robot_model_->getJointOfVariable(index);
234 markDirtyJointTransforms(jm);
235 updateMimicJoint(jm);
242 return position_[robot_model_->getVariableIndex(variable)];
250 return position_[index];
264 return has_velocity_;
283 void zeroVelocities();
288 has_velocity_ =
true;
290 memcpy(velocity_, velocity, robot_model_->getVariableCount() *
sizeof(double));
296 assert(robot_model_->getVariableCount() <= velocity.size());
297 setVariableVelocities(&velocity[0]);
302 void setVariableVelocities(
const std::map<std::string, double>& variable_map);
306 void setVariableVelocities(
const std::map<std::string, double>& variable_map,
307 std::vector<std::string>& missing_variables);
311 void setVariableVelocities(
const std::vector<std::string>& variable_names,
312 const std::vector<double>& variable_velocity);
317 setVariableVelocity(robot_model_->getVariableIndex(variable), value);
325 velocity_[index] = value;
331 return velocity_[robot_model_->getVariableIndex(variable)];
339 return velocity_[index];
343 void dropVelocities();
357 return has_acceleration_;
366 return acceleration_;
373 return acceleration_;
377 void zeroAccelerations();
383 has_acceleration_ =
true;
387 memcpy(acceleration_, acceleration, robot_model_->getVariableCount() *
sizeof(double));
394 assert(robot_model_->getVariableCount() <= acceleration.size());
395 setVariableAccelerations(&acceleration[0]);
400 void setVariableAccelerations(
const std::map<std::string, double>& variable_map);
405 void setVariableAccelerations(
const std::map<std::string, double>& variable_map,
406 std::vector<std::string>& missing_variables);
410 void setVariableAccelerations(
const std::vector<std::string>& variable_names,
411 const std::vector<double>& variable_acceleration);
416 setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
424 acceleration_[index] = value;
430 return acceleration_[robot_model_->getVariableIndex(variable)];
438 return acceleration_[index];
442 void dropAccelerations();
481 has_acceleration_ =
false;
483 memcpy(effort_, effort, robot_model_->getVariableCount() *
sizeof(double));
489 assert(robot_model_->getVariableCount() <= effort.size());
490 setVariableEffort(&effort[0]);
494 void setVariableEffort(
const std::map<std::string, double>& variable_map);
498 void setVariableEffort(
const std::map<std::string, double>& variable_map, std::vector<std::string>& missing_variables);
501 void setVariableEffort(
const std::vector<std::string>& variable_names,
502 const std::vector<double>& variable_acceleration);
507 setVariableEffort(robot_model_->getVariableIndex(variable), value);
515 effort_[index] = value;
521 return effort_[robot_model_->getVariableIndex(variable)];
529 return effort_[index];
539 void invertVelocity();
550 setJointPositions(robot_model_->getJointModel(joint_name), position);
555 setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
560 setJointPositions(joint, &position[0]);
566 markDirtyJointTransforms(joint);
567 updateMimicJoint(joint);
572 setJointPositions(robot_model_->getJointModel(joint_name), transform);
578 markDirtyJointTransforms(joint);
579 updateMimicJoint(joint);
584 has_velocity_ =
true;
588 void setJointEfforts(
const JointModel* joint,
const double* effort);
592 return getJointPositions(robot_model_->getJointModel(joint_name));
602 return getJointVelocities(robot_model_->getJointModel(joint_name));
612 return getJointAccelerations(robot_model_->getJointModel(joint_name));
622 return getJointEffort(robot_model_->getJointModel(joint_name));
641 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
643 setJointGroupPositions(jmg, gstate);
651 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
653 setJointGroupPositions(jmg, &gstate[0]);
661 setJointGroupPositions(group, &gstate[0]);
667 void setJointGroupPositions(
const JointModelGroup* group,
const double* gstate);
674 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
676 setJointGroupPositions(jmg, values);
689 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
693 copyJointGroupPositions(jmg, &gstate[0]);
702 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
704 copyJointGroupPositions(jmg, gstate);
713 copyJointGroupPositions(group, &gstate[0]);
719 void copyJointGroupPositions(
const JointModelGroup* group,
double* gstate)
const;
726 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
728 copyJointGroupPositions(jmg, values);
734 void copyJointGroupPositions(
const JointModelGroup* group, Eigen::VectorXd& values)
const;
747 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
749 setJointGroupVelocities(jmg, gstate);
757 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
759 setJointGroupVelocities(jmg, &gstate[0]);
767 setJointGroupVelocities(group, &gstate[0]);
773 void setJointGroupVelocities(
const JointModelGroup* group,
const double* gstate);
780 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
782 setJointGroupVelocities(jmg, values);
788 void setJointGroupVelocities(
const JointModelGroup* group,
const Eigen::VectorXd& values);
795 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
799 copyJointGroupVelocities(jmg, &gstate[0]);
808 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
810 copyJointGroupVelocities(jmg, gstate);
819 copyJointGroupVelocities(group, &gstate[0]);
825 void copyJointGroupVelocities(
const JointModelGroup* group,
double* gstate)
const;
832 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
834 copyJointGroupVelocities(jmg, values);
840 void copyJointGroupVelocities(
const JointModelGroup* group, Eigen::VectorXd& values)
const;
853 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
855 setJointGroupAccelerations(jmg, gstate);
863 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
865 setJointGroupAccelerations(jmg, &gstate[0]);
873 setJointGroupAccelerations(group, &gstate[0]);
879 void setJointGroupAccelerations(
const JointModelGroup* group,
const double* gstate);
886 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
888 setJointGroupAccelerations(jmg, values);
894 void setJointGroupAccelerations(
const JointModelGroup* group,
const Eigen::VectorXd& values);
901 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
905 copyJointGroupAccelerations(jmg, &gstate[0]);
914 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
916 copyJointGroupAccelerations(jmg, gstate);
925 copyJointGroupAccelerations(group, &gstate[0]);
931 void copyJointGroupAccelerations(
const JointModelGroup* group,
double* gstate)
const;
938 const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
940 copyJointGroupAccelerations(jmg, values);
946 void copyJointGroupAccelerations(
const JointModelGroup* group, Eigen::VectorXd& values)
const;
960 bool setToIKSolverFrame(Eigen::Isometry3d& pose,
const kinematics::KinematicsBaseConstPtr& solver);
968 bool setToIKSolverFrame(Eigen::Isometry3d& pose,
const std::string& ik_frame);
976 bool setFromIK(
const JointModelGroup* group,
const geometry_msgs::Pose& pose,
double timeout = 0.0,
979 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 984 return setFromIK(group, pose, timeout, constraint,
options);
994 bool setFromIK(
const JointModelGroup* group,
const geometry_msgs::Pose& pose,
const std::string& tip,
997 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 999 unsigned int ,
double timeout = 0.0,
1003 return setFromIK(group, pose, tip, timeout, constraint,
options);
1012 bool setFromIK(
const JointModelGroup* group,
const Eigen::Isometry3d& pose,
double timeout = 0.0,
1015 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 1020 return setFromIK(group, pose, timeout, constraint,
options);
1029 bool setFromIK(
const JointModelGroup* group,
const Eigen::Isometry3d& pose,
const std::string& tip,
1032 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 1034 unsigned int ,
double timeout = 0.0,
1038 return setFromIK(group, pose, tip, timeout, constraint,
options);
1049 bool setFromIK(
const JointModelGroup* group,
const Eigen::Isometry3d& pose,
const std::string& tip,
1050 const std::vector<double>& consistency_limits,
double timeout = 0.0,
1053 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 1055 const std::vector<double>& consistency_limits,
unsigned int ,
double timeout = 0.0,
1059 return setFromIK(group, pose, tip, consistency_limits, timeout, constraint,
options);
1072 const std::vector<std::string>& tips,
double timeout = 0.0,
1075 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 1077 const std::vector<std::string>& tips,
unsigned int ,
double timeout = 0.0,
1081 return setFromIK(group, poses, tips, timeout, constraint,
options);
1095 const std::vector<std::string>& tips,
const std::vector<std::vector<double> >& consistency_limits,
1098 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 1100 const std::vector<std::string>& tips,
const std::vector<std::vector<double> >& consistency_limits,
1101 unsigned int ,
double timeout = 0.0,
1105 return setFromIK(group, poses, tips, consistency_limits, timeout, constraint,
options);
1117 const std::vector<std::string>& tips,
1118 const std::vector<std::vector<double> >& consistency_limits,
double timeout = 0.0,
1121 [[deprecated(
"The attempts argument is not supported anymore.")]]
bool 1123 const std::vector<std::string>& tips,
const std::vector<std::vector<double> >& consistency_limits,
1124 unsigned int ,
double timeout = 0.0,
1128 return setFromIKSubgroups(group, poses, tips, consistency_limits, timeout, constraint,
options);
1138 bool setFromDiffIK(
const JointModelGroup* group,
const Eigen::VectorXd& twist,
const std::string& tip,
double dt,
1148 bool setFromDiffIK(
const JointModelGroup* group,
const geometry_msgs::Twist& twist,
const std::string& tip,
double dt,
1188 const Eigen::Vector3d& direction,
bool global_reference_frame,
double distance,
1189 double max_step,
double jump_threshold_factor,
1193 return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance,
1205 const Eigen::Isometry3d& target,
bool global_reference_frame,
const MaxEEFStep& max_step,
1211 const Eigen::Isometry3d& target,
bool global_reference_frame,
double max_step,
1212 double jump_threshold_factor,
1216 return computeCartesianPath(group, traj, link, target, global_reference_frame,
MaxEEFStep(max_step),
1234 double max_step,
double jump_threshold_factor,
1238 return computeCartesianPath(group, traj, link, waypoints, global_reference_frame,
MaxEEFStep(max_step),
1257 static double testJointSpaceJump(
const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1270 static double testRelativeJointSpaceJump(
const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1271 double jump_threshold_factor);
1285 static double testAbsoluteJointSpaceJump(
const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
1286 double revolute_jump_threshold,
double prismatic_jump_threshold);
1298 Eigen::MatrixXd& jacobian,
bool use_quaternion_representation =
false)
const;
1310 Eigen::MatrixXd& jacobian,
bool use_quaternion_representation =
false)
1312 updateLinkTransforms();
1313 return static_cast<const RobotState*
>(
this)->getJacobian(group, link, reference_point_position, jacobian,
1314 use_quaternion_representation);
1335 updateLinkTransforms();
1336 return static_cast<const RobotState*
>(
this)->getJacobian(group, reference_point_position);
1341 void computeVariableVelocity(
const JointModelGroup* jmg, Eigen::VectorXd& qdot,
const Eigen::VectorXd& twist,
1349 updateLinkTransforms();
1350 static_cast<const RobotState*
>(
this)->computeVariableVelocity(jmg, qdot, twist, tip);
1356 bool integrateVariableVelocity(
const JointModelGroup* jmg,
const Eigen::VectorXd& qdot,
double dt,
1367 if (!msg.position.empty())
1368 setVariablePositions(msg.name, msg.position);
1369 if (!msg.velocity.empty())
1370 setVariableVelocities(msg.name, msg.velocity);
1376 void setToDefaultValues();
1379 bool setToDefaultValues(
const JointModelGroup* group,
const std::string& name);
1382 void setToRandomPositions();
1406 const std::vector<double>& distances);
1416 void updateCollisionBodyTransforms();
1420 void updateLinkTransforms();
1423 void update(
bool force =
false);
1435 updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
1439 void updateStateWithLinkAt(
const LinkModel* link,
const Eigen::Isometry3d& transform,
bool backward =
false);
1443 return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1448 updateLinkTransforms();
1454 return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1459 updateCollisionBodyTransforms();
1465 return getJointTransform(robot_model_->getJointModel(joint_name));
1471 unsigned char& dirty = dirty_joint_transforms_[idx];
1477 return variable_joint_transforms_[idx];
1482 return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
1487 BOOST_VERIFY(checkLinkTransforms());
1493 return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
1498 BOOST_VERIFY(checkCollisionTransforms());
1504 return getJointTransform(robot_model_->getJointModel(joint_name));
1509 BOOST_VERIFY(checkJointTransforms(joint));
1520 return dirty_link_transforms_;
1525 return dirty_link_transforms_ || dirty_collision_body_transforms_;
1531 return dirtyCollisionBodyTransforms();
1574 void enforceBounds();
1578 enforcePositionBounds(joint);
1580 enforceVelocityBounds(joint);
1586 markDirtyJointTransforms(joint);
1587 updateMimicJoint(joint);
1592 void harmonizePositions();
1598 updateMimicJoint(joint);
1606 bool satisfiesBounds(
double margin = 0.0)
const;
1607 bool satisfiesBounds(
const JointModelGroup* joint_group,
double margin = 0.0)
const;
1610 return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
1623 std::pair<double, const JointModel*> getMinDistanceToPositionBounds()
const;
1627 std::pair<double, const JointModel*> getMinDistanceToPositionBounds(
const JointModelGroup* group)
const;
1631 std::pair<double, const JointModel*>
1632 getMinDistanceToPositionBounds(
const std::vector<const JointModel*>& joints)
const;
1683 void attachBody(
const std::string&
id,
const std::vector<shapes::ShapeConstPtr>&
shapes,
1685 const std::string& link_name,
1686 const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory());
1702 void attachBody(
const std::string&
id,
const std::vector<shapes::ShapeConstPtr>& shapes,
1704 const std::string& link_name,
1705 const trajectory_msgs::JointTrajectory& detach_posture = trajectory_msgs::JointTrajectory())
1707 std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
1708 attachBody(
id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
1712 void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies)
const;
1715 void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies,
const JointModelGroup* lm)
const;
1718 void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies,
const LinkModel* lm)
const;
1722 bool clearAttachedBody(
const std::string&
id);
1725 void clearAttachedBodies(
const LinkModel* link);
1731 void clearAttachedBodies();
1734 const AttachedBody* getAttachedBody(
const std::string& name)
const;
1737 bool hasAttachedBody(
const std::string&
id)
const;
1744 void computeAABB(std::vector<double>& aabb)
const;
1750 updateLinkTransforms();
1751 static_cast<const RobotState*
>(
this)->computeAABB(aabb);
1763 const Eigen::Isometry3d& getFrameTransform(
const std::string& frame_id);
1766 const Eigen::Isometry3d& getFrameTransform(
const std::string& frame_id)
const;
1769 bool knowsFrameTransform(
const std::string& frame_id)
const;
1778 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
const std::vector<std::string>& link_names,
1779 const std_msgs::ColorRGBA& color,
const std::string& ns,
const ros::Duration& dur,
1780 bool include_attached =
false)
const;
1789 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
const std::vector<std::string>& link_names,
1790 const std_msgs::ColorRGBA& color,
const std::string& ns,
const ros::Duration& dur,
1791 bool include_attached =
false)
1793 updateCollisionBodyTransforms();
1794 static_cast<const RobotState*
>(
this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
1801 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
const std::vector<std::string>& link_names,
1802 bool include_attached =
false)
const;
1808 void getRobotMarkers(visualization_msgs::MarkerArray& arr,
const std::vector<std::string>& link_names,
1809 bool include_attached =
false)
1811 updateCollisionBodyTransforms();
1812 static_cast<const RobotState*
>(
this)->getRobotMarkers(arr, link_names, include_attached);
1815 void printStatePositions(std::ostream& out = std::cout)
const;
1820 void printStateInfo(std::ostream& out = std::cout)
const;
1822 void printTransforms(std::ostream& out = std::cout)
const;
1824 void printTransform(
const Eigen::Isometry3d& transform, std::ostream& out = std::cout)
const;
1826 void printDirtyInfo(std::ostream& out = std::cout)
const;
1828 std::string getStateTreeString(
const std::string& prefix =
"")
const;
1832 void initTransforms();
1838 dirty_link_transforms_ =
1839 dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
1845 for (std::size_t i = 0; i < jm.size(); ++i)
1846 dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
1847 dirty_link_transforms_ = dirty_link_transforms_ == NULL ?
1849 robot_model_->getCommonRoot(dirty_link_transforms_, group->
getCommonRoot());
1852 void markVelocity();
1853 void markAcceleration();
1860 for (std::size_t i = 0; i < mim.size(); ++i)
1862 position_[mim[i]->getFirstVariableIndex()] = mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
1863 markDirtyJointTransforms(mim[i]);
1871 for (std::size_t i = 0; i < mim.size(); ++i)
1873 const int fvi = mim[i]->getFirstVariableIndex();
1875 mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset();
1879 dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
1888 const int fvi = jm->getFirstVariableIndex();
1889 position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
1890 markDirtyJointTransforms(jm);
1892 markDirtyJointTransforms(group);
1897 void getMissingKeys(
const std::map<std::string, double>& variable_map,
1898 std::vector<std::string>& missing_variables)
const;
1899 void getStateTreeJointString(std::ostream& ss,
const JointModel* jm,
const std::string& pfx0,
bool last)
const;
1902 bool checkJointTransforms(
const JointModel* joint)
const;
1905 bool checkLinkTransforms()
const;
1908 bool checkCollisionTransforms()
const;
void setJointPositions(const std::string &joint_name, const double *position)
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known...
random_numbers::RandomNumberGenerator & getRandomNumberGenerator()
Return the instance of a random number generator.
JumpThreshold(double jt_factor)
A set of options for the kinematics solver.
Eigen::Isometry3d * variable_joint_transforms_
void computeAABB(std::vector< double > &aabb)
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
void updateMimicJoint(const JointModel *joint)
const double * getJointEffort(const JointModel *joint) const
Core components of MoveIt!
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...
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 ...
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...
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...
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...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known...
const double * getVariableVelocities() const
Get const access to the velocities of the variables that make up this state. The values are in the sa...
void harmonizePosition(const JointModel *joint)
const double * getJointVelocities(const std::string &joint_name) const
void attachBody(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &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.
Vec3fX< details::Vec3Data< double > > Vector3d
const std::vector< const JointModel * > & getMimicRequests() const
The joint models whose values would be modified if the value of this joint changed.
void updateMimicJoints(const JointModelGroup *group)
Update all mimic joints within group.
double * getVariableEffort()
Get raw access to the effort of the variables that make up this state. The values are in the same ord...
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
const double * getJointVelocities(const JointModel *joint) const
std::vector< double > values
bool dirtyJointTransform(const JointModel *joint) const
void enforcePositionBounds(const JointModel *joint)
const double * getJointAccelerations(const JointModel *joint) const
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...
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const EigenSTL::vector_Isometry3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
int getJointIndex() const
Get the index of this joint within the robot model.
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
int getLinkIndex() const
The index of this joint when traversing the kinematic tree in depth first fashion.
void setVariableVelocities(const double *velocity)
Given an array with velocity values for all variables, set those values as the velocities in this sta...
const JointModel * dirty_collision_body_transforms_
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, const std::string &tip, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name)
void enforceVelocityBounds(const JointModel *joint)
void setVariablePositions(const std::vector< double > &position)
It is assumed positions is an array containing the new positions for all variables in this state...
virtual bool harmonizePosition(double *values, const Bounds &other_bounds) const
virtual void computeTransform(const double *joint_values, Eigen::Isometry3d &transf) const =0
Given the joint values for a joint, compute the corresponding transform.
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index)
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
void setJointPositions(const JointModel *joint, const double *position)
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 ...
Eigen::Isometry3d * global_link_transforms_
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.
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...
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name) const
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...
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).
void setVariableAcceleration(int index, double value)
Set the acceleration of a single variable. The variable is specified by its index (a value associated...
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
geometry_msgs::TransformStamped t
void markDirtyJointTransforms(const JointModel *joint)
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...
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...
bool dirtyLinkTransforms() const
void setJointPositions(const std::string &joint_name, const std::vector< double > &position)
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...
const JointModel * getCommonRoot() const
Get the common root of all joint roots; not necessarily part of this group.
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
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...
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link) const
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known...
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...
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...
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...
const double * getJointEffort(const std::string &joint_name) const
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
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...
const double * getJointAccelerations(const std::string &joint_name) const
bool satisfiesPositionBounds(const JointModel *joint, double margin=0.0) const
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...
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...
void enforceBounds(const JointModel *joint)
const Eigen::Isometry3d & getJointTransform(const JointModel *joint)
const Eigen::Isometry3d & getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
Struct for containing jump_threshold.
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) ...
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...
MOVEIT_CLASS_FORWARD(RobotModel)
MaxEEFStep(double translation=0.0, double rotation=0.0)
void setJointPositions(const JointModel *joint, const std::vector< double > &position)
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...
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
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. ...
void setVariableAccelerations(const std::vector< double > &acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
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...
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
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...
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.
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
unsigned char * dirty_joint_transforms_
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const =0
Given the transform generated by joint, compute the corresponding joint values.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
bool dirty() const
Returns true if anything in this state is dirty.
const Eigen::Isometry3d & getJointTransform(const std::string &joint_name) const
AttachedBodyCallback attached_body_update_callback_
This event is called when there is a change in the attached bodies for this state; The event specifie...
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...
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Struct for containing max_step for computeCartesianPath.
double computeCartesianPath(const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Isometry3d &target, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
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.
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...
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...
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
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...
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...
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
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 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...
bool setFromIKSubgroups(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
bool enforceVelocityBounds(double *values) const
Force the specified velocities to be within bounds. Return true if changes were made.
void markDirtyJointTransforms(const JointModelGroup *group)
int getFirstCollisionBodyTransformIndex() const
double * getVariableVelocities()
Get raw access to the velocities of the variables that make up this state. The values are in the same...
bool dirtyCollisionBodyTransforms() const
void setJointPositions(const std::string &joint_name, const Eigen::Isometry3d &transform)
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 ...
bool setFromIK(const JointModelGroup *group, const Eigen::Isometry3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
void setVariableVelocity(int index, double value)
Set the velocity of a single variable. The variable is specified by its index (a value associated by ...
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
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 ...
JumpThreshold(double jt_revolute, double jt_prismatic)
double getVariableEffort(const std::string &variable) const
Get the effort of a particular variable. An exception is thrown if the variable is not known...
const JointModel * dirty_link_transforms_
std::size_t getVariableCount() const
Get the number of variables that make up this state.
double * getVariableAccelerations()
Get raw access to the accelerations of the variables that make up this state. The values are in the s...
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 ...
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
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...
random_numbers::RandomNumberGenerator * rng_
For certain operations a state needs a random number generator. However, it may be slightly expensive...
const double * getVariablePositions() const
Get a raw pointer to the positions of the variables stored in this state.
RobotModelConstPtr robot_model_
void setVariableValues(const sensor_msgs::JointState &msg)
void setVariableAccelerations(const double *acceleration)
Given an array with acceleration values for all variables, set those values as the accelerations in t...
Eigen::Isometry3d * global_collision_body_transforms_
double getVariableEffort(int index) const
Get the effort of a particular variable. The variable is specified by its index. No checks are perfor...
Representation of a robot's state. This includes position, velocity, acceleration and effort...
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...
bool satisfiesVelocityBounds(const JointModel *joint, double margin=0.0) const
A link from the robot. Contains the constant transform applied to the link and its geometry...
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
void setVariableEffort(const double *effort)
Given an array with effort values for all variables, set those values as the effort in this state...
bool hasEffort() const
By default, if effort is never set or initialized, the state remembers that there is no effort set...
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
bool satisfiesBounds(const JointModel *joint, double margin=0.0) const
int getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
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...
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...
Main namespace for MoveIt!
double distance(const RobotState &other, const JointModel *joint) const
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.
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.
const JointModel * getJointModel(const std::string &joint) const
Get the model of a particular joint.
const Eigen::Isometry3d & getJointTransform(const JointModel *joint) const
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
bool setFromIK(const JointModelGroup *group, const EigenSTL::vector_Isometry3d &poses, const std::vector< std::string > &tips, unsigned int, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
std::ostream & operator<<(std::ostream &out, const VariableBounds &b)
Operator overload for printing variable bounds to a stream.
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...
void setVariablePosition(int index, double value)
Set the position of a single variable. The variable is specified by its index (a value associated by ...
double distance(const RobotState &other) const
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...
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...
double distance(const urdf::Pose &transform)
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_...
void setJointPositions(const JointModel *joint, const Eigen::Isometry3d &transform)
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...
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())
const double * getJointPositions(const std::string &joint_name) const
std::map< std::string, AttachedBody * > attached_body_map_
All attached bodies that are part of this state, indexed by their name.
const double * getJointPositions(const JointModel *joint) const
const Eigen::Isometry3d & getGlobalLinkTransform(const LinkModel *link)
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()
void setJointVelocities(const JointModel *joint, const double *velocity)
void updateMimicJoint(const std::vector< const JointModel *> &mim)
Update a set of joints that are certain to be mimicking other joints.