rofunc.utils.robolab.kinematics.pytorch_kinematics_utils

1.  Module Contents

1.1.  Classes

IKSolution
LineSearch
BacktrackingLineSearch
InverseKinematics	Jacobian follower based inverse kinematics solver
PseudoInverseIK
PseudoInverseIKWithSVD

1.2.  Functions

build_chain_from_model	Build a serial chain from a URDF or MuJoCo XML file :param model_path: the path of the URDF or MuJoCo XML file :param verbose: whether to print the chain :return: robot kinematics chain
gaussian_around_config
delta_pose	Determine the error in position and rotation between the given poses and the target poses :param m: (N x M x 4 x 4) tensor of homogenous transforms :param target_pos: :param target_wxyz: target orientation represented in unit quaternion :return: (N*M, 6, 1) tensor of delta pose (dx, dy, dz, droll, dpitch, dyaw)
apply_mask

1.3.  Data

JOINT_TYPE_MAP

1.4.  API

rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.JOINT_TYPE_MAP = None

rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.build_chain_from_model(model_path: str, verbose=False)

Build a serial chain from a URDF or MuJoCo XML file :param model_path: the path of the URDF or MuJoCo XML file :param verbose: whether to print the chain :return: robot kinematics chain

class rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.IKSolution(dof, num_problems, num_retries, pos_tolerance, rot_tolerance, device='cpu')

Initialization

update_remaining_with_keep_mask(keep: torch.tensor)

update(q: torch.tensor, err: torch.tensor, use_keep_mask=True, keep_mask=None)

rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.gaussian_around_config(config: torch.Tensor, std: float) → Callable[[int], torch.Tensor]

class rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.LineSearch

abstract do_line_search(chain, q, dq, target_pos, target_wxyz, initial_dx, problem_remaining=None)

class rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.BacktrackingLineSearch(max_lr=1.0, decrease_factor=0.5, max_iterations=5, sufficient_decrease=0.01)

Bases: rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.LineSearch

Initialization

do_line_search(chain, q, dq, target_pos, target_wxyz, initial_dx, problem_remaining=None)

class rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.InverseKinematics(serial_chain: pytorch_kinematics.chain.SerialChain, pos_tolerance: float = 0.001, rot_tolerance: float = 0.01, retry_configs: Optional[torch.Tensor] = None, num_retries: Optional[int] = None, joint_limits: Optional[torch.Tensor] = None, config_sampling_method: Union[str, Callable[[int], torch.Tensor]] = 'uniform', max_iterations: int = 50, lr: float = 0.2, line_search: Optional[rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.LineSearch] = None, regularlization: float = 1e-09, debug=False, early_stopping_any_converged=False, early_stopping_no_improvement='any', early_stopping_no_improvement_patience=2, optimizer_method: Union[str, Type[torch.optim.Optimizer]] = 'sgd')

Jacobian follower based inverse kinematics solver

Initialization

Parameters:

• serial_chain –

• pos_tolerance – position tolerance in meters

• rot_tolerance – rotation tolerance in radians

• retry_configs – (M, DOF) tensor of initial configs to try for each problem; leave as None to sample

• num_retries – number, M, of random initial configs to try for that problem; implemented with batching

• joint_limits – (DOF, 2) tensor of joint limits (min, max) for each joint in radians

• config_sampling_method – either “uniform” or “gaussian” or a function that takes in the number of configs

• max_iterations – maximum number of iterations to run

• lr – learning rate

• line_search – LineSearch object to use for line search

• regularlization – regularization term to add to the Jacobian

• debug – whether to print debug information

• early_stopping_any_converged – whether to stop when any of the retries for a problem converged

• early_stopping_no_improvement – {None, “all”, “any”, ratio} whether to stop when no improvement is made

(consecutive iterations no improvement in minimum error - number of consecutive iterations is the patience). None means no early stopping from this, “all” means stop when all retries for that problem makes no improvement, “any” means stop when any of the retries for that problem makes no improvement, and ratio means stop when the ratio (between 0 and 1) of the number of retries that is making improvement falls below the ratio. So “all” is equivalent to ratio=0.999, and “any” is equivalent to ratio=0.001 :param early_stopping_no_improvement_patience: number of consecutive iterations with no improvement before considering it no improvement :param optimizer_method: either a string or a torch.optim.Optimizer class

clear()

sample_configs(num_configs: int) → torch.Tensor

abstract solve(target_poses: pytorch_kinematics.transforms.Transform3d) → rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.IKSolution

Solve IK for the given target poses in robot frame :param target_poses: (N, 4, 4) tensor, goal pose in robot frame :return: IKSolution solutions

rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.delta_pose(m: torch.tensor, target_pos, target_wxyz)

Determine the error in position and rotation between the given poses and the target poses :param m: (N x M x 4 x 4) tensor of homogenous transforms :param target_pos: :param target_wxyz: target orientation represented in unit quaternion :return: (N*M, 6, 1) tensor of delta pose (dx, dy, dz, droll, dpitch, dyaw)

rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.apply_mask(mask, *args)

class rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.PseudoInverseIK(serial_chain: pytorch_kinematics.chain.SerialChain, pos_tolerance: float = 0.001, rot_tolerance: float = 0.01, retry_configs: Optional[torch.Tensor] = None, num_retries: Optional[int] = None, joint_limits: Optional[torch.Tensor] = None, config_sampling_method: Union[str, Callable[[int], torch.Tensor]] = 'uniform', max_iterations: int = 50, lr: float = 0.2, line_search: Optional[rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.LineSearch] = None, regularlization: float = 1e-09, debug=False, early_stopping_any_converged=False, early_stopping_no_improvement='any', early_stopping_no_improvement_patience=2, optimizer_method: Union[str, Type[torch.optim.Optimizer]] = 'sgd')

Bases: rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.InverseKinematics

compute_dq(J, dx)

solve(target_poses: pytorch_kinematics.transforms.Transform3d) → rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.IKSolution

class rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.PseudoInverseIKWithSVD(serial_chain: pytorch_kinematics.chain.SerialChain, pos_tolerance: float = 0.001, rot_tolerance: float = 0.01, retry_configs: Optional[torch.Tensor] = None, num_retries: Optional[int] = None, joint_limits: Optional[torch.Tensor] = None, config_sampling_method: Union[str, Callable[[int], torch.Tensor]] = 'uniform', max_iterations: int = 50, lr: float = 0.2, line_search: Optional[rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.LineSearch] = None, regularlization: float = 1e-09, debug=False, early_stopping_any_converged=False, early_stopping_no_improvement='any', early_stopping_no_improvement_patience=2, optimizer_method: Union[str, Type[torch.optim.Optimizer]] = 'sgd')

Bases: rofunc.utils.robolab.kinematics.pytorch_kinematics_utils.PseudoInverseIK

compute_dq(J, dx)