Coverage for src/pyroboplan/ik/nullspace_components.py: 100%
31 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-15 22:28 -0400
1"""Library of common nullspace components for inverse kinematics."""
3import numpy as np
4import pinocchio
6from pyroboplan.core.utils import calculate_collision_vector_and_jacobians
9def zero_nullspace_component(model, q):
10 """
11 Returns a zero nullspace component, which is effectively a no-op.
13 Parameters
14 ----------
15 model : `pinocchio.Model`
16 The model from which to generate a random state.
17 q : array-like
18 The joint configuration for the model. Not used, but required to match the function interface.
20 Returns
21 -------
22 array-like
23 An array of zeros whose length is the number of joint variables in the model.
24 """
25 return np.zeros_like(model.lowerPositionLimit)
28def joint_limit_nullspace_component(model, q, gain=1.0, padding=0.0):
29 """
30 Returns a joint limits avoidance nullspace component.
32 Parameters
33 ----------
34 model : `pinocchio.Model`
35 The model from which to generate a random state.
36 q : array-like
37 The joint configuration for the model.
38 gain : float, optional
39 A gain to modify the relative weight of this term.
40 padding : float, optional
41 Optional padding around the joint limits.
43 Returns
44 -------
45 array-like
46 An array containing the joint space avoidance nullspace terms.
47 """
48 upper_limits = model.upperPositionLimit - padding
49 lower_limits = model.lowerPositionLimit + padding
51 grad = zero_nullspace_component(model, q)
52 for idx in range(len(grad)):
53 if q[idx] > upper_limits[idx]:
54 grad[idx] = -gain * (q[idx] - upper_limits[idx])
55 elif q[idx] < lower_limits[idx]:
56 grad[idx] = -gain * (q[idx] - lower_limits[idx])
57 return grad
60def joint_center_nullspace_component(model, q, gain=1.0):
61 """
62 Returns a joint centering nullspace component.
64 Parameters
65 ----------
66 model : `pinocchio.Model`
67 The model from which to generate a random state.
68 q : array-like
69 The joint configuration for the model.
70 gain : float, optional
71 A gain to modify the relative weight of this term.
73 Returns
74 -------
75 array-like
76 An array containing the joint centering nullspace terms.
77 """
78 joint_center_positions = 0.5 * (model.lowerPositionLimit + model.upperPositionLimit)
79 return gain * (joint_center_positions - q)
82def collision_avoidance_nullspace_component(
83 model,
84 data,
85 collision_model,
86 collision_data,
87 q,
88 dist_padding=0.05,
89 gain=1.0,
90):
91 """
92 Returns a collision avoidance nullspace component.
94 These calculations are based off the following resources:
95 * https://typeset.io/pdf/a-collision-free-mpc-for-whole-body-dynamic-locomotion-and-1l6itpfk.pdf
96 * https://laas.hal.science/hal-04425002
98 Parameters
99 ----------
100 model : `pinocchio.Model`
101 The model from which to generate a random state.
102 data : `pinocchio.Data`
103 The model data to use for collision distance checks.
104 collision_model : `pinocchio.GeometryModel`
105 The model with which to check collision distances.
106 collision_data : `pinocchio.GeometryData`
107 The collision model data to use for collision distance checks.
108 q : array-like
109 The joint configuration for the model.
110 dist_padding : float
111 The distance padding, in meters, on the collision distances.
112 For example, a distance padding of 0.1 means collisions have an influence 10 cm away from actual collision..
113 gain : float, optional
114 A gain to modify the relative weight of this term.
116 Returns
117 -------
118 array-like
119 An array containing the collision avoidance nullspace terms.
120 """
121 coll_component = np.zeros_like(model.lowerPositionLimit)
123 # Find all the collision distances at the current state.
124 pinocchio.framesForwardKinematics(model, data, q)
125 pinocchio.computeCollisions(model, data, collision_model, collision_data, q, False)
126 pinocchio.computeDistances(model, data, collision_model, collision_data, q)
128 # For each collision pair within a distance threshold, calculate its collision Jacobians
129 # and use them to push the corresponding joint values away from collision.
130 for idx in range(len(collision_model.collisionPairs)):
131 dist = collision_data.distanceResults[idx].min_distance
132 if dist > dist_padding:
133 continue
135 distance_vec, Jcoll1, Jcoll2 = calculate_collision_vector_and_jacobians(
136 model, collision_model, data, collision_data, idx, q
137 )
138 coll_component -= (distance_vec - dist_padding) @ (Jcoll2 - Jcoll1)
140 coll_component = gain * coll_component
142 return coll_component