1
2
3
4
5
6
7
8
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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#![allow(missing_docs)]
use downcast_rs::Downcast;
use na::{DVectorSliceMut, Real};
use crate::object::{Multibody, MultibodyLink};
use crate::math::{Isometry, JacobianSliceMut, Vector, Velocity};
use crate::solver::{ConstraintSet, GenericNonlinearConstraint, IntegrationParameters};
pub trait Joint<N: Real>: Downcast + Send + Sync {
fn ndofs(&self) -> usize;
fn body_to_parent(&self, parent_shift: &Vector<N>, body_shift: &Vector<N>) -> Isometry<N>;
fn update_jacobians(&mut self, body_shift: &Vector<N>, vels: &[N]);
fn integrate(&mut self, params: &IntegrationParameters<N>, vels: &[N]);
fn apply_displacement(&mut self, disp: &[N]);
fn jacobian(&self, transform: &Isometry<N>, out: &mut JacobianSliceMut<N>);
fn jacobian_dot(&self, transform: &Isometry<N>, out: &mut JacobianSliceMut<N>);
fn jacobian_dot_veldiff_mul_coordinates(
&self,
transform: &Isometry<N>,
vels: &[N],
out: &mut JacobianSliceMut<N>,
);
fn jacobian_mul_coordinates(&self, vels: &[N]) -> Velocity<N>;
fn jacobian_dot_mul_coordinates(&self, vels: &[N]) -> Velocity<N>;
fn default_damping(&self, out: &mut DVectorSliceMut<N>);
fn nimpulses(&self) -> usize {
self.ndofs() * 3
}
fn num_velocity_constraints(&self) -> usize {
0
}
fn velocity_constraints(
&self,
_params: &IntegrationParameters<N>,
_multibody: &Multibody<N>,
_link: &MultibodyLink<N>,
_assembly_id: usize,
_dof_id: usize,
_ext_vels: &[N],
_ground_j_id: &mut usize,
_jacobians: &mut [N],
_velocity_constraints: &mut ConstraintSet<N>,
) {}
fn num_position_constraints(&self) -> usize {
0
}
fn position_constraint(
&self,
_i: usize,
_multibody: &Multibody<N>,
_link: &MultibodyLink<N>,
_dof_id: usize,
_jacobians: &mut [N],
) -> Option<GenericNonlinearConstraint<N>> {
None
}
fn clone(&self) -> Box<Joint<N>>;
}
impl_downcast!(Joint<N> where N: Real);