moveit.core.robot_state
Functions
Classes
Representation of a robot's state. |
- class moveit.core.robot_state.RobotState
Representation of a robot’s state. 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.
- clear_attached_bodies()
Clear all attached bodies. We only allow for attaching of objects via the PlanningScene instance. This method allows any attached objects that are associated to this RobotState instance to be removed.
- property dirty
True if the robot state is dirty.
- Type
bool
- get_frame_transform()
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_id. If frame_id was not found, frame_found is set to false and an identity transform is returned. This method is restricted to frames defined within the robot state and doesn’t include collision object present in the collision world. Please use the PlanningScene.getFrameTransform method for collision world objects.
- Parameters
frame_id (str) – The id of the frame to get the transform for.
- Returns
numpy.ndarray
– The transformation matrix from the model frame to the frame identified by frame_id.
- get_global_link_transform()
Returns the transform of the specified link in the global frame.
- Parameters
link_name (str) – The name of the link to get the transform for.
- Returns
numpy.ndarray
– The transform of the specified link in the global frame.
- get_jacobian()
Compute the Jacobian with reference to the last link of a specified group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to compute the Jacobian for.
reference_point_position (
numpy.ndarray
) – The position of the reference point in the link frame.
- Returns
numpy.ndarray
– The Jacobian of the specified group with respect to the reference point.- Raises
Exception – If the group is not a chain.
Compute the Jacobian with reference to a particular point on a given link, for a specified group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to compute the Jacobian for.
link_name (str) – The name of the link model to compute the Jacobian for.
reference_point_position (
numpy.ndarray
) – The position of the reference point in the link frame.use_quaternion_representation (bool) – If true, the Jacobian will be represented using a quaternion representation, if false it defaults to euler angle representation.
- Returns
numpy.ndarray
– The Jacobian of the specified group with respect to the reference point.
- get_joint_group_accelerations()
For a given group, get the acceleration values of the variables that make up the group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to copy the accelerations for.
- Returns
numpy.ndarray
– The accelerations of the joints in the joint model group.
- get_joint_group_positions()
For a given group, get the position values of the variables that make up the group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to copy the positions for.
- Returns
numpy.ndarray
– The positions of the joints in the joint model group.
- get_joint_group_velocities()
For a given group, get the velocity values of the variables that make up the group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to copy the velocities for.
- Returns
numpy.ndarray
– The velocities of the joints in the joint model group.
- get_pose()
Get the pose of a link that is defined in the robot model.
- Parameters
link_name (str) – The name of the link to get the pose for.
- Returns
geometry_msgs.msg.Pose – A ROS geometry message containing the pose of the link.
- property joint_accelerations
- property joint_efforts
- property joint_positions
- property joint_velocities
- property robot_model
The robot model instance associated to this robot state.
- Type
moveit_py.core.RobotModel
- set_from_ik()
Sets the state of the robot to the one that results from solving the inverse kinematics for the specified group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to set the state for.
geometry_pose (geometry_msgs.msg.Pose) – The pose of the end-effector to solve the inverse kinematics for.
tip_name (str) – The name of the link that is the tip of the end-effector.
timeout (float) – The amount of time to wait for the IK solution to be found.
- set_joint_group_accelerations()
Sets the accelerations of the joints in the specified joint model group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to set the accelerations for.
acceleration_values (
numpy.ndarray
) – The accelerations of the joints in the joint model group.
- set_joint_group_active_positions()
Sets the active positions of joints in the specified joint model group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to set the active positions for.
position_values (
numpy.ndarray
) – The positions of the joints in the joint model group.
- set_joint_group_positions()
Sets the positions of the joints in the specified joint model group.
- Parameters
joint_model_group_name (str)
position_values (
numpy.ndarray
) – The positions of the joints in the joint model group.
- set_joint_group_velocities()
Sets the velocities of the joints in the specified joint model group.
- Parameters
joint_model_group_name (str) – The name of the joint model group to set the velocities for.
velocity_values (
numpy.ndarray
) – The velocities of the joints in the joint model group.
- set_to_default_values()
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.
Set the joints in group to the position name defined in the SRDF.
- Parameters
joint_model_group (
moveit_py.core.JointModelGroup
) – The joint model group to set the default values for.name (str) – The name of a predefined state which is defined in the robot model SRDF.
Set the joints in group to the position name defined in the SRDF.
- Parameters
joint_model_group_name (str) – The name of the joint model group to set the default values for.
name (str) – The name of a predefined state which is defined in the robot model SRDF.
- set_to_random_positions()
Set all joints to random positions within the default bounds.
Set all joints in the joint model group to random positions within the default bounds.
- Parameters
joint_model_group (
moveit_py.core.JointModelGroup
) – The joint model group to set the random values for.
- property state_info
- property state_tree
represents the state tree of the robot state.
- Type
str
- update()
Update state transforms.
- Parameters
force (bool) – when true forces the update of the transforms from scratch.
category (str) – specifies the category to update. All indicates updating all transforms while “links_only” and “collisions_only” ensure that only links or collision transforms are updated.
- moveit.core.robot_state.robotStateToRobotStateMsg()