Public Member Functions | Public Attributes | Private Member Functions | Private Attributes
pykdl_utils.kdl_kinematics.KDLKinematics Class Reference

Provides wrappers for performing KDL functions on a designated kinematic chain given a URDF representation of a robot. More...

Inheritance diagram for pykdl_utils.kdl_kinematics.KDLKinematics:
Inheritance graph
[legend]

List of all members.

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

Detailed Description

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.


Constructor & Destructor Documentation

def pykdl_utils.kdl_kinematics.KDLKinematics.__init__ (   self,
  urdf,
  base_link,
  end_link,
  kdl_tree = None 
)

Constructor.

Parameters:
urdfURDF object of robot.
base_linkName of the root link of the kinematic chain.
end_linkName of the end link of the kinematic chain.
kdl_treeOptional 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.


Member Function Documentation

def pykdl_utils.kdl_kinematics.KDLKinematics._do_kdl_fk (   self,
  q,
  link_number 
) [private]

Definition at line 202 of file kdl_kinematics.py.

Returns the cartesian space mass matrix at the end_link for the given joint angles.

Parameters:
qList of joint angles.
Returns:
6x6 np.mat Cartesian inertia matrix

Reimplemented in pykdl_utils.joint_kinematics.JointKinematicsBase.

Definition at line 302 of file kdl_kinematics.py.

Clips joint angles to the safety limits.

Parameters:
Listof joint angles.
Returns:
np.array list of clipped joint angles.

Definition at line 329 of file kdl_kinematics.py.

Returns a difference between the two sets of joint angles while insuring that the shortest angle is returned for the continuous joints.

Parameters:
q1List of joint angles.
q2List of joint angles.
Returns:
np.array of wrapped joint angles for retval = q1 - q2

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.

Parameters:
qList of joint angles for the full kinematic chain.
end_linkName of the link the pose should be obtained for.
base_linkName of the root link frame the end_link should be found in.
Returns:
4x4 numpy.mat homogeneous transformation

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 
)
Returns:
List of joint names in the kinematic chain.

Definition at line 150 of file kdl_kinematics.py.

def pykdl_utils.kdl_kinematics.KDLKinematics.get_link_names (   self,
  joints = False,
  fixed = True 
)
Returns:
List of link names in the kinematic chain.

Definition at line 145 of file kdl_kinematics.py.

Returns the joint space mass matrix at the end_link for the given joint angles.

Parameters:
qList of joint angles.
Returns:
NxN np.mat Inertia matrix

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.

Parameters:
posePose-like object represeting the target pose of the end effector.
q_guessList of joint angles to seed the IK search.
min_jointsList of joint angles to lower bound the angles on the IK search. If None, the safety limits are used.
max_jointsList of joint angles to upper bound the angles on the IK search. If None, the safety limits are used.
Returns:
np.array of joint angles needed to reach the pose or None if no solution was found.

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.

Parameters:
posePose-like object represeting the target pose of the end effector.
timeoutTime in seconds to look for a solution.
Returns:
np.array of joint angles needed to reach the pose or None if no solution was found.

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.

Parameters:
qList of joint angles.
Returns:
6xN np.mat Jacobian
Parameters:
posPoint 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.

Tests to see if the given joint angles are in the joint limits.

Parameters:
Listof joint angles.
Returns:
True if joint angles in joint limits.

Definition at line 311 of file kdl_kinematics.py.

Tests to see if the given joint angles are in the joint safety limits.

Parameters:
Listof joint angles.
Returns:
True if joint angles in joint safety limits.

Definition at line 320 of file kdl_kinematics.py.

Returns a set of random joint angles distributed uniformly in the safety limits.

Returns:
np.array list of random joint angles.

Definition at line 337 of file kdl_kinematics.py.


Member Data Documentation

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.


The documentation for this class was generated from the following file:


pykdl_utils
Author(s): Kelsey Hawkins
autogenerated on Fri Aug 28 2015 11:05:34