Representation of a robot's state. This includes position, velocity, acceleration and effort. More...
#include <robot_state.h>
Public Member Functions | |
| void | computeAABB (std::vector< double > &aabb) const |
| Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz) More... | |
| void | computeAABB (std::vector< double > &aabb) |
| Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz) More... | |
| const Eigen::Affine3d & | getFrameTransform (const std::string &id) |
| Get the transformation matrix from the model frame to the frame identified by id. More... | |
| const Eigen::Affine3d & | getFrameTransform (const std::string &id) const |
| Get the transformation matrix from the model frame to the frame identified by id. More... | |
| const JointModel * | getJointModel (const std::string &joint) const |
| Get the model of a particular joint. More... | |
| const JointModelGroup * | getJointModelGroup (const std::string &group) const |
| Get the model of a particular joint group. More... | |
| const LinkModel * | getLinkModel (const std::string &link) const |
| Get the model of a particular link. More... | |
| random_numbers::RandomNumberGenerator & | getRandomNumberGenerator () |
| Return the instance of a random number generator. More... | |
| void | getRobotMarkers (visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, const std_msgs::ColorRGBA &color, const std::string &ns, const ros::Duration &dur, bool include_attached=false) const |
| Get a MarkerArray that fully describes the robot markers for a given robot. More... | |
| 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. More... | |
| void | getRobotMarkers (visualization_msgs::MarkerArray &arr, const std::vector< std::string > &link_names, bool include_attached=false) const |
| Get a MarkerArray that fully describes the robot markers for a given robot. More... | |
| 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. More... | |
| const RobotModelConstPtr & | getRobotModel () const |
| Get the robot model this state is constructed for. More... | |
| std::string | getStateTreeString (const std::string &prefix="") const |
| std::size_t | getVariableCount () const |
| Get the number of variables that make up this state. More... | |
| 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. More... | |
| bool | knowsFrameTransform (const std::string &id) const |
| Check if a transformation matrix from the model frame to frame id is known. More... | |
| RobotState & | operator= (const RobotState &other) |
| Copy operator. More... | |
| void | printDirtyInfo (std::ostream &out=std::cout) const |
| void | printStateInfo (std::ostream &out=std::cout) const |
| void | printStatePositions (std::ostream &out=std::cout) const |
| void | printTransform (const Eigen::Affine3d &transform, std::ostream &out=std::cout) const |
| void | printTransforms (std::ostream &out=std::cout) const |
| RobotState (const RobotModelConstPtr &robot_model) | |
| A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information. More... | |
| RobotState (const RobotState &other) | |
| Copy constructor. More... | |
| ~RobotState () | |
Getting and setting variable position | |
| 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). More... | |
| const double * | getVariablePositions () const |
| Get a raw pointer to the positions of the variables stored in this state. More... | |
| void | setVariablePositions (const double *position) |
| It is assumed positions is an array containing the new positions for all variables in this state. Those values are copied into the state. More... | |
| void | setVariablePositions (const std::vector< double > &position) |
| It is assumed positions is an array containing the new positions for all variables in this state. Those values are copied into the state. More... | |
| void | setVariablePositions (const std::map< std::string, double > &variable_map) |
| Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| void | setVariablePositions (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) |
| Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set. More... | |
| void | setVariablePositions (const std::vector< std::string > &variable_names, const std::vector< double > &variable_position) |
| Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set. More... | |
| 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. More... | |
| void | setVariablePosition (int index, double value) |
| Set the position of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable) More... | |
| double | getVariablePosition (const std::string &variable) const |
| Get the position of a particular variable. An exception is thrown if the variable is not known. More... | |
| 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. More... | |
Getting and setting variable velocity | |
| bool | hasVelocities () const |
| By default, if velocities are never set or initialized, the state remembers that there are no velocities set. This is useful to know when serializing or copying the state. More... | |
| double * | getVariableVelocities () |
| Get raw access to the velocities of the variables that make up this state. The values are in the same order as reported by getVariableNames() More... | |
| const double * | getVariableVelocities () const |
| Get const access to the velocities of the variables that make up this state. The values are in the same order as reported by getVariableNames() More... | |
| void | setVariableVelocities (const double *velocity) |
| Given an array with velocity values for all variables, set those values as the velocities in this state. More... | |
| void | setVariableVelocities (const std::vector< double > &velocity) |
| Given an array with velocity values for all variables, set those values as the velocities in this state. More... | |
| void | setVariableVelocities (const std::map< std::string, double > &variable_map) |
| Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| void | setVariableVelocities (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) |
| Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set. More... | |
| void | setVariableVelocities (const std::vector< std::string > &variable_names, const std::vector< double > &variable_velocity) |
| Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| 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. More... | |
| void | setVariableVelocity (int index, double value) |
| Set the velocity of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable) More... | |
| double | getVariableVelocity (const std::string &variable) const |
| Get the velocity of a particular variable. An exception is thrown if the variable is not known. More... | |
| 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. More... | |
Getting and setting variable acceleration | |
| bool | hasAccelerations () const |
| By default, if accelerations are never set or initialized, the state remembers that there are no accelerations set. This is useful to know when serializing or copying the state. If hasAccelerations() reports true, hasEffort() will certainly report false. More... | |
| double * | getVariableAccelerations () |
| Get raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames(). The area of memory overlaps with effort (effort and acceleration should not be set at the same time) More... | |
| 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() More... | |
| void | setVariableAccelerations (const double *acceleration) |
| Given an array with acceleration values for all variables, set those values as the accelerations in this state. More... | |
| void | setVariableAccelerations (const std::vector< double > &acceleration) |
| Given an array with acceleration values for all variables, set those values as the accelerations in this state. More... | |
| void | setVariableAccelerations (const std::map< std::string, double > &variable_map) |
| Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| void | setVariableAccelerations (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) |
| Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set. More... | |
| void | setVariableAccelerations (const std::vector< std::string > &variable_names, const std::vector< double > &variable_acceleration) |
| Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| 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. More... | |
| void | setVariableAcceleration (int index, double value) |
| Set the acceleration of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable) More... | |
| double | getVariableAcceleration (const std::string &variable) const |
| Get the acceleration of a particular variable. An exception is thrown if the variable is not known. More... | |
| 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. More... | |
Getting and setting variable effort | |
| bool | hasEffort () const |
| By default, if effort is never set or initialized, the state remembers that there is no effort set. This is useful to know when serializing or copying the state. If hasEffort() reports true, hasAccelerations() will certainly report false. More... | |
| double * | getVariableEffort () |
| Get raw access to the effort of the variables that make up this state. The values are in the same order as reported by getVariableNames(). The area of memory overlaps with accelerations (effort and acceleration should not be set at the same time) More... | |
| const double * | getVariableEffort () const |
| Get const raw access to the effort of the variables that make up this state. The values are in the same order as reported by getVariableNames(). More... | |
| void | setVariableEffort (const double *effort) |
| Given an array with effort values for all variables, set those values as the effort in this state. More... | |
| 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. More... | |
| void | setVariableEffort (const std::map< std::string, double > &variable_map) |
| Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| void | setVariableEffort (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) |
| Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set. More... | |
| void | setVariableEffort (const std::vector< std::string > &variable_names, const std::vector< double > &variable_acceleration) |
| Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. More... | |
| 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. More... | |
| void | setVariableEffort (int index, double value) |
| Set the effort of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable) More... | |
| double | getVariableEffort (const std::string &variable) const |
| Get the effort of a particular variable. An exception is thrown if the variable is not known. More... | |
| double | getVariableEffort (int index) const |
| Get the effort of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed. More... | |
| void | invertVelocity () |
| Invert velocity if present. More... | |
Getting and setting joint positions, velocities, accelerations and effort | |
| void | setJointPositions (const std::string &joint_name, const double *position) |
| void | setJointPositions (const std::string &joint_name, const std::vector< double > &position) |
| void | setJointPositions (const JointModel *joint, const std::vector< double > &position) |
| void | setJointPositions (const JointModel *joint, const double *position) |
| void | setJointPositions (const std::string &joint_name, const Eigen::Affine3d &transform) |
| void | setJointPositions (const JointModel *joint, const Eigen::Affine3d &transform) |
| void | setJointVelocities (const JointModel *joint, const double *velocity) |
| void | setJointEfforts (const JointModel *joint, const double *effort) |
| const double * | getJointPositions (const std::string &joint_name) const |
| const double * | getJointPositions (const JointModel *joint) const |
| const double * | getJointVelocities (const std::string &joint_name) const |
| const double * | getJointVelocities (const JointModel *joint) const |
| const double * | getJointAccelerations (const std::string &joint_name) const |
| const double * | getJointAccelerations (const JointModel *joint) const |
| const double * | getJointEffort (const std::string &joint_name) const |
| const double * | getJointEffort (const JointModel *joint) const |
Getting and setting group positions | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| void | setJointGroupPositions (const JointModelGroup *group, const double *gstate) |
| Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| void | setJointGroupPositions (const JointModelGroup *group, const Eigen::VectorXd &values) |
| Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| void | copyJointGroupPositions (const JointModelGroup *group, double *gstate) const |
| For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| void | copyJointGroupPositions (const JointModelGroup *group, Eigen::VectorXd &values) const |
| For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
Getting and setting group velocities | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| void | setJointGroupVelocities (const JointModelGroup *group, const double *gstate) |
| Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 values of mimic joints), set those as the new values that correspond to the group. More... | |
| void | setJointGroupVelocities (const JointModelGroup *group, const Eigen::VectorXd &values) |
| Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| void | copyJointGroupVelocities (const JointModelGroup *group, double *gstate) const |
| For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| void | copyJointGroupVelocities (const JointModelGroup *group, Eigen::VectorXd &values) const |
| For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
Getting and setting group accelerations | |
| 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 (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| void | setJointGroupAccelerations (const JointModelGroup *group, const double *gstate) |
| Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| void | setJointGroupAccelerations (const JointModelGroup *group, const Eigen::VectorXd &values) |
| Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| void | copyJointGroupAccelerations (const JointModelGroup *group, double *gstate) const |
| For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| 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 location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
| void | copyJointGroupAccelerations (const JointModelGroup *group, Eigen::VectorXd &values) const |
| For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer. More... | |
Getting and setting whole states | |
| void | setVariableValues (const sensor_msgs::JointState &msg) |
| void | setToDefaultValues () |
| Set all joints to their default positions. The default position is 0, or if that is not within bounds then half way between min and max bound. More... | |
| bool | setToDefaultValues (const JointModelGroup *group, const std::string &name) |
| Set the joints in group to the position name defined in the SRDF. More... | |
| void | setToRandomPositions () |
| Set all joints to random values. Values will be within default bounds. More... | |
| void | setToRandomPositions (const JointModelGroup *group) |
| Set all joints in group to random values. Values will be within default bounds. More... | |
| void | setToRandomPositions (const JointModelGroup *group, random_numbers::RandomNumberGenerator &rng) |
| Set all joints in group to random values using a specified random number generator. Values will be within default bounds. More... | |
| void | setToRandomPositionsNearBy (const JointModelGroup *group, const RobotState &near, double distance) |
| Set all joints in group to random values near the value in . distance is the maximum amount each joint value will vary from the corresponding value in near. represents meters for prismatic/postitional joints and radians for revolute/orientation joints. Resulting values are clamped within default bounds. More... | |
| void | setToRandomPositionsNearBy (const JointModelGroup *group, const RobotState &near, const std::vector< double > &distances) |
Set all joints in group to random values near the value in . distances MUST have the same size as group.getActiveJointModels(). Each value in distances is the maximum amount the corresponding active joint in group will vary from the corresponding value in near. represents meters for prismatic/postitional joints and radians for revolute/orientation joints. Resulting values are clamped within default bounds. More... | |
Updating and getting transforms | |
| void | updateCollisionBodyTransforms () |
| Update the transforms for the collision bodies. This call is needed before calling collision checking. If updating link transforms or joint transorms is needed, the corresponding updates are also triggered. More... | |
| void | updateLinkTransforms () |
| Update the reference frame transforms for links. This call is needed before using the transforms of links for coordinate transforms. More... | |
| void | update (bool force=false) |
| Update all transforms. More... | |
| void | updateStateWithLinkAt (const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false) |
| Update the state after setting a particular link to the input global transform pose. More... | |
| void | updateStateWithLinkAt (const LinkModel *link, const Eigen::Affine3d &transform, bool backward=false) |
| Update the state after setting a particular link to the input global transform pose. More... | |
| const Eigen::Affine3d & | getGlobalLinkTransform (const std::string &link_name) |
| const Eigen::Affine3d & | getGlobalLinkTransform (const LinkModel *link) |
| const Eigen::Affine3d & | getCollisionBodyTransforms (const std::string &link_name, std::size_t index) |
| const Eigen::Affine3d & | getCollisionBodyTransform (const LinkModel *link, std::size_t index) |
| const Eigen::Affine3d & | getJointTransform (const std::string &joint_name) |
| const Eigen::Affine3d & | getJointTransform (const JointModel *joint) |
| const Eigen::Affine3d & | getGlobalLinkTransform (const std::string &link_name) const |
| const Eigen::Affine3d & | getGlobalLinkTransform (const LinkModel *link) const |
| const Eigen::Affine3d & | getCollisionBodyTransform (const std::string &link_name, std::size_t index) const |
| const Eigen::Affine3d & | getCollisionBodyTransform (const LinkModel *link, std::size_t index) const |
| const Eigen::Affine3d & | getJointTransform (const std::string &joint_name) const |
| const Eigen::Affine3d & | getJointTransform (const JointModel *joint) const |
| bool | dirtyJointTransform (const JointModel *joint) const |
| bool | dirtyLinkTransforms () const |
| bool | dirtyCollisionBodyTransforms () const |
| bool | dirty () const |
| Returns true if anything in this state is dirty. More... | |
Computing distances | |
| double | distance (const RobotState &other) const |
| double | distance (const RobotState &other, const JointModelGroup *joint_group) const |
| double | distance (const RobotState &other, const JointModel *joint) const |
| void | interpolate (const RobotState &to, double t, RobotState &state) const |
| Interpolate from this state towards state to, at time t in [0,1]. The result is stored in state, mimic joints are correctly updated and flags are set so that FK is recomputed when needed. More... | |
| void | interpolate (const RobotState &to, double t, RobotState &state, const JointModelGroup *joint_group) const |
| Interpolate from this state towards to, at time t in [0,1], but only for the joints in the specified group. If mimic joints need to be updated, they are updated accordingly. Flags are set so that FK computation is triggered when needed. More... | |
| 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 joint. If there are joints that mimic this joint, they are updated. Flags are set so that FK computation is triggered as needed. More... | |
| void | enforceBounds () |
| void | enforceBounds (const JointModelGroup *joint_group) |
| void | enforceBounds (const JointModel *joint) |
| void | enforcePositionBounds (const JointModel *joint) |
| void | enforceVelocityBounds (const JointModel *joint) |
| bool | satisfiesBounds (double margin=0.0) const |
| bool | satisfiesBounds (const JointModelGroup *joint_group, double margin=0.0) const |
| bool | satisfiesBounds (const JointModel *joint, double margin=0.0) const |
| bool | satisfiesPositionBounds (const JointModel *joint, double margin=0.0) const |
| bool | satisfiesVelocityBounds (const JointModel *joint, double margin=0.0) const |
| std::pair< double, const JointModel * > | getMinDistanceToPositionBounds () const |
| Get the minimm distance from this state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned. More... | |
| std::pair< double, const JointModel * > | getMinDistanceToPositionBounds (const JointModelGroup *group) const |
| Get the minimm distance from a group in this state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned. More... | |
| std::pair< double, const JointModel * > | getMinDistanceToPositionBounds (const std::vector< const JointModel * > &joints) const |
| Get the minimm distance from a set of joints in the state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned. More... | |
| bool | isValidVelocityMove (const RobotState &other, const JointModelGroup *group, double dt) const |
| Check that the time to move between two waypoints is sufficient given velocity limits and time step. More... | |
Managing attached bodies | |
| void | attachBody (AttachedBody *attached_body) |
| Add an attached body to this state. Ownership of the memory for the attached body is assumed by the state. More... | |
| void | attachBody (const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &attach_trans, const std::set< 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. More... | |
| void | attachBody (const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &attach_trans, const std::vector< std::string > &touch_links, const std::string &link_name, const trajectory_msgs::JointTrajectory &detach_posture=trajectory_msgs::JointTrajectory()) |
| Add an attached body to a link. More... | |
| void | getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies) const |
| Get all bodies attached to the model corresponding to this state. More... | |
| void | getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies, const JointModelGroup *lm) const |
| Get all bodies attached to a particular group the model corresponding to this state. More... | |
| void | getAttachedBodies (std::vector< const AttachedBody * > &attached_bodies, const LinkModel *lm) const |
| Get all bodies attached to a particular link in the model corresponding to this state. More... | |
| bool | clearAttachedBody (const std::string &id) |
| Remove the attached body named id. Return false if the object was not found (and thus not removed). Return true on success. More... | |
| void | clearAttachedBodies (const LinkModel *link) |
| Clear the bodies attached to a specific link. More... | |
| void | clearAttachedBodies (const JointModelGroup *group) |
| Clear the bodies attached to a specific group. More... | |
| void | clearAttachedBodies () |
| Clear all attached bodies. This calls delete on the AttachedBody instances, if needed. More... | |
| const AttachedBody * | getAttachedBody (const std::string &name) const |
| Get the attached body named name. Return NULL if not found. More... | |
| bool | hasAttachedBody (const std::string &id) const |
| Check if an attached body named id exists in this state. More... | |
| void | setAttachedBodyUpdateCallback (const AttachedBodyCallback &callback) |
Private Member Functions | |
| void | allocMemory () |
| bool | checkCollisionTransforms () const |
| This function is only called in debug mode. More... | |
| bool | checkJointTransforms (const JointModel *joint) const |
| This function is only called in debug mode. More... | |
| bool | checkLinkTransforms () const |
| This function is only called in debug mode. More... | |
| void | copyFrom (const RobotState &other) |
| void | getMissingKeys (const std::map< std::string, double > &variable_map, std::vector< std::string > &missing_variables) const |
| void | getStateTreeJointString (std::ostream &ss, const JointModel *jm, const std::string &pfx0, bool last) const |
| void | markAcceleration () |
| void | markDirtyJointTransforms (const JointModel *joint) |
| void | markDirtyJointTransforms (const JointModelGroup *group) |
| void | markEffort () |
| void | markVelocity () |
| void | updateLinkTransformsInternal (const JointModel *start) |
| void | updateMimicJoint (const JointModel *joint) |
| MOVEIT_DEPRECATED void | updateMimicJoint (const std::vector< const JointModel * > &mim) |
| Update a set of joints that are certain to be mimicking other joints. More... | |
| void | updateMimicJoints (const JointModelGroup *group) |
| Update all mimic joints within group. More... | |
Private Attributes | |
| double * | acceleration_ |
| std::map< std::string, AttachedBody * > | attached_body_map_ |
| All attached bodies that are part of this state, indexed by their name. More... | |
| AttachedBodyCallback | attached_body_update_callback_ |
| This event is called when there is a change in the attached bodies for this state; The event specifies the body that changed and whether it was just attached or about to be detached. More... | |
| const JointModel * | dirty_collision_body_transforms_ |
| unsigned char * | dirty_joint_transforms_ |
| const JointModel * | dirty_link_transforms_ |
| double * | effort_ |
| Eigen::Affine3d * | global_collision_body_transforms_ |
| Eigen::Affine3d * | global_link_transforms_ |
| bool | has_acceleration_ |
| bool | has_effort_ |
| bool | has_velocity_ |
| void * | memory_ |
| double * | position_ |
| random_numbers::RandomNumberGenerator * | rng_ |
| For certain operations a state needs a random number generator. However, it may be slightly expensive to allocate the random number generator if many state instances are generated. For this reason, the generator is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but call getRandomNumberGenerator() instead. More... | |
| RobotModelConstPtr | robot_model_ |
| Eigen::Affine3d * | variable_joint_transforms_ |
| double * | velocity_ |
Setting from Inverse Kinematics | |
| bool | setToIKSolverFrame (Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr &solver) |
| Convert the frame of reference of the pose to that same frame as the IK solver expects. More... | |
| bool | setToIKSolverFrame (Eigen::Affine3d &pose, const std::string &ik_frame) |
| Convert the frame of reference of the pose to that same frame as the IK solver expects. More... | |
| bool | setFromIK (const JointModelGroup *group, const geometry_msgs::Pose &pose, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. More... | |
| bool | setFromIK (const JointModelGroup *group, const geometry_msgs::Pose &pose, const std::string &tip, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. More... | |
| bool | setFromIK (const JointModelGroup *group, const Eigen::Affine3d &pose, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. More... | |
| bool | setFromIK (const JointModelGroup *group, const Eigen::Affine3d &pose, const std::string &tip, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. More... | |
| bool | setFromIK (const JointModelGroup *group, const Eigen::Affine3d &pose, const std::string &tip, const std::vector< double > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success. More... | |
| bool | setFromIK (const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success. More... | |
| bool | setFromIK (const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success. More... | |
| bool | setFromIKSubgroups (const JointModelGroup *group, const EigenSTL::vector_Affine3d &poses, const std::vector< std::string > &tips, const std::vector< std::vector< double > > &consistency_limits, unsigned int attempts=0, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for non-chain kinematics. In this case, we divide the group into subgroups and do IK solving individually More... | |
| bool | setFromDiffIK (const JointModelGroup *group, const Eigen::VectorXd &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn()) |
| Set the joint values from a Cartesian velocity applied during a time dt. More... | |
| bool | setFromDiffIK (const JointModelGroup *group, const geometry_msgs::Twist &twist, const std::string &tip, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn()) |
| Set the joint values from a Cartesian velocity applied during a time dt. More... | |
| double | computeCartesianPath (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Vector3d &direction, bool global_reference_frame, double distance, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group. More... | |
| 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()) |
| double | computeCartesianPath (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Affine3d &target, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group. More... | |
| double | computeCartesianPath (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| double | computeCartesianPath (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, const MaxEEFStep &max_step, const JumpThreshold &jump_threshold, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| Compute the sequence of joint values that perform a general Cartesian path. More... | |
| double | computeCartesianPath (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const LinkModel *link, const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold_factor, const GroupStateValidityCallbackFn &validCallback=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) |
| bool | getJacobian (const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd &jacobian, bool use_quaternion_representation=false) const |
| Compute the Jacobian with reference to a particular point on a given link, for a specified group. More... | |
| 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. More... | |
| Eigen::MatrixXd | getJacobian (const JointModelGroup *group, const Eigen::Vector3d &reference_point_position=Eigen::Vector3d(0.0, 0.0, 0.0)) const |
| Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an exception is thrown. More... | |
| 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 chain, an exception is thrown. More... | |
| void | computeVariableVelocity (const JointModelGroup *jmg, Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const LinkModel *tip) const |
| Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot. More... | |
| 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 store it in qdot. More... | |
| bool | integrateVariableVelocity (const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn()) |
| Given the velocities for the variables in this group (qdot) and an amount of time (dt), update the current state using the Euler forward method. If the constraint specified is satisfied, return true, otherwise return false. More... | |
| static double | testJointSpaceJump (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, const JumpThreshold &jump_threshold) |
| Tests joint space jumps of a trajectory. More... | |
| static double | testRelativeJointSpaceJump (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double jump_threshold_factor) |
| Tests for relative joint space jumps of the trajectory traj. More... | |
| static double | testAbsoluteJointSpaceJump (const JointModelGroup *group, std::vector< RobotStatePtr > &traj, double revolute_jump_threshold, double prismatic_jump_threshold) |
| Tests for absolute joint space jumps of the trajectory traj. More... | |
Representation of a robot's state. This includes position, velocity, acceleration and effort.
At the lowest level, a state is a collection of variables. Each variable has a name and can have position, velocity, acceleration and effort associated to it. Effort and acceleration share the memory area for efficiency reasons (one should not set both acceleration and effort in the same state and expect things to work). Often variables correspond to joint names as well (joints with one degree of freedom have one variable), but joints with multiple degrees of freedom have more variables. Operations are allowed at variable level, joint level (see JointModel) and joint group level (see JointModelGroup).
For efficiency reasons a state computes forward kinematics in a lazy fashion. This can sometimes lead to problems if the update() function was not called on the state.
Definition at line 123 of file robot_state.h.
| moveit::core::RobotState::RobotState | ( | const RobotModelConstPtr & | robot_model | ) |
A state can be constructed from a specified robot model. No values are initialized. Call setToDefaultValues() if a state needs to provide valid information.
Definition at line 59 of file robot_state.cpp.
| moveit::core::RobotState::~RobotState | ( | ) |
Definition at line 83 of file robot_state.cpp.
| moveit::core::RobotState::RobotState | ( | const RobotState & | other | ) |
Copy constructor.
Definition at line 76 of file robot_state.cpp.
|
private |
Definition at line 91 of file robot_state.cpp.
| void moveit::core::RobotState::attachBody | ( | AttachedBody * | attached_body | ) |
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the state.
This only adds the given body to this RobotState instance. It does not change anything about other representations of the object elsewhere in the system. So if the body represents an object in a collision_detection::World (like from a planning_scene::PlanningScene), you will likely need to remove the corresponding object from that world to avoid having collisions detected against it.
Definition at line 858 of file robot_state.cpp.
| void moveit::core::RobotState::attachBody | ( | const std::string & | id, |
| const std::vector< shapes::ShapeConstPtr > & | shapes, | ||
| const EigenSTL::vector_Affine3d & | attach_trans, | ||
| const std::set< 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.
| id | The string id associated with the attached body |
| shapes | The shapes that make up the attached body |
| attach_trans | The desired transform between this link and the attached body |
| touch_links | The set of links that the attached body is allowed to touch |
| link_name | The link to attach to |
This only adds the given body to this RobotState instance. It does not change anything about other representations of the object elsewhere in the system. So if the body represents an object in a collision_detection::World (like from a planning_scene::PlanningScene), you will likely need to remove the corresponding object from that world to avoid having collisions detected against it.
Definition at line 866 of file robot_state.cpp.
|
inline |
Add an attached body to a link.
| id | The string id associated with the attached body |
| shapes | The shapes that make up the attached body |
| attach_trans | The desired transform between this link and the attached body |
| touch_links | The set of links that the attached body is allowed to touch |
| link_name | The link to attach to |
This only adds the given body to this RobotState instance. It does not change anything about other representations of the object elsewhere in the system. So if the body represents an object in a collision_detection::World (like from a planning_scene::PlanningScene), you will likely need to remove the corresponding object from that world to avoid having collisions detected against it.
Definition at line 1618 of file robot_state.h.
|
private |
This function is only called in debug mode.
Definition at line 185 of file robot_state.cpp.
|
private |
This function is only called in debug mode.
Definition at line 165 of file robot_state.cpp.
|
private |
This function is only called in debug mode.
Definition at line 175 of file robot_state.cpp.
| void moveit::core::RobotState::clearAttachedBodies | ( | const LinkModel * | link | ) |
Clear the bodies attached to a specific link.
Definition at line 918 of file robot_state.cpp.
| void moveit::core::RobotState::clearAttachedBodies | ( | const JointModelGroup * | group | ) |
Clear the bodies attached to a specific group.
Definition at line 936 of file robot_state.cpp.
| void moveit::core::RobotState::clearAttachedBodies | ( | ) |
Clear all attached bodies. This calls delete on the AttachedBody instances, if needed.
Definition at line 906 of file robot_state.cpp.
| bool moveit::core::RobotState::clearAttachedBody | ( | const std::string & | id | ) |
Remove the attached body named id. Return false if the object was not found (and thus not removed). Return true on success.
Definition at line 954 of file robot_state.cpp.
| void moveit::core::RobotState::computeAABB | ( | std::vector< double > & | aabb | ) | const |
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz)
Definition at line 2117 of file robot_state.cpp.
|
inline |
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx, maxx, miny, maxy, minz, maxz)
Definition at line 1664 of file robot_state.h.
| double moveit::core::RobotState::computeCartesianPath | ( | const JointModelGroup * | group, |
| std::vector< RobotStatePtr > & | traj, | ||
| const LinkModel * | link, | ||
| const Eigen::Vector3d & | direction, | ||
| bool | global_reference_frame, | ||
| double | distance, | ||
| const MaxEEFStep & | max_step, | ||
| const JumpThreshold & | jump_threshold, | ||
| const GroupStateValidityCallbackFn & | validCallback = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group.
The Cartesian path to be followed is specified as a direction of motion (direction, unit vector) for the origin The Cartesian path to be followed is specified as a direction of motion (direction, unit vector) for the origin of a robot link (link). The direction is assumed to be either in a global reference frame or in the local reference frame of the link. In the latter case (global_reference_frame is false) the direction is rotated accordingly. The link needs to move in a straight line, following the specified direction, for the desired distance. The resulting joint values are stored in the vector traj, one by one. The maximum distance in Cartesian space between consecutive points on the resulting path is specified in the MaxEEFStep struct which provides two fields: translation and rotation. If a validCallback is specified, this is passed to the internal call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to the distance that was computed and for which corresponding states were added to the path. At the end of the function call, the state of the group corresponds to the last attempted Cartesian pose.
During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. To account for this, the jump_threshold struct is provided, which comprises three fields: jump_threshold_factor, revolute_jump_threshold and prismatic_jump_threshold. If either revolute_jump_threshold or prismatic_jump_threshold are non-zero, we test for absolute jumps. If jump_threshold_factor is non-zero, we test for relative jumps. Otherwise (all params are zero), jump detection is disabled.
For relative jump detection, the average joint-space distance between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger then this average distance by a factor of jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just before the jump.
For absolute jump thresholds, if any individual joint-space motion delta is larger then revolute_jump_threshold for revolute joints or prismatic_jump_threshold for prismatic joints then this step is considered a failure and the returned path is truncated up to just before the jump.
Definition at line 1878 of file robot_state.cpp.
|
inline |
Definition at line 1114 of file robot_state.h.
| double moveit::core::RobotState::computeCartesianPath | ( | const JointModelGroup * | group, |
| std::vector< RobotStatePtr > & | traj, | ||
| const LinkModel * | link, | ||
| const Eigen::Affine3d & | target, | ||
| bool | global_reference_frame, | ||
| const MaxEEFStep & | max_step, | ||
| const JumpThreshold & | jump_threshold, | ||
| const GroupStateValidityCallbackFn & | validCallback = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group.
In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (target) for the origin of a robot link (link). The target frame is assumed to be either in a global reference frame or in the local reference frame of the link. In the latter case (global_reference_frame is false) the target is rotated accordingly. All other comments from the previous function apply.
Definition at line 1900 of file robot_state.cpp.
|
inline |
Definition at line 1137 of file robot_state.h.
| double moveit::core::RobotState::computeCartesianPath | ( | const JointModelGroup * | group, |
| std::vector< RobotStatePtr > & | traj, | ||
| const LinkModel * | link, | ||
| const EigenSTL::vector_Affine3d & | waypoints, | ||
| bool | global_reference_frame, | ||
| const MaxEEFStep & | max_step, | ||
| const JumpThreshold & | jump_threshold, | ||
| const GroupStateValidityCallbackFn & | validCallback = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
Compute the sequence of joint values that perform a general Cartesian path.
In contrast to the previous functions, the Cartesian path is specified as a set of waypoints to be sequentially reached for the origin of a robot link (link). The waypoints are transforms given either in a global reference frame or in the local reference frame of the link at the immediately preceeding waypoint. The link needs to move in a straight line between two consecutive waypoints. All other comments apply.
Definition at line 1973 of file robot_state.cpp.
|
inline |
Definition at line 1159 of file robot_state.h.
| void moveit::core::RobotState::computeVariableVelocity | ( | const JointModelGroup * | jmg, |
| Eigen::VectorXd & | qdot, | ||
| const Eigen::VectorXd & | twist, | ||
| const LinkModel * | tip | ||
| ) | const |
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot.
Definition at line 1238 of file robot_state.cpp.
|
inline |
Given a twist for a particular link (tip), compute the corresponding velocity for every variable and store it in qdot.
Definition at line 1273 of file robot_state.h.
|
private |
Definition at line 121 of file robot_state.cpp.
|
inline |
For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 878 of file robot_state.h.
|
inline |
For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 891 of file robot_state.h.
|
inline |
For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 901 of file robot_state.h.
| void moveit::core::RobotState::copyJointGroupAccelerations | ( | const JointModelGroup * | group, |
| double * | gstate | ||
| ) | const |
For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 545 of file robot_state.cpp.
|
inline |
For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 915 of file robot_state.h.
| void moveit::core::RobotState::copyJointGroupAccelerations | ( | const JointModelGroup * | group, |
| Eigen::VectorXd & | values | ||
| ) | const |
For a given group, copy the acceleration values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 555 of file robot_state.cpp.
|
inline |
For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 666 of file robot_state.h.
|
inline |
For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 679 of file robot_state.h.
|
inline |
For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 689 of file robot_state.h.
| void moveit::core::RobotState::copyJointGroupPositions | ( | const JointModelGroup * | group, |
| double * | gstate | ||
| ) | const |
For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 467 of file robot_state.cpp.
|
inline |
For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 703 of file robot_state.h.
| void moveit::core::RobotState::copyJointGroupPositions | ( | const JointModelGroup * | group, |
| Eigen::VectorXd & | values | ||
| ) | const |
For a given group, copy the position values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 477 of file robot_state.cpp.
|
inline |
For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 772 of file robot_state.h.
|
inline |
For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 785 of file robot_state.h.
|
inline |
For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 795 of file robot_state.h.
| void moveit::core::RobotState::copyJointGroupVelocities | ( | const JointModelGroup * | group, |
| double * | gstate | ||
| ) | const |
For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 506 of file robot_state.cpp.
|
inline |
For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 809 of file robot_state.h.
| void moveit::core::RobotState::copyJointGroupVelocities | ( | const JointModelGroup * | group, |
| Eigen::VectorXd & | values | ||
| ) | const |
For a given group, copy the velocity values of the variables that make up the group into another location, in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the RobotState itself, so we copy instead of returning a pointer.
Definition at line 516 of file robot_state.cpp.
|
inline |
Returns true if anything in this state is dirty.
Definition at line 1456 of file robot_state.h.
|
inline |
Definition at line 1450 of file robot_state.h.
|
inline |
Definition at line 1440 of file robot_state.h.
|
inline |
Definition at line 1445 of file robot_state.h.
|
inline |
Definition at line 1467 of file robot_state.h.
| double moveit::core::RobotState::distance | ( | const RobotState & | other, |
| const JointModelGroup * | joint_group | ||
| ) | const |
Definition at line 804 of file robot_state.cpp.
|
inline |
Definition at line 1474 of file robot_state.h.
| void moveit::core::RobotState::enforceBounds | ( | ) |
Definition at line 722 of file robot_state.cpp.
| void moveit::core::RobotState::enforceBounds | ( | const JointModelGroup * | joint_group | ) |
Definition at line 729 of file robot_state.cpp.
|
inline |
Definition at line 1503 of file robot_state.h.
|
inline |
Definition at line 1509 of file robot_state.h.
|
inline |
Definition at line 1517 of file robot_state.h.
| void moveit::core::RobotState::getAttachedBodies | ( | std::vector< const AttachedBody * > & | attached_bodies | ) | const |
Get all bodies attached to the model corresponding to this state.
Definition at line 878 of file robot_state.cpp.
| void moveit::core::RobotState::getAttachedBodies | ( | std::vector< const AttachedBody * > & | attached_bodies, |
| const JointModelGroup * | lm | ||
| ) | const |
Get all bodies attached to a particular group the model corresponding to this state.
Definition at line 887 of file robot_state.cpp.
| void moveit::core::RobotState::getAttachedBodies | ( | std::vector< const AttachedBody * > & | attached_bodies, |
| const LinkModel * | lm | ||
| ) | const |
Get all bodies attached to a particular link in the model corresponding to this state.
Definition at line 897 of file robot_state.cpp.
| const AttachedBody * moveit::core::RobotState::getAttachedBody | ( | const std::string & | name | ) | const |
Get the attached body named name. Return NULL if not found.
Definition at line 846 of file robot_state.cpp.
|
inline |
Definition at line 1384 of file robot_state.h.
|
inline |
Definition at line 1418 of file robot_state.h.
|
inline |
Definition at line 1423 of file robot_state.h.
|
inline |
Definition at line 1379 of file robot_state.h.
| const Eigen::Affine3d & moveit::core::RobotState::getFrameTransform | ( | const std::string & | id | ) |
Get the transformation matrix from the model frame to the frame identified by id.
Definition at line 969 of file robot_state.cpp.
| const Eigen::Affine3d & moveit::core::RobotState::getFrameTransform | ( | const std::string & | id | ) | const |
Get the transformation matrix from the model frame to the frame identified by id.
Definition at line 975 of file robot_state.cpp.
|
inline |
Definition at line 1368 of file robot_state.h.
|
inline |
Definition at line 1373 of file robot_state.h.
|
inline |
Definition at line 1407 of file robot_state.h.
|
inline |
Definition at line 1412 of file robot_state.h.
| bool moveit::core::RobotState::getJacobian | ( | const JointModelGroup * | group, |
| const LinkModel * | link, | ||
| const Eigen::Vector3d & | reference_point_position, | ||
| Eigen::MatrixXd & | jacobian, | ||
| bool | use_quaternion_representation = false |
||
| ) | const |
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
| group | The group to compute the Jacobian for |
| link_name | The name of the link |
| reference_point_position | The reference point position (with respect to the link specified in link_name) |
| jacobian | The resultant jacobian |
| use_quaternion_representation | Flag indicating if the Jacobian should use a quaternion representation (default is false) |
Definition at line 1115 of file robot_state.cpp.
|
inline |
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
| group | The group to compute the Jacobian for |
| link_name | The name of the link |
| reference_point_position | The reference point position (with respect to the link specified in link_name) |
| jacobian | The resultant jacobian |
| use_quaternion_representation | Flag indicating if the Jacobian should use a quaternion representation (default is false) |
Definition at line 1236 of file robot_state.h.
| Eigen::MatrixXd moveit::core::RobotState::getJacobian | ( | const JointModelGroup * | group, |
| const Eigen::Vector3d & | reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0) |
||
| ) | const |
Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an exception is thrown.
| group | The group to compute the Jacobian for |
| reference_point_position | The reference point position (with respect to the link specified in link_name) |
Definition at line 1106 of file robot_state.cpp.
|
inline |
Compute the Jacobian with reference to the last link of a specified group. If the group is not a chain, an exception is thrown.
| group | The group to compute the Jacobian for |
| reference_point_position | The reference point position (with respect to the link specified in link_name) |
Definition at line 1259 of file robot_state.h.
|
inline |
Definition at line 589 of file robot_state.h.
|
inline |
Definition at line 594 of file robot_state.h.
|
inline |
Definition at line 599 of file robot_state.h.
|
inline |
Definition at line 604 of file robot_state.h.
|
inline |
Get the model of a particular joint.
Definition at line 162 of file robot_state.h.
|
inline |
Get the model of a particular joint group.
Definition at line 168 of file robot_state.h.
|
inline |
Definition at line 569 of file robot_state.h.
|
inline |
Definition at line 574 of file robot_state.h.
|
inline |
Definition at line 1390 of file robot_state.h.
|
inline |
Definition at line 1395 of file robot_state.h.
|
inline |
Definition at line 1429 of file robot_state.h.
|
inline |
Definition at line 1434 of file robot_state.h.
|
inline |
Definition at line 579 of file robot_state.h.
|
inline |
Definition at line 584 of file robot_state.h.
|
inline |
Get the model of a particular link.
Definition at line 156 of file robot_state.h.
| std::pair< double, const JointModel * > moveit::core::RobotState::getMinDistanceToPositionBounds | ( | ) | const |
Get the minimm distance from this state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned.
Definition at line 736 of file robot_state.cpp.
| std::pair< double, const JointModel * > moveit::core::RobotState::getMinDistanceToPositionBounds | ( | const JointModelGroup * | group | ) | const |
Get the minimm distance from a group in this state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned.
Definition at line 741 of file robot_state.cpp.
| std::pair< double, const JointModel * > moveit::core::RobotState::getMinDistanceToPositionBounds | ( | const std::vector< const JointModel * > & | joints | ) | const |
Get the minimm distance from a set of joints in the state to the bounds. The minimum distance and the joint for which this minimum is achieved are returned.
Definition at line 747 of file robot_state.cpp.
|
private |
Definition at line 322 of file robot_state.cpp.
|
inline |
Return the instance of a random number generator.
Definition at line 1671 of file robot_state.h.
| void moveit::core::RobotState::getRobotMarkers | ( | visualization_msgs::MarkerArray & | arr, |
| const std::vector< std::string > & | link_names, | ||
| const std_msgs::ColorRGBA & | color, | ||
| const std::string & | ns, | ||
| const ros::Duration & | dur, | ||
| bool | include_attached = false |
||
| ) | const |
Get a MarkerArray that fully describes the robot markers for a given robot.
| arr | The returned marker array |
| link_names | The list of link names for which the markers should be created. |
| color | The color for the marker |
| ns | The namespace for the markers |
| dur | The ros::Duration for which the markers should stay visible |
Definition at line 1021 of file robot_state.cpp.
|
inline |
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first.
| arr | The returned marker array |
| link_names | The list of link names for which the markers should be created. |
| color | The color for the marker |
| ns | The namespace for the markers |
| dur | The ros::Duration for which the markers should stay visible |
Definition at line 1705 of file robot_state.h.
| void moveit::core::RobotState::getRobotMarkers | ( | visualization_msgs::MarkerArray & | arr, |
| const std::vector< std::string > & | link_names, | ||
| bool | include_attached = false |
||
| ) | const |
Get a MarkerArray that fully describes the robot markers for a given robot.
| arr | The returned marker array |
| link_names | The list of link names for which the markers should be created. |
Definition at line 1037 of file robot_state.cpp.
|
inline |
Get a MarkerArray that fully describes the robot markers for a given robot. Update the state first.
| arr | The returned marker array |
| link_names | The list of link names for which the markers should be created. |
Definition at line 1724 of file robot_state.h.
|
inline |
Get the robot model this state is constructed for.
Definition at line 138 of file robot_state.h.
|
private |
Definition at line 2278 of file robot_state.cpp.
| std::string moveit::core::RobotState::getStateTreeString | ( | const std::string & | prefix = "" | ) | const |
Definition at line 2253 of file robot_state.cpp.
|
inline |
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
Definition at line 420 of file robot_state.h.
|
inline |
Get the acceleration of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition at line 428 of file robot_state.h.
|
inline |
Get raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames(). The area of memory overlaps with effort (effort and acceleration should not be set at the same time)
Definition at line 358 of file robot_state.h.
|
inline |
Get const raw access to the accelerations of the variables that make up this state. The values are in the same order as reported by getVariableNames()
Definition at line 366 of file robot_state.h.
|
inline |
Get the number of variables that make up this state.
Definition at line 144 of file robot_state.h.
|
inline |
Get raw access to the effort of the variables that make up this state. The values are in the same order as reported by getVariableNames(). The area of memory overlaps with accelerations (effort and acceleration should not be set at the same time)
Definition at line 450 of file robot_state.h.
|
inline |
Get const raw access to the effort of the variables that make up this state. The values are in the same order as reported by getVariableNames().
Definition at line 458 of file robot_state.h.
|
inline |
Get the effort of a particular variable. An exception is thrown if the variable is not known.
Definition at line 506 of file robot_state.h.
|
inline |
Get the effort of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition at line 514 of file robot_state.h.
|
inline |
Get the names of the variables that make up this state, in the order they are stored in memory.
Definition at line 150 of file robot_state.h.
|
inline |
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition at line 241 of file robot_state.h.
|
inline |
Get the position of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition at line 249 of file robot_state.h.
|
inline |
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition at line 181 of file robot_state.h.
|
inline |
Get a raw pointer to the positions of the variables stored in this state.
Definition at line 188 of file robot_state.h.
|
inline |
Get raw access to the velocities of the variables that make up this state. The values are in the same order as reported by getVariableNames()
Definition at line 270 of file robot_state.h.
|
inline |
Get const access to the velocities of the variables that make up this state. The values are in the same order as reported by getVariableNames()
Definition at line 278 of file robot_state.h.
|
inline |
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
Definition at line 327 of file robot_state.h.
|
inline |
Get the velocity of a particular variable. The variable is specified by its index. No checks are performed for the validity of the index passed.
Definition at line 335 of file robot_state.h.
|
inline |
By default, if accelerations are never set or initialized, the state remembers that there are no accelerations set. This is useful to know when serializing or copying the state. If hasAccelerations() reports true, hasEffort() will certainly report false.
Definition at line 350 of file robot_state.h.
| bool moveit::core::RobotState::hasAttachedBody | ( | const std::string & | id | ) | const |
Check if an attached body named id exists in this state.
Definition at line 841 of file robot_state.cpp.
|
inline |
By default, if effort is never set or initialized, the state remembers that there is no effort set. This is useful to know when serializing or copying the state. If hasEffort() reports true, hasAccelerations() will certainly report false.
Definition at line 442 of file robot_state.h.
|
inline |
By default, if velocities are never set or initialized, the state remembers that there are no velocities set. This is useful to know when serializing or copying the state.
Definition at line 263 of file robot_state.h.
| bool moveit::core::RobotState::integrateVariableVelocity | ( | const JointModelGroup * | jmg, |
| const Eigen::VectorXd & | qdot, | ||
| double | dt, | ||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn() |
||
| ) |
Given the velocities for the variables in this group (qdot) and an amount of time (dt), update the current state using the Euler forward method. If the constraint specified is satisfied, return true, otherwise return false.
Definition at line 1279 of file robot_state.cpp.
| void moveit::core::RobotState::interpolate | ( | const RobotState & | to, |
| double | t, | ||
| RobotState & | state | ||
| ) | const |
Interpolate from this state towards state to, at time t in [0,1]. The result is stored in state, mimic joints are correctly updated and flags are set so that FK is recomputed when needed.
Definition at line 816 of file robot_state.cpp.
| void moveit::core::RobotState::interpolate | ( | const RobotState & | to, |
| double | t, | ||
| RobotState & | state, | ||
| const JointModelGroup * | joint_group | ||
| ) | const |
Interpolate from this state towards to, at time t in [0,1], but only for the joints in the specified group. If mimic joints need to be updated, they are updated accordingly. Flags are set so that FK computation is triggered when needed.
Definition at line 824 of file robot_state.cpp.
|
inline |
Update state by interpolating form this state towards to, at time t in [0,1] but only for the joint joint. If there are joints that mimic this joint, they are updated. Flags are set so that FK computation is triggered as needed.
Definition at line 1493 of file robot_state.h.
| void moveit::core::RobotState::invertVelocity | ( | ) |
Invert velocity if present.
Definition at line 425 of file robot_state.cpp.
| bool moveit::core::RobotState::isValidVelocityMove | ( | const RobotState & | other, |
| const JointModelGroup * | group, | ||
| double | dt | ||
| ) | const |
Check that the time to move between two waypoints is sufficient given velocity limits and time step.
| other | - robot state to compare joint positions against |
| group | - planning group to compare joint positions against |
| dt | - time step between the two points |
Definition at line 783 of file robot_state.cpp.
| bool moveit::core::RobotState::knowsFrameTransform | ( | const std::string & | id | ) | const |
Check if a transformation matrix from the model frame to frame id is known.
Definition at line 1011 of file robot_state.cpp.
|
private |
Definition at line 204 of file robot_state.cpp.
|
inlineprivate |
Definition at line 1748 of file robot_state.h.
|
inlineprivate |
Definition at line 1755 of file robot_state.h.
|
private |
Definition at line 214 of file robot_state.cpp.
|
private |
Definition at line 195 of file robot_state.cpp.
| RobotState & moveit::core::RobotState::operator= | ( | const RobotState & | other | ) |
Copy operator.
Definition at line 114 of file robot_state.cpp.
| void moveit::core::RobotState::printDirtyInfo | ( | std::ostream & | out = std::cout | ) | const |
Definition at line 2160 of file robot_state.cpp.
| void moveit::core::RobotState::printStateInfo | ( | std::ostream & | out = std::cout | ) | const |
Definition at line 2173 of file robot_state.cpp.
| void moveit::core::RobotState::printStatePositions | ( | std::ostream & | out = std::cout | ) | const |
Definition at line 2153 of file robot_state.cpp.
| void moveit::core::RobotState::printTransform | ( | const Eigen::Affine3d & | transform, |
| std::ostream & | out = std::cout |
||
| ) | const |
Definition at line 2216 of file robot_state.cpp.
| void moveit::core::RobotState::printTransforms | ( | std::ostream & | out = std::cout | ) | const |
Definition at line 2224 of file robot_state.cpp.
| bool moveit::core::RobotState::satisfiesBounds | ( | double | margin = 0.0 | ) | const |
Definition at line 704 of file robot_state.cpp.
| bool moveit::core::RobotState::satisfiesBounds | ( | const JointModelGroup * | joint_group, |
| double | margin = 0.0 |
||
| ) | const |
Definition at line 713 of file robot_state.cpp.
|
inline |
Definition at line 1524 of file robot_state.h.
|
inline |
Definition at line 1528 of file robot_state.h.
|
inline |
Definition at line 1532 of file robot_state.h.
| void moveit::core::RobotState::setAttachedBodyUpdateCallback | ( | const AttachedBodyCallback & | callback | ) |
Definition at line 836 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromDiffIK | ( | const JointModelGroup * | group, |
| const Eigen::VectorXd & | twist, | ||
| const std::string & | tip, | ||
| double | dt, | ||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn() |
||
| ) |
Set the joint values from a Cartesian velocity applied during a time dt.
| group | the group of joints this function operates on |
| twist | a Cartesian velocity on the 'tip' frame |
| tip | the frame for which the twist is given |
| dt | a time interval (seconds) |
| st | a secondary task computation function |
Definition at line 1222 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromDiffIK | ( | const JointModelGroup * | group, |
| const geometry_msgs::Twist & | twist, | ||
| const std::string & | tip, | ||
| double | dt, | ||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn() |
||
| ) |
Set the joint values from a Cartesian velocity applied during a time dt.
| group | the group of joints this function operates on |
| twist | a Cartesian velocity on the 'tip' frame |
| tip | the frame for which the twist is given |
| dt | a time interval (seconds) |
| st | a secondary task computation function |
Definition at line 1230 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const geometry_msgs::Pose & | pose, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
| pose | The pose the last link in the chain needs to achieve |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1298 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const geometry_msgs::Pose & | pose, | ||
| const std::string & | tip, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
| pose | The pose the tip link in the chain needs to achieve |
| tip | The name of the link the pose is specified for |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1311 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const Eigen::Affine3d & | pose, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
| pose | The pose the last link in the chain needs to achieve |
| tip | The name of the link the pose is specified for |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
Definition at line 1321 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const Eigen::Affine3d & | pose, | ||
| const std::string & | tip, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
| pose | The pose the last link in the chain needs to achieve |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1335 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const Eigen::Affine3d & | pose, | ||
| const std::string & | tip, | ||
| const std::vector< double > & | consistency_limits, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
If the group this state corresponds to is a chain and a solver is available, then the joint values can be set by computing inverse kinematics. The pose is assumed to be in the reference frame of the kinematic model. Returns true on success.
| pose | The pose the last link in the chain needs to achieve |
| tip | The name of the frame for which IK is attempted. |
| consistency_limits | This specifies the desired distance between the solution and the seed state |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1382 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const EigenSTL::vector_Affine3d & | poses, | ||
| const std::vector< std::string > & | tips, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success.
| poses | The poses the last link in each chain needs to achieve |
| tips | The names of the frames for which IK is attempted. |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1400 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIK | ( | const JointModelGroup * | group, |
| const EigenSTL::vector_Affine3d & | poses, | ||
| const std::vector< std::string > & | tips, | ||
| const std::vector< std::vector< double > > & | consistency_limits, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
Warning: This function inefficiently copies all transforms around. If the group consists of a set of sub-groups that are each a chain and a solver is available for each sub-group, then the joint values can be set by computing inverse kinematics. The poses are assumed to be in the reference frame of the kinematic model. The poses are assumed to be in the same order as the order of the sub-groups in this group. Returns true on success.
| poses | The poses the last link in each chain needs to achieve |
| tips | The names of the frames for which IK is attempted. |
| consistency_limits | This specifies the desired distance between the solution and the seed state |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1409 of file robot_state.cpp.
| bool moveit::core::RobotState::setFromIKSubgroups | ( | const JointModelGroup * | group, |
| const EigenSTL::vector_Affine3d & | poses, | ||
| const std::vector< std::string > & | tips, | ||
| const std::vector< std::vector< double > > & | consistency_limits, | ||
| unsigned int | attempts = 0, |
||
| double | timeout = 0.0, |
||
| const GroupStateValidityCallbackFn & | constraint = GroupStateValidityCallbackFn(), |
||
| const kinematics::KinematicsQueryOptions & | options = kinematics::KinematicsQueryOptions() |
||
| ) |
setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for non-chain kinematics. In this case, we divide the group into subgroups and do IK solving individually
| poses | The poses the last link in each chain needs to achieve |
| tips | The names of the frames for which IK is attempted. |
| consistency_limits | This specifies the desired distance between the solution and the seed state |
| attempts | The number of times IK is attempted |
| timeout | The timeout passed to the kinematics solver on each attempt |
| constraint | A state validity constraint to be required for IK solutions |
Definition at line 1678 of file robot_state.cpp.
| void moveit::core::RobotState::setJointEfforts | ( | const JointModel * | joint, |
| const double * | effort | ||
| ) |
Definition at line 434 of file robot_state.cpp.
|
inline |
Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 830 of file robot_state.h.
|
inline |
Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 840 of file robot_state.h.
|
inline |
Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 850 of file robot_state.h.
| void moveit::core::RobotState::setJointGroupAccelerations | ( | const JointModelGroup * | group, |
| const double * | gstate | ||
| ) |
Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 524 of file robot_state.cpp.
|
inline |
Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 863 of file robot_state.h.
| void moveit::core::RobotState::setJointGroupAccelerations | ( | const JointModelGroup * | group, |
| const Eigen::VectorXd & | values | ||
| ) |
Given accelerations for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 537 of file robot_state.cpp.
|
inline |
Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 618 of file robot_state.h.
|
inline |
Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 628 of file robot_state.h.
|
inline |
Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 638 of file robot_state.h.
| void moveit::core::RobotState::setJointGroupPositions | ( | const JointModelGroup * | group, |
| const double * | gstate | ||
| ) |
Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 446 of file robot_state.cpp.
|
inline |
Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 651 of file robot_state.h.
| void moveit::core::RobotState::setJointGroupPositions | ( | const JointModelGroup * | group, |
| const Eigen::VectorXd & | values | ||
| ) |
Given positions for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 459 of file robot_state.cpp.
|
inline |
Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 724 of file robot_state.h.
|
inline |
Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 734 of file robot_state.h.
|
inline |
Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 744 of file robot_state.h.
| void moveit::core::RobotState::setJointGroupVelocities | ( | const JointModelGroup * | group, |
| const double * | gstate | ||
| ) |
Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 485 of file robot_state.cpp.
|
inline |
Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 757 of file robot_state.h.
| void moveit::core::RobotState::setJointGroupVelocities | ( | const JointModelGroup * | group, |
| const Eigen::VectorXd & | values | ||
| ) |
Given velocities for the variables that make up a group, in the order found in the group (including values of mimic joints), set those as the new values that correspond to the group.
Definition at line 498 of file robot_state.cpp.
|
inline |
Definition at line 527 of file robot_state.h.
|
inline |
Definition at line 532 of file robot_state.h.
|
inline |
Definition at line 537 of file robot_state.h.
|
inline |
Definition at line 542 of file robot_state.h.
|
inline |
Definition at line 549 of file robot_state.h.
|
inline |
Definition at line 554 of file robot_state.h.
|
inline |
Definition at line 561 of file robot_state.h.
| void moveit::core::RobotState::setToDefaultValues | ( | ) |
Set all joints to their default positions. The default position is 0, or if that is not within bounds then half way between min and max bound.
Definition at line 288 of file robot_state.cpp.
| bool moveit::core::RobotState::setToDefaultValues | ( | const JointModelGroup * | group, |
| const std::string & | name | ||
| ) |
Set the joints in group to the position name defined in the SRDF.
Definition at line 280 of file robot_state.cpp.
| bool moveit::core::RobotState::setToIKSolverFrame | ( | Eigen::Affine3d & | pose, |
| const kinematics::KinematicsBaseConstPtr & | solver | ||
| ) |
Convert the frame of reference of the pose to that same frame as the IK solver expects.
| pose | - the input to change |
| solver | - a kin solver whose base frame is important to us |
Definition at line 1361 of file robot_state.cpp.
| bool moveit::core::RobotState::setToIKSolverFrame | ( | Eigen::Affine3d & | pose, |
| const std::string & | ik_frame | ||
| ) |
Convert the frame of reference of the pose to that same frame as the IK solver expects.
| pose | - the input to change |
| ik_frame | - the name of frame of reference of base of ik solver |
Definition at line 1366 of file robot_state.cpp.
| void moveit::core::RobotState::setToRandomPositions | ( | ) |
Set all joints to random values. Values will be within default bounds.
Definition at line 224 of file robot_state.cpp.
| void moveit::core::RobotState::setToRandomPositions | ( | const JointModelGroup * | group | ) |
Set all joints in group to random values. Values will be within default bounds.
Definition at line 233 of file robot_state.cpp.
| void moveit::core::RobotState::setToRandomPositions | ( | const JointModelGroup * | group, |
| random_numbers::RandomNumberGenerator & | rng | ||
| ) |
Set all joints in group to random values using a specified random number generator. Values will be within default bounds.
Definition at line 240 of file robot_state.cpp.
| void moveit::core::RobotState::setToRandomPositionsNearBy | ( | const JointModelGroup * | group, |
| const RobotState & | near, | ||
| double | distance | ||
| ) |
Set all joints in group to random values near the value in . distance is the maximum amount each joint value will vary from the corresponding value in near. represents meters for prismatic/postitional joints and radians for revolute/orientation joints. Resulting values are clamped within default bounds.
Definition at line 265 of file robot_state.cpp.
| void moveit::core::RobotState::setToRandomPositionsNearBy | ( | const JointModelGroup * | group, |
| const RobotState & | near, | ||
| const std::vector< double > & | distances | ||
| ) |
Set all joints in group to random values near the value in . distances MUST have the same size as group.getActiveJointModels(). Each value in distances is the maximum amount the corresponding active joint in group will vary from the corresponding value in near. represents meters for prismatic/postitional joints and radians for revolute/orientation joints. Resulting values are clamped within default bounds.
Definition at line 248 of file robot_state.cpp.
|
inline |
Set the acceleration of a variable. If an unknown variable name is specified, an exception is thrown.
Definition at line 406 of file robot_state.h.
|
inline |
Set the acceleration of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
Definition at line 413 of file robot_state.h.
|
inline |
Given an array with acceleration values for all variables, set those values as the accelerations in this state.
Definition at line 373 of file robot_state.h.
|
inline |
Given an array with acceleration values for all variables, set those values as the accelerations in this state.
Definition at line 384 of file robot_state.h.
| void moveit::core::RobotState::setVariableAccelerations | ( | const std::map< std::string, double > & | variable_map | ) |
Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 377 of file robot_state.cpp.
| void moveit::core::RobotState::setVariableAccelerations | ( | const std::map< std::string, double > & | variable_map, |
| std::vector< std::string > & | missing_variables | ||
| ) |
Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
Definition at line 385 of file robot_state.cpp.
| void moveit::core::RobotState::setVariableAccelerations | ( | const std::vector< std::string > & | variable_names, |
| const std::vector< double > & | variable_acceleration | ||
| ) |
Set the accelerations of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 392 of file robot_state.cpp.
|
inline |
Given an array with effort values for all variables, set those values as the effort in this state.
Definition at line 464 of file robot_state.h.
|
inline |
Given an array with effort values for all variables, set those values as the effort in this state.
Definition at line 473 of file robot_state.h.
| void moveit::core::RobotState::setVariableEffort | ( | const std::map< std::string, double > & | variable_map | ) |
Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 401 of file robot_state.cpp.
| void moveit::core::RobotState::setVariableEffort | ( | const std::map< std::string, double > & | variable_map, |
| std::vector< std::string > & | missing_variables | ||
| ) |
Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
Definition at line 409 of file robot_state.cpp.
| void moveit::core::RobotState::setVariableEffort | ( | const std::vector< std::string > & | variable_names, |
| const std::vector< double > & | variable_acceleration | ||
| ) |
Set the effort of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 416 of file robot_state.cpp.
|
inline |
Set the effort of a variable. If an unknown variable name is specified, an exception is thrown.
Definition at line 492 of file robot_state.h.
|
inline |
Set the effort of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
Definition at line 499 of file robot_state.h.
|
inline |
Set the position of a single variable. An exception is thrown if the variable name is not known.
Definition at line 222 of file robot_state.h.
|
inline |
Set the position of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
Definition at line 229 of file robot_state.h.
| void moveit::core::RobotState::setVariablePositions | ( | const double * | position | ) |
It is assumed positions is an array containing the new positions for all variables in this state. Those values are copied into the state.
Definition at line 297 of file robot_state.cpp.
|
inline |
It is assumed positions is an array containing the new positions for all variables in this state. Those values are copied into the state.
Definition at line 201 of file robot_state.h.
| void moveit::core::RobotState::setVariablePositions | ( | const std::map< std::string, double > & | variable_map | ) |
Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 309 of file robot_state.cpp.
| void moveit::core::RobotState::setVariablePositions | ( | const std::map< std::string, double > & | variable_map, |
| std::vector< std::string > & | missing_variables | ||
| ) |
Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
Definition at line 333 of file robot_state.cpp.
| void moveit::core::RobotState::setVariablePositions | ( | const std::vector< std::string > & | variable_names, |
| const std::vector< double > & | variable_position | ||
| ) |
Set the positions of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
Definition at line 340 of file robot_state.cpp.
|
inline |
Definition at line 1292 of file robot_state.h.
|
inline |
Given an array with velocity values for all variables, set those values as the velocities in this state.
Definition at line 284 of file robot_state.h.
|
inline |
Given an array with velocity values for all variables, set those values as the velocities in this state.
Definition at line 292 of file robot_state.h.
| void moveit::core::RobotState::setVariableVelocities | ( | const std::map< std::string, double > & | variable_map | ) |
Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 353 of file robot_state.cpp.
| void moveit::core::RobotState::setVariableVelocities | ( | const std::map< std::string, double > & | variable_map, |
| std::vector< std::string > & | missing_variables | ||
| ) |
Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown. Additionally, missing_variables is filled with the names of the variables that are not set.
Definition at line 361 of file robot_state.cpp.
| void moveit::core::RobotState::setVariableVelocities | ( | const std::vector< std::string > & | variable_names, |
| const std::vector< double > & | variable_velocity | ||
| ) |
Set the velocities of a set of variables. If unknown variable names are specified, an exception is thrown.
Definition at line 368 of file robot_state.cpp.
|
inline |
Set the velocity of a variable. If an unknown variable name is specified, an exception is thrown.
Definition at line 313 of file robot_state.h.
|
inline |
Set the velocity of a single variable. The variable is specified by its index (a value associated by the RobotModel to each variable)
Definition at line 320 of file robot_state.h.
|
static |
Tests for absolute joint space jumps of the trajectory traj.
The joint-space difference between consecutive waypoints is computed for each active joint and compared to the absolute thresholds revolute_jump_threshold for revolute joints and prismatic_jump_threshold for prismatic joints. If these thresholds are exceeded, the trajectory is truncated.
| group | The joint model group of the robot state. |
| traj | The trajectory that should be tested. |
| revolute_jump_threshold | Absolute joint-space threshold for revolute joints. |
| prismatic_jump_threshold | Absolute joint-space threshold for prismatic joints. |
Definition at line 2063 of file robot_state.cpp.
|
static |
Tests joint space jumps of a trajectory.
If jump_threshold_factor is non-zero, we test for relative jumps. If revolute_jump_threshold or prismatic_jump_threshold are non-zero, we test for absolute jumps. Both tests can be combined. If all params are zero, jump detection is disabled. For relative jump detection, the average joint-space distance between consecutive points in the trajectory is computed. If any individual joint-space motion delta is larger then this average distance by a factor of jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just before the jump.
| group | The joint model group of the robot state. |
| traj | The trajectory that should be tested. |
| jump_threshold | The struct holding jump thresholds to determine if a joint space jump has occurred. |
Definition at line 2012 of file robot_state.cpp.
|
static |
Tests for relative joint space jumps of the trajectory traj.
First, the average distance between adjacent trajectory points is computed. If two adjacent trajectory points have distance > jump_threshold_factor * average, the trajectory is truncated at this point.
| group | The joint model group of the robot state. |
| traj | The trajectory that should be tested. |
| jump_threshold_factor | The threshold to determine if a joint space jump has occurred . |
Definition at line 2028 of file robot_state.cpp.
| void moveit::core::RobotState::update | ( | bool | force = false | ) |
Update all transforms.
Definition at line 563 of file robot_state.cpp.
| void moveit::core::RobotState::updateCollisionBodyTransforms | ( | ) |
Update the transforms for the collision bodies. This call is needed before calling collision checking. If updating link transforms or joint transorms is needed, the corresponding updates are also triggered.
Definition at line 576 of file robot_state.cpp.
| void moveit::core::RobotState::updateLinkTransforms | ( | ) |
Update the reference frame transforms for links. This call is needed before using the transforms of links for coordinate transforms.
Definition at line 600 of file robot_state.cpp.
|
private |
Definition at line 614 of file robot_state.cpp.
|
inlineprivate |
Definition at line 1769 of file robot_state.h.
|
inlineprivate |
Update a set of joints that are certain to be mimicking other joints.
Definition at line 1782 of file robot_state.h.
|
inlineprivate |
Update all mimic joints within group.
Definition at line 1797 of file robot_state.h.
|
inline |
Update the state after setting a particular link to the input global transform pose.
This "warps" the given link to the given pose, neglecting the joint values of its parent joint. The link transforms of link and all its descendants are updated, but not marked as dirty, although they do not match the joint values anymore! Collision body transforms are not yet updated, but marked dirty only. Use update(false) or updateCollisionBodyTransforms() to update them as well.
Definition at line 1360 of file robot_state.h.
| void moveit::core::RobotState::updateStateWithLinkAt | ( | const LinkModel * | link, |
| const Eigen::Affine3d & | transform, | ||
| bool | backward = false |
||
| ) |
Update the state after setting a particular link to the input global transform pose.
Definition at line 652 of file robot_state.cpp.
|
private |
Definition at line 1828 of file robot_state.h.
|
private |
All attached bodies that are part of this state, indexed by their name.
Definition at line 1843 of file robot_state.h.
|
private |
This event is called when there is a change in the attached bodies for this state; The event specifies the body that changed and whether it was just attached or about to be detached.
Definition at line 1847 of file robot_state.h.
|
private |
Definition at line 1835 of file robot_state.h.
|
private |
Definition at line 1840 of file robot_state.h.
|
private |
Definition at line 1834 of file robot_state.h.
|
private |
Definition at line 1829 of file robot_state.h.
|
private |
Definition at line 1839 of file robot_state.h.
|
private |
Definition at line 1838 of file robot_state.h.
|
private |
Definition at line 1831 of file robot_state.h.
|
private |
Definition at line 1832 of file robot_state.h.
|
private |
Definition at line 1830 of file robot_state.h.
|
private |
Definition at line 1824 of file robot_state.h.
|
private |
Definition at line 1826 of file robot_state.h.
|
private |
For certain operations a state needs a random number generator. However, it may be slightly expensive to allocate the random number generator if many state instances are generated. For this reason, the generator is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but call getRandomNumberGenerator() instead.
Definition at line 1854 of file robot_state.h.
|
private |
Definition at line 1823 of file robot_state.h.
|
private |
Definition at line 1837 of file robot_state.h.
|
private |
Definition at line 1827 of file robot_state.h.