rapier3d/dynamics/joint/multibody_joint/
multibody_ik.rs

1use crate::dynamics::{JointAxesMask, Multibody, MultibodyLink, RigidBodySet};
2use crate::math::{ANG_DIM, DIM, Isometry, Jacobian, Real, SPATIAL_DIM};
3use na::{self, DVector, SMatrix};
4use parry::math::SpacialVector;
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: &SpacialVector<Real>,
51        damping: Real,
52        displacements: &mut DVector<Real>,
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: &SpacialVector<Real>,
71        damping: Real,
72        displacements: &mut DVector<Real>,
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: &Isometry<Real>,
99        joint_can_move: impl Fn(&MultibodyLink) -> bool,
100        displacements: &mut DVector<Real>,
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.vector - pose.translation.vector;
128            let delta_ang = (target_pose.rotation * pose.rotation.inverse()).scaled_axis();
129
130            #[cfg(feature = "dim2")]
131            let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang.x];
132            #[cfg(feature = "dim3")]
133            let mut delta = na::vector![
134                delta_lin.x,
135                delta_lin.y,
136                delta_lin.z,
137                delta_ang.x,
138                delta_ang.y,
139                delta_ang.z
140            ];
141
142            if !options.constrained_axes.contains(JointAxesMask::LIN_X) {
143                delta[0] = 0.0;
144            }
145            if !options.constrained_axes.contains(JointAxesMask::LIN_Y) {
146                delta[1] = 0.0;
147            }
148            #[cfg(feature = "dim3")]
149            if !options.constrained_axes.contains(JointAxesMask::LIN_Z) {
150                delta[2] = 0.0;
151            }
152            if !options.constrained_axes.contains(JointAxesMask::ANG_X) {
153                delta[DIM] = 0.0;
154            }
155            #[cfg(feature = "dim3")]
156            if !options.constrained_axes.contains(JointAxesMask::ANG_Y) {
157                delta[DIM + 1] = 0.0;
158            }
159            #[cfg(feature = "dim3")]
160            if !options.constrained_axes.contains(JointAxesMask::ANG_Z) {
161                delta[DIM + 2] = 0.0;
162            }
163
164            // TODO: measure convergence on the error variation instead?
165            if delta.rows(0, DIM).norm() <= options.epsilon_linear
166                && delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular
167            {
168                break;
169            }
170
171            Self::inverse_kinematics_delta_with_jacobian(
172                &jacobian,
173                &delta,
174                options.damping,
175                displacements,
176            );
177        }
178    }
179}
180
181#[cfg(test)]
182mod test {
183    use crate::dynamics::{
184        MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder,
185        RigidBodySet,
186    };
187    use crate::math::{Jacobian, Real, Vector};
188    use approx::assert_relative_eq;
189
190    #[test]
191    fn one_link_fwd_kinematics() {
192        let mut bodies = RigidBodySet::new();
193        let mut multibodies = MultibodyJointSet::new();
194
195        let num_segments = 10;
196        let body = RigidBodyBuilder::fixed();
197        let mut last_body = bodies.insert(body);
198        let mut last_link = MultibodyJointHandle::invalid();
199
200        for _ in 0..num_segments {
201            let body = RigidBodyBuilder::dynamic().can_sleep(false);
202            let new_body = bodies.insert(body);
203
204            #[cfg(feature = "dim2")]
205            let builder = RevoluteJointBuilder::new();
206            #[cfg(feature = "dim3")]
207            let builder = RevoluteJointBuilder::new(Vector::z_axis());
208            let link_ab = builder
209                .local_anchor1((Vector::y() * (0.5 / num_segments as Real)).into())
210                .local_anchor2((Vector::y() * (-0.5 / num_segments as Real)).into());
211            last_link = multibodies
212                .insert(last_body, new_body, link_ab, true)
213                .unwrap();
214
215            last_body = new_body;
216        }
217
218        let (multibody, last_id) = multibodies.get_mut(last_link).unwrap();
219        multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date.
220        assert_eq!(multibody.ndofs(), num_segments);
221
222        /*
223         * No displacement.
224         */
225        let mut jacobian2 = Jacobian::zeros(0);
226        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
227        let jacobian1 = multibody.body_jacobian(last_id);
228        let link_pose2 =
229            multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2));
230        assert_eq!(link_pose1, link_pose2);
231        assert_eq!(jacobian1, &jacobian2);
232
233        /*
234         * Arbitrary displacement.
235         */
236        let niter = 100;
237        let displacement_part: Vec<_> = (0..multibody.ndofs())
238            .map(|i| i as Real * -0.1 / niter as Real)
239            .collect();
240        let displacement_total: Vec<_> = displacement_part
241            .iter()
242            .map(|d| *d * niter as Real)
243            .collect();
244        let link_pose2 = multibody.forward_kinematics_single_link(
245            &bodies,
246            last_id,
247            Some(&displacement_total),
248            Some(&mut jacobian2),
249        );
250
251        for _ in 0..niter {
252            multibody.apply_displacements(&displacement_part);
253            multibody.forward_kinematics(&bodies, false);
254        }
255
256        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
257        let jacobian1 = multibody.body_jacobian(last_id);
258        assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5);
259        assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5);
260    }
261}
rapier3d/dynamics/joint/multibody_joint/multibody_ik.rs

rapier3d/dynamics/joint/multibody_joint/
multibody_ik.rs
