Skip to content

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 
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
class IKSolver:
    """
    Class for thinly wrapping Lula IK solver
    """

    def __init__(
        self,
        robot_description_path,
        robot_urdf_path,
        eef_name,
        default_joint_pos,
    ):
        # Create robot description, kinematics, and config
        self.robot_description = lula.load_robot(robot_description_path, robot_urdf_path)
        self.kinematics = self.robot_description.kinematics()
        self.config = lula.CyclicCoordDescentIkConfig()
        self.eef_name = eef_name
        self.default_joint_pos = default_joint_pos

    def solve(
        self,
        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

        Args:
            target_pos (3-array): desired (x,y,z) local target cartesian position in robot's base coordinate frame
            target_quat (4-array or None): If specified, desired (x,y,z,w) local target quaternion orientation in
            robot's base coordinate frame. If None, IK will be position-only (will override settings such that
            orientation's tolerance is very high and weight is 0)
            tolerance_pos (float): Maximum position error (L2-norm) for a successful IK solution
            tolerance_quat (float): Maximum orientation error (per-axis L2-norm) for a successful IK solution
            weight_pos (float): Weight for the relative importance of position error during CCD
            weight_quat (float): Weight for the relative importance of position error during CCD
            max_iterations (int): Number of iterations used for each cyclic coordinate descent.
            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

        Returns:
            None or n-array: Joint positions for reaching desired target_pos and target_quat, otherwise None if no
                solution was found
        """
        pos = np.array(target_pos, dtype=np.float64).reshape(3, 1)
        rot = np.array(T.quat2mat(np.array([0, 0, 0, 1.0]) if target_quat is None else target_quat), dtype=np.float64)
        ik_target_pose = lula.Pose3(lula.Rotation3(rot), pos)

        # Set the cspace seed and tolerance
        initial_joint_pos = self.default_joint_pos if initial_joint_pos is None else np.array(initial_joint_pos)
        self.config.cspace_seeds = [initial_joint_pos]
        self.config.position_tolerance = tolerance_pos
        self.config.orientation_tolerance = 100.0 if target_quat is None else tolerance_quat
        self.config.position_weight = weight_pos
        self.config.orientation_weight = 0.0 if target_quat is None else weight_quat
        self.config.max_iterations_per_descent = max_iterations

        # Compute target joint positions
        ik_results = lula.compute_ik_ccd(self.kinematics, ik_target_pose, self.eef_name, self.config)
        return np.array(ik_results.cspace_position)

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
Source code in omnigibson/utils/control_utils.py 
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
def solve(
    self,
    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

    Args:
        target_pos (3-array): desired (x,y,z) local target cartesian position in robot's base coordinate frame
        target_quat (4-array or None): If specified, desired (x,y,z,w) local target quaternion orientation in
        robot's base coordinate frame. If None, IK will be position-only (will override settings such that
        orientation's tolerance is very high and weight is 0)
        tolerance_pos (float): Maximum position error (L2-norm) for a successful IK solution
        tolerance_quat (float): Maximum orientation error (per-axis L2-norm) for a successful IK solution
        weight_pos (float): Weight for the relative importance of position error during CCD
        weight_quat (float): Weight for the relative importance of position error during CCD
        max_iterations (int): Number of iterations used for each cyclic coordinate descent.
        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

    Returns:
        None or n-array: Joint positions for reaching desired target_pos and target_quat, otherwise None if no
            solution was found
    """
    pos = np.array(target_pos, dtype=np.float64).reshape(3, 1)
    rot = np.array(T.quat2mat(np.array([0, 0, 0, 1.0]) if target_quat is None else target_quat), dtype=np.float64)
    ik_target_pose = lula.Pose3(lula.Rotation3(rot), pos)

    # Set the cspace seed and tolerance
    initial_joint_pos = self.default_joint_pos if initial_joint_pos is None else np.array(initial_joint_pos)
    self.config.cspace_seeds = [initial_joint_pos]
    self.config.position_tolerance = tolerance_pos
    self.config.orientation_tolerance = 100.0 if target_quat is None else tolerance_quat
    self.config.position_weight = weight_pos
    self.config.orientation_weight = 0.0 if target_quat is None else weight_quat
    self.config.max_iterations_per_descent = max_iterations

    # Compute target joint positions
    ik_results = lula.compute_ik_ccd(self.kinematics, ik_target_pose, self.eef_name, self.config)
    return np.array(ik_results.cspace_position)