control_utils
Set of utilities for helping to execute robot control
IKSolver
Class for thinly wrapping Lula IK solver
Source code in omnigibson/utils/control_utils.py
solve(target_pos, target_quat=None, tolerance_pos=0.002, tolerance_quat=0.01, weight_pos=1.0, weight_quat=0.05, max_iterations=150, initial_joint_pos=None)
Backs out joint positions to achieve desired @target_pos and @target_quat
Parameters:
Name | Type | Description | Default |
---|---|---|---|
target_pos |
3-array
|
desired (x,y,z) local target cartesian position in robot's base coordinate frame |
required |
target_quat |
4-array or None
|
If specified, desired (x,y,z,w) local target quaternion orientation in |
None
|
tolerance_pos |
float
|
Maximum position error (L2-norm) for a successful IK solution |
0.002
|
tolerance_quat |
float
|
Maximum orientation error (per-axis L2-norm) for a successful IK solution |
0.01
|
weight_pos |
float
|
Weight for the relative importance of position error during CCD |
1.0
|
weight_quat |
float
|
Weight for the relative importance of position error during CCD |
0.05
|
max_iterations |
int
|
Number of iterations used for each cyclic coordinate descent. |
150
|
initial_joint_pos |
None or n-array
|
If specified, will set the initial cspace seed when solving for joint positions. Otherwise, will use self.default_joint_pos |
None
|
Returns:
Type | Description |
---|---|
None or n-array: Joint positions for reaching desired target_pos and target_quat, otherwise None if no solution was found |