rapier2d/dynamics/joint/multibody_joint/
multibody_ik.rs

1use crate::dynamics::{JointAxesMask, Multibody, MultibodyLink, RigidBodySet};
2use crate::math::{ANG_DIM, DIM, DVector, Jacobian, Pose, Real, SPATIAL_DIM};
3use na::{self, SMatrix, SVector};
4
5#[derive(Copy, Clone, Debug, PartialEq)]
6/// Options for the jacobian-based Inverse Kinematics solver for multibodies.
7pub struct InverseKinematicsOption {
8    /// A damping coefficient.
9    ///
10    /// Small value can lead to overshooting preventing convergence. Large
11    /// values can slow down convergence, requiring more iterations to converge.
12    pub damping: Real,
13    /// The maximum number of iterations the iterative IK solver can take.
14    pub max_iters: usize,
15    /// The axes the IK solver will solve for.
16    pub constrained_axes: JointAxesMask,
17    /// The error threshold on the linear error.
18    ///
19    /// If errors on both linear and angular parts fall below this
20    /// threshold, the iterative resolution will stop.
21    pub epsilon_linear: Real,
22    /// The error threshold on the angular error.
23    ///
24    /// If errors on both linear and angular parts fall below this
25    /// threshold, the iterative resolution will stop.
26    pub epsilon_angular: Real,
27}
28
29impl Default for InverseKinematicsOption {
30    fn default() -> Self {
31        Self {
32            damping: 1.0,
33            max_iters: 10,
34            constrained_axes: JointAxesMask::all(),
35            epsilon_linear: 1.0e-3,
36            epsilon_angular: 1.0e-3,
37        }
38    }
39}
40
41impl Multibody {
42    /// Computes the displacement needed to have the link identified by `link_id` move by the
43    /// desired transform.
44    ///
45    /// The displacement calculated by this function is added to the `displacement` vector.
46    pub fn inverse_kinematics_delta(
47        &self,
48        link_id: usize,
49        desired_movement: &SVector<Real, SPATIAL_DIM>,
50        damping: Real,
51        displacements: &mut DVector,
52    ) {
53        let body_jacobian = self.body_jacobian(link_id);
54        Self::inverse_kinematics_delta_with_jacobian(
55            body_jacobian,
56            desired_movement,
57            damping,
58            displacements,
59        );
60    }
61
62    /// Computes the displacement needed to have a link with the given jacobian move by the
63    /// desired transform.
64    ///
65    /// The displacement calculated by this function is added to the `displacement` vector.
66    #[profiling::function]
67    pub fn inverse_kinematics_delta_with_jacobian(
68        jacobian: &Jacobian<Real>,
69        desired_movement: &SVector<Real, SPATIAL_DIM>,
70        damping: Real,
71        displacements: &mut DVector,
72    ) {
73        let identity = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::identity();
74        let jj = jacobian * &jacobian.transpose() + identity * (damping * damping);
75        let inv_jj = jj.pseudo_inverse(1.0e-5).unwrap_or(identity);
76        displacements.gemv_tr(1.0, jacobian, &(inv_jj * desired_movement), 1.0);
77    }
78
79    /// Computes the displacement needed to have the link identified by `link_id` have a pose
80    /// equal (or as close as possible) to `target_pose`.
81    ///
82    /// If `displacement` is given non-zero, the current pose of the rigid-body is considered to be
83    /// obtained from its current generalized coordinates summed with the `displacement` vector.
84    ///
85    /// The `displacements` vector is overwritten with the new displacement.
86    ///
87    /// The `joint_can_move` argument is a closure that lets you indicate which joint
88    /// can be moved through the inverse-kinematics process. Any joint for which `joint_can_move`
89    /// returns `false` will have its corresponding displacement constrained to 0.
90    /// Set the closure to `|_| true` if all the joints are free to move.
91    #[profiling::function]
92    pub fn inverse_kinematics(
93        &self,
94        bodies: &RigidBodySet,
95        link_id: usize,
96        options: &InverseKinematicsOption,
97        target_pose: &Pose,
98        joint_can_move: impl Fn(&MultibodyLink) -> bool,
99        displacements: &mut DVector,
100    ) {
101        let mut jacobian = Jacobian::zeros(0);
102        let branch = self.kinematic_branch(link_id);
103        let can_move: Vec<_> = branch
104            .iter()
105            .map(|id| joint_can_move(&self.links[*id]))
106            .collect();
107
108        for _ in 0..options.max_iters {
109            let pose = self.forward_kinematics_single_branch(
110                bodies,
111                &branch,
112                Some(displacements.as_slice()),
113                Some(&mut jacobian),
114            );
115
116            // Adjust the jacobian to account for non-movable joints.
117            for (id, can_move) in branch.iter().zip(can_move.iter()) {
118                if !*can_move {
119                    let link = &self.links[*id];
120                    jacobian
121                        .columns_mut(link.assembly_id, link.joint.ndofs())
122                        .fill(0.0);
123                }
124            }
125
126            let delta_lin = target_pose.translation - pose.translation;
127            #[cfg(feature = "dim2")]
128            let delta_ang = (target_pose.rotation * pose.rotation.inverse()).angle();
129            #[cfg(feature = "dim3")]
130            let delta_ang = (target_pose.rotation * pose.rotation.inverse()).to_scaled_axis();
131
132            #[cfg(feature = "dim2")]
133            let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang];
134            #[cfg(feature = "dim3")]
135            let mut delta = na::vector![
136                delta_lin.x,
137                delta_lin.y,
138                delta_lin.z,
139                delta_ang.x,
140                delta_ang.y,
141                delta_ang.z
142            ];
143
144            if !options.constrained_axes.contains(JointAxesMask::LIN_X) {
145                delta[0] = 0.0;
146            }
147            if !options.constrained_axes.contains(JointAxesMask::LIN_Y) {
148                delta[1] = 0.0;
149            }
150            #[cfg(feature = "dim3")]
151            if !options.constrained_axes.contains(JointAxesMask::LIN_Z) {
152                delta[2] = 0.0;
153            }
154            if !options.constrained_axes.contains(JointAxesMask::ANG_X) {
155                delta[DIM] = 0.0;
156            }
157            #[cfg(feature = "dim3")]
158            if !options.constrained_axes.contains(JointAxesMask::ANG_Y) {
159                delta[DIM + 1] = 0.0;
160            }
161            #[cfg(feature = "dim3")]
162            if !options.constrained_axes.contains(JointAxesMask::ANG_Z) {
163                delta[DIM + 2] = 0.0;
164            }
165
166            // TODO: measure convergence on the error variation instead?
167            if delta.rows(0, DIM).norm() <= options.epsilon_linear
168                && delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular
169            {
170                break;
171            }
172
173            Self::inverse_kinematics_delta_with_jacobian(
174                &jacobian,
175                &delta,
176                options.damping,
177                displacements,
178            );
179        }
180    }
181}
182
183#[cfg(test)]
184mod test {
185    use crate::dynamics::{
186        MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder,
187        RigidBodySet,
188    };
189    use crate::math::{Jacobian, Real, Vector};
190    use approx::assert_relative_eq;
191
192    #[test]
193    fn one_link_fwd_kinematics() {
194        let mut bodies = RigidBodySet::new();
195        let mut multibodies = MultibodyJointSet::new();
196
197        let num_segments = 10;
198        let body = RigidBodyBuilder::fixed();
199        let mut last_body = bodies.insert(body);
200        let mut last_link = MultibodyJointHandle::invalid();
201
202        for _ in 0..num_segments {
203            let body = RigidBodyBuilder::dynamic().can_sleep(false);
204            let new_body = bodies.insert(body);
205
206            #[cfg(feature = "dim2")]
207            let builder = RevoluteJointBuilder::new();
208            #[cfg(feature = "dim3")]
209            let builder = RevoluteJointBuilder::new(Vector::Z);
210            let link_ab = builder
211                .local_anchor1((Vector::Y * (0.5 / num_segments as Real)).into())
212                .local_anchor2((Vector::Y * (-0.5 / num_segments as Real)).into());
213            last_link = multibodies
214                .insert(last_body, new_body, link_ab, true)
215                .unwrap();
216
217            last_body = new_body;
218        }
219
220        let (multibody, last_id) = multibodies.get_mut(last_link).unwrap();
221        multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date.
222        assert_eq!(multibody.ndofs(), num_segments);
223
224        /*
225         * No displacement.
226         */
227        let mut jacobian2 = Jacobian::zeros(0);
228        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
229        let jacobian1 = multibody.body_jacobian(last_id);
230        let link_pose2 =
231            multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2));
232        assert_eq!(link_pose1, link_pose2);
233        assert_eq!(jacobian1, &jacobian2);
234
235        /*
236         * Arbitrary displacement.
237         */
238        let niter = 100;
239        let displacement_part: Vec<_> = (0..multibody.ndofs())
240            .map(|i| i as Real * -0.1 / niter as Real)
241            .collect();
242        let displacement_total: Vec<_> = displacement_part
243            .iter()
244            .map(|d| *d * niter as Real)
245            .collect();
246        let link_pose2 = multibody.forward_kinematics_single_link(
247            &bodies,
248            last_id,
249            Some(&displacement_total),
250            Some(&mut jacobian2),
251        );
252
253        for _ in 0..niter {
254            multibody.apply_displacements(&displacement_part);
255            multibody.forward_kinematics(&bodies, false);
256        }
257
258        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
259        let jacobian1 = multibody.body_jacobian(last_id);
260        assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5);
261        assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5);
262    }
263}
rapier2d/dynamics/joint/multibody_joint/multibody_ik.rs

rapier2d/dynamics/joint/multibody_joint/
multibody_ik.rs
