rapier3d/dynamics/joint/multibody_joint/
multibody_ik.rs

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

rapier3d/dynamics/joint/multibody_joint/
multibody_ik.rs
