Provides wrappers for performing KDL functions on a designated kinematic chain given a URDF representation of a robot. More...
Public Member Functions | |
def | __init__ |
Constructor. | |
def | cart_inertia |
Returns the cartesian space mass matrix at the end_link for the given joint angles. | |
def | clip_joints_safe |
Clips joint angles to the safety limits. | |
def | difference_joints |
Returns a difference between the two sets of joint angles while insuring that the shortest angle is returned for the continuous joints. | |
def | extract_joint_state |
def | FK |
def | forward |
Forward kinematics on the given joint angles, returning the location of the end_link in the base_link's frame. | |
def | get_joint_limits |
def | get_joint_names |
def | get_link_names |
def | inertia |
Returns the joint space mass matrix at the end_link for the given joint angles. | |
def | inverse |
Inverse kinematics for a given pose, returning the joint angles required to obtain the target pose. | |
def | inverse_biased |
Performs an IK search while trying to balance the demands of reaching the goal, maintaining a posture, and prioritizing rotation or position. | |
def | inverse_biased_search |
inverse_biased with random restarts. | |
def | inverse_search |
Repeats IK for different sets of random initial angles until a solution is found or the call times out. | |
def | jacobian |
Returns the Jacobian matrix at the end_link for the given joint angles. | |
def | joints_in_limits |
Tests to see if the given joint angles are in the joint limits. | |
def | joints_in_safe_limits |
Tests to see if the given joint angles are in the joint safety limits. | |
def | random_joint_angles |
Returns a set of random joint angles distributed uniformly in the safety limits. | |
Public Attributes | |
base_link | |
chain | |
end_link | |
joint_limits_lower | |
joint_limits_upper | |
joint_safety_lower | |
joint_safety_upper | |
joint_types | |
num_joints | |
tree | |
urdf | |
Private Member Functions | |
def | _do_kdl_fk |
Private Attributes | |
_dyn_kdl | |
_fk_kdl | |
_ik_p_kdl | |
_ik_v_kdl | |
_jac_kdl |
Provides wrappers for performing KDL functions on a designated kinematic chain given a URDF representation of a robot.
Definition at line 56 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.__init__ | ( | self, | |
urdf, | |||
base_link, | |||
end_link, | |||
kdl_tree = None |
|||
) |
Constructor.
urdf | URDF object of robot. |
base_link | Name of the root link of the kinematic chain. |
end_link | Name of the end link of the kinematic chain. |
kdl_tree | Optional KDL.Tree object to use. If None, one will be generated from the URDF. |
Reimplemented in pykdl_utils.joint_kinematics.JointKinematicsWait.
Definition at line 64 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics._do_kdl_fk | ( | self, | |
q, | |||
link_number | |||
) | [private] |
Definition at line 202 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.cart_inertia | ( | self, | |
q | |||
) |
Returns the cartesian space mass matrix at the end_link for the given joint angles.
q | List of joint angles. |
Reimplemented in pykdl_utils.joint_kinematics.JointKinematicsBase.
Definition at line 302 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.clip_joints_safe | ( | self, | |
q | |||
) |
Clips joint angles to the safety limits.
List | of joint angles. |
Definition at line 329 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.difference_joints | ( | self, | |
q1, | |||
q2 | |||
) |
Returns a difference between the two sets of joint angles while insuring that the shortest angle is returned for the continuous joints.
q1 | List of joint angles. |
q2 | List of joint angles. |
Definition at line 351 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.extract_joint_state | ( | self, | |
js, | |||
joint_names = None |
|||
) |
Definition at line 121 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.FK | ( | self, | |
q, | |||
link_number = None |
|||
) |
Definition at line 157 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.forward | ( | self, | |
q, | |||
end_link = None , |
|||
base_link = None |
|||
) |
Forward kinematics on the given joint angles, returning the location of the end_link in the base_link's frame.
q | List of joint angles for the full kinematic chain. |
end_link | Name of the link the pose should be obtained for. |
base_link | Name of the root link frame the end_link should be found in. |
Reimplemented in pykdl_utils.joint_kinematics.JointKinematicsBase.
Definition at line 174 of file kdl_kinematics.py.
Definition at line 154 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.get_joint_names | ( | self, | |
links = False , |
|||
fixed = False |
|||
) |
Definition at line 150 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.get_link_names | ( | self, | |
joints = False , |
|||
fixed = True |
|||
) |
Definition at line 145 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.inertia | ( | self, | |
q | |||
) |
Returns the joint space mass matrix at the end_link for the given joint angles.
q | List of joint angles. |
Reimplemented in pykdl_utils.joint_kinematics.JointKinematicsBase.
Definition at line 293 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.inverse | ( | self, | |
pose, | |||
q_guess = None , |
|||
min_joints = None , |
|||
max_joints = None |
|||
) |
Inverse kinematics for a given pose, returning the joint angles required to obtain the target pose.
pose | Pose-like object represeting the target pose of the end effector. |
q_guess | List of joint angles to seed the IK search. |
min_joints | List of joint angles to lower bound the angles on the IK search. If None, the safety limits are used. |
max_joints | List of joint angles to upper bound the angles on the IK search. If None, the safety limits are used. |
Definition at line 227 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.inverse_biased | ( | self, | |
pose, | |||
q_init, | |||
q_bias, | |||
q_bias_weights, | |||
rot_weight = 1. , |
|||
bias_vel = 0.01 , |
|||
num_iter = 100 |
|||
) |
Performs an IK search while trying to balance the demands of reaching the goal, maintaining a posture, and prioritizing rotation or position.
Definition at line 366 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.inverse_biased_search | ( | self, | |
pos, | |||
rot, | |||
q_bias, | |||
q_bias_weights, | |||
rot_weight = 1. , |
|||
bias_vel = 0.01 , |
|||
num_iter = 100 , |
|||
num_search = 20 |
|||
) |
inverse_biased with random restarts.
Definition at line 392 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.inverse_search | ( | self, | |
pose, | |||
timeout = 1. |
|||
) |
Repeats IK for different sets of random initial angles until a solution is found or the call times out.
pose | Pose-like object represeting the target pose of the end effector. |
timeout | Time in seconds to look for a solution. |
Definition at line 263 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.jacobian | ( | self, | |
q, | |||
pos = None |
|||
) |
Returns the Jacobian matrix at the end_link for the given joint angles.
q | List of joint angles. |
pos | Point in base frame where the jacobian is acting on. If None, we assume the end_link |
Definition at line 278 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.joints_in_limits | ( | self, | |
q | |||
) |
Tests to see if the given joint angles are in the joint limits.
List | of joint angles. |
Definition at line 311 of file kdl_kinematics.py.
def pykdl_utils.kdl_kinematics.KDLKinematics.joints_in_safe_limits | ( | self, | |
q | |||
) |
Tests to see if the given joint angles are in the joint safety limits.
List | of joint angles. |
Definition at line 320 of file kdl_kinematics.py.
Returns a set of random joint angles distributed uniformly in the safety limits.
Definition at line 337 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Reimplemented in pykdl_utils.joint_kinematics.JointKinematicsBase.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.
Definition at line 64 of file kdl_kinematics.py.