nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Address issues with the genral-case for multibody joints

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-08 21:09:11 +01:00
parent 9726738cd2
commit 87ec0ced40
7 changed files with 261 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files

274
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -9,7 +9,7 @@ use crate::dynamics::{
#[cfg(feature = "dim3")]
use crate::math::Matrix;
use crate::math::{
AngDim, AngVector, Dim, Isometry, Jacobian, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
};
use crate::prelude::MultibodyJoint;
use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
@@ -66,7 +66,7 @@ pub struct Multibody {
pub(crate) accelerations: DVector<Real>,
body_jacobians: Vec<Jacobian<Real>>,
// FIXME: use sparse matrices.
// TODO: use sparse matrices?
augmented_mass: DMatrix<Real>,
inv_augmented_mass: LU<Real, Dynamic, Dynamic>,
@@ -141,7 +141,7 @@ impl Multibody {
if is_new_root {
let joint = MultibodyJoint::fixed(*link.local_to_world());
link.state.joint = joint;
link.joint = joint;
}
curr_mb.ndofs += link.joint().ndofs();
@@ -178,7 +178,7 @@ impl Multibody {
}
pub fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) {
let rhs_root_ndofs = rhs.links[0].state.joint.ndofs();
let rhs_root_ndofs = rhs.links[0].joint.ndofs();
let rhs_copy_shift = self.ndofs + rhs_root_ndofs;
let rhs_copy_ndofs = rhs.ndofs - rhs_root_ndofs;
@@ -195,17 +195,14 @@ impl Multibody {
// Adjust the first link.
{
rhs.links[0].state.joint = joint;
rhs.links[0].joint = joint;
rhs.links[0].assembly_id = self.velocities.len();
rhs.links[0].internal_id = self.links.len();
rhs.links[0].parent_internal_id = parent;
}
// Grow buffers and append data from rhs.
self.grow_buffers(
rhs_copy_ndofs + rhs.links[0].state.joint.ndofs(),
rhs.links.len(),
);
self.grow_buffers(rhs_copy_ndofs + rhs.links[0].joint.ndofs(), rhs.links.len());
if rhs_copy_ndofs > 0 {
self.velocities
@@ -220,7 +217,6 @@ impl Multibody {
}
rhs.links[0]
.state
.joint
.default_damping(&mut self.damping.rows_mut(base_assembly_id, rhs_root_ndofs));
@@ -261,17 +257,6 @@ impl Multibody {
self.links.get_mut(id)
}
/// The links of this multibody with the given `name`.
pub fn links_with_name<'a>(
&'a self,
name: &'a str,
) -> impl Iterator<Item = (usize, &'a MultibodyLink)> {
self.links
.iter()
.enumerate()
.filter(move |(_i, l)| l.name == name)
}
/// The number of links on this multibody.
pub fn num_links(&self) -> usize {
self.links.len()
@@ -306,7 +291,7 @@ impl Multibody {
pub fn add_link(
&mut self,
parent: Option<usize>, // FIXME: should be a RigidBodyHandle?
mut dof: MultibodyJoint,
dof: MultibodyJoint,
body: RigidBodyHandle,
) -> &mut MultibodyLink {
assert!(
@@ -335,21 +320,16 @@ impl Multibody {
/*
* Create the multibody.
*/
dof.update_jacobians(&self.velocities.as_slice()[assembly_id..]);
let local_to_parent = dof.body_to_parent();
let local_to_world;
let parent_to_world;
let parent_internal_id;
if let Some(parent) = parent {
parent_internal_id = parent;
let parent_link = &mut self.links[parent_internal_id];
parent_link.is_leaf = false;
parent_to_world = parent_link.state.local_to_world;
local_to_world = parent_link.state.local_to_world * local_to_parent;
local_to_world = parent_link.local_to_world * local_to_parent;
} else {
parent_internal_id = 0;
parent_to_world = Isometry::identity();
local_to_world = local_to_parent;
}
@@ -359,7 +339,6 @@ impl Multibody {
assembly_id,
parent_internal_id,
dof,
parent_to_world,
local_to_world,
local_to_parent,
);
@@ -400,29 +379,31 @@ impl Multibody {
&RigidBodyForces,
) = bodies.index_bundle(link.rigid_body.0);
let mut acc = link.velocity_dot_wrt_joint;
let mut acc = RigidBodyVelocity::zero();
if i != 0 {
let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_id];
let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(parent_link.rigid_body.0);
let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0);
acc += self.workspace.accs[parent_id];
acc.linvel += parent_rb_vels.angvel.gcross(link.velocity_wrt_joint.linvel);
// The 2.0 originates from the two identical terms of Jdot (the terms become
// identical once they are multiplied by the generalized velocities).
acc.linvel += 2.0 * parent_rb_vels.angvel.gcross(link.joint_velocity.linvel);
#[cfg(feature = "dim3")]
{
acc.angvel += parent_rb_vels.angvel.cross(&link.velocity_wrt_joint.angvel);
acc.angvel += parent_rb_vels.angvel.cross(&link.joint_velocity.angvel);
}
let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
let dvel = rb_vels.linvel - parent_rb_vels.linvel;
acc.linvel += parent_rb_vels.angvel.gcross(dvel);
acc.linvel += self.workspace.accs[parent_id].angvel.gcross(shift);
acc.linvel += parent_rb_vels
.angvel
.gcross(parent_rb_vels.angvel.gcross(link.shift02));
acc.linvel += self.workspace.accs[parent_id].angvel.gcross(link.shift02);
}
acc.linvel += rb_vels.angvel.gcross(rb_vels.angvel.gcross(link.shift23));
acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
self.workspace.accs[i] = acc;
// TODO: should gyroscopic forces already be computed by the rigid-body itself
@@ -469,18 +450,12 @@ impl Multibody {
* NOTE: this is needed for kinematic bodies too.
*/
let link = &mut self.links[0];
let velocity_wrt_joint = link
.state
let joint_velocity = link
.joint
.jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
let velocity_dot_wrt_joint = link
.state
.joint
.jacobian_dot_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
link.velocity_dot_wrt_joint = velocity_dot_wrt_joint;
link.velocity_wrt_joint = velocity_wrt_joint;
bodies.set_internal(link.rigid_body.0, link.velocity_wrt_joint);
link.joint_velocity = joint_velocity;
bodies.set_internal(link.rigid_body.0, link.joint_velocity);
for i in 1..self.links.len() {
let (link, parent_link) = self.links.get_mut_with_parent(i);
@@ -488,22 +463,16 @@ impl Multibody {
let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(parent_link.rigid_body.0);
let velocity_wrt_joint = link
.state
let joint_velocity = link
.joint
.jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
let velocity_dot_wrt_joint = link
.state
.joint
.jacobian_dot_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
link.velocity_dot_wrt_joint =
velocity_dot_wrt_joint.transformed(&parent_link.state.local_to_world);
link.velocity_wrt_joint =
velocity_wrt_joint.transformed(&parent_link.state.local_to_world);
let mut new_rb_vels = *parent_rb_vels + link.velocity_wrt_joint;
link.joint_velocity = joint_velocity.transformed(
&(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation),
);
let mut new_rb_vels = *parent_rb_vels + link.joint_velocity;
let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift);
new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23);
bodies.set_internal(link.rigid_body.0, new_rb_vels);
}
@@ -514,23 +483,21 @@ impl Multibody {
self.update_inertias(dt, bodies);
}
fn update_body_next_jacobians<Bodies>(&mut self, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps>,
{
fn update_body_jacobians(&mut self) {
for i in 0..self.links.len() {
let link = &self.links[i];
let rb_mprops = bodies.index(link.rigid_body.0);
if self.body_jacobians[i].ncols() != self.ndofs {
// FIXME: use a resize instead.
self.body_jacobians[i] = Jacobian::zeros(self.ndofs);
}
let parent_to_world;
if i != 0 {
let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_id];
let parent_rb_mprops = bodies.index(parent_link.rigid_body.0);
parent_to_world = parent_link.local_to_world;
let (link_j, parent_j) = self.body_jacobians.index_mut_const(i, parent_id);
link_j.copy_from(&parent_j);
@@ -539,25 +506,31 @@ impl Multibody {
let mut link_j_v = link_j.fixed_rows_mut::<DIM>(0);
let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
let shift_tr = (link.state.local_to_world * rb_mprops.local_mprops.local_com
- parent_link.state.local_to_world
* parent_rb_mprops.local_mprops.local_com)
.gcross_matrix_tr();
let shift_tr = (link.shift02).gcross_matrix_tr();
link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
}
} else {
self.body_jacobians[i].fill(0.0);
parent_to_world = Isometry::identity();
}
let ndofs = link.state.joint.ndofs();
let ndofs = link.joint.ndofs();
let mut tmp = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
let mut link_joint_j = tmp.columns_mut(0, ndofs);
let mut link_j_part = self.body_jacobians[i].columns_mut(link.assembly_id, ndofs);
link.state
.joint
.jacobian(&link.state.parent_to_world, &mut link_joint_j);
link.joint.jacobian(
&(parent_to_world.rotation * link.joint.data.local_frame1.rotation),
&mut link_joint_j,
);
link_j_part += link_joint_j;
{
let link_j = &mut self.body_jacobians[i];
let (mut link_j_v, link_j_w) =
link_j.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
let shift_tr = link.shift23.gcross_matrix_tr();
link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
}
}
}
@@ -626,64 +599,63 @@ impl Multibody {
*
*/
let rb_j = &self.body_jacobians[i];
let rb_j_v = rb_j.fixed_rows::<DIM>(0);
let rb_j_w = rb_j.fixed_rows::<ANG_DIM>(DIM);
let ndofs = link.state.joint.ndofs();
let ndofs = link.joint.ndofs();
if i != 0 {
let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_id];
let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(parent_link.rigid_body.0);
let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0);
let parent_j = &self.body_jacobians[parent_id];
let parent_j_v = parent_j.fixed_rows::<DIM>(0);
let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
let parent_w = parent_rb_vels.angvel.gcross_matrix();
let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id);
let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id);
// JDot + JDot/u * qdot
coriolis_v.copy_from(&parent_coriolis_v);
coriolis_w.copy_from(&parent_coriolis_w);
let shift_cross =
(rb_mprops.world_com - parent_rb_mprops.world_com).gcross_matrix_tr();
coriolis_v.gemm(1.0, &shift_cross, &parent_coriolis_w, 1.0);
// [c1 - c0].gcross() * (JDot + JDot/u * qdot)"
let shift_cross_tr = link.shift02.gcross_matrix_tr();
coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0);
// JDot
let dvel_cross = (rb_vels.linvel - parent_rb_vels.linvel).gcross_matrix_tr();
// JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
let dvel_cross = (rb_vels.angvel.gcross(link.shift02)
+ 2.0 * link.joint_velocity.linvel)
.gcross_matrix_tr();
coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0);
// JDot/u * qdot
coriolis_v.gemm(
1.0,
&link.velocity_wrt_joint.linvel.gcross_matrix_tr(),
&link.joint_velocity.linvel.gcross_matrix_tr(),
&parent_j_w,
1.0,
);
coriolis_v.gemm(1.0, &parent_w, &rb_j_v, 1.0);
coriolis_v.gemm(-1.0, &parent_w, &parent_j_v, 1.0);
coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr), &parent_j_w, 1.0);
#[cfg(feature = "dim3")]
{
let vel_wrt_joint_w = link.velocity_wrt_joint.angvel.gcross_matrix();
let vel_wrt_joint_w = link.joint_velocity.angvel.gcross_matrix();
coriolis_w.gemm(-1.0, &vel_wrt_joint_w, &parent_j_w, 1.0);
}
// JDot
// JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
{
let mut coriolis_v_part = coriolis_v.columns_mut(link.assembly_id, ndofs);
let mut tmp1 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
let mut rb_joint_j = tmp1.columns_mut(0, ndofs);
link.state
.joint
.jacobian(&parent_link.state.local_to_world, &mut rb_joint_j);
link.joint.jacobian(
&(parent_link.local_to_world.rotation
* link.joint.data.local_frame1.rotation),
&mut rb_joint_j,
);
let rb_joint_j_v = rb_joint_j.fixed_rows::<DIM>(0);
coriolis_v_part.gemm(1.0, &parent_w, &rb_joint_j_v, 1.0);
coriolis_v_part.gemm(2.0, &parent_w, &rb_joint_j_v, 1.0);
#[cfg(feature = "dim3")]
{
@@ -701,37 +673,26 @@ impl Multibody {
let coriolis_w = &mut self.coriolis_w[i];
{
let mut tmp1 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
let mut tmp2 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
let mut rb_joint_j_dot = tmp1.columns_mut(0, ndofs);
let mut rb_joint_j_dot_veldiff = tmp2.columns_mut(0, ndofs);
link.state
.joint
.jacobian_dot(&link.state.parent_to_world, &mut rb_joint_j_dot);
link.state.joint.jacobian_dot_veldiff_mul_coordinates(
&link.state.parent_to_world,
&self.velocities.as_slice()[link.assembly_id..],
&mut rb_joint_j_dot_veldiff,
);
let rb_joint_j_v_dot = rb_joint_j_dot.fixed_rows::<DIM>(0);
let rb_joint_j_w_dot = rb_joint_j_dot.fixed_rows::<ANG_DIM>(DIM);
let rb_joint_j_v_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<DIM>(0);
let rb_joint_j_w_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<ANG_DIM>(DIM);
let mut coriolis_v_part = coriolis_v.columns_mut(link.assembly_id, ndofs);
let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs);
// [c3 - c2].gcross() * (JDot + JDot/u * qdot)
let shift_cross_tr = link.shift23.gcross_matrix_tr();
coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0);
// JDot
coriolis_v_part += rb_joint_j_v_dot;
coriolis_w_part += rb_joint_j_w_dot;
let dvel_cross = rb_vels.angvel.gcross(link.shift23).gcross_matrix_tr();
coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0);
// JDot/u * qdot
coriolis_v_part += rb_joint_j_v_dot_veldiff;
coriolis_w_part += rb_joint_j_w_dot_veldiff;
coriolis_v.gemm(
1.0,
&(rb_vels.angvel.gcross_matrix() * shift_cross_tr),
&rb_j_w,
1.0,
);
}
let coriolis_v = &mut self.coriolis_v[i];
let coriolis_w = &mut self.coriolis_w[i];
/*
* Meld with the mass matrix.
*/
@@ -768,6 +729,8 @@ impl Multibody {
// FIXME: avoid allocation inside LU at each timestep.
self.acc_inv_augmented_mass = LU::new(self.acc_augmented_mass.clone());
self.inv_augmented_mass = LU::new(self.augmented_mass.clone());
// self.acc_inv_augmented_mass = self.inv_augmented_mass.clone();
// self.augmented_mass = self.acc_augmented_mass.clone();
// self.inv_augmented_mass = self.acc_inv_augmented_mass.clone();
}
@@ -797,19 +760,16 @@ impl Multibody {
}
#[inline]
pub fn integrate_next(&mut self, dt: Real) {
pub fn integrate(&mut self, dt: Real) {
for rb in self.links.iter_mut() {
rb.state
.joint
rb.joint
.integrate(dt, &self.velocities.as_slice()[rb.assembly_id..])
}
}
pub fn apply_next_displacements(&mut self, disp: &[Real]) {
pub fn apply_displacements(&mut self, disp: &[Real]) {
for link in self.links.iter_mut() {
link.state
.joint
.apply_displacement(&disp[link.assembly_id..])
link.joint.apply_displacement(&disp[link.assembly_id..])
}
}
@@ -825,7 +785,7 @@ impl Multibody {
if rb_type.is_dynamic() {
let free_joint = MultibodyJoint::free(rb_pos.position);
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].state.joint = free_joint;
self.links[0].joint = free_joint;
self.links[0].assembly_id = 0;
self.ndofs += SPATIAL_DIM;
@@ -844,7 +804,7 @@ impl Multibody {
let fixed_joint = MultibodyJoint::fixed(rb_pos.position);
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].state.joint = fixed_joint;
self.links[0].joint = fixed_joint;
self.links[0].assembly_id = 0;
self.ndofs -= prev_root_ndofs;
@@ -871,15 +831,15 @@ impl Multibody {
}
// Make sure the positions are properly set to match the rigid-body’s.
if self.links[0].state.joint.data.locked_axes.is_empty() {
self.links[0].state.joint.set_free_pos(rb_pos.position);
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(rb_pos.position);
} else {
self.links[0].state.joint.data.local_frame1 = rb_pos.position;
self.links[0].joint.data.local_frame1 = rb_pos.position;
}
}
}
pub fn forward_kinematics_next<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyMassProps>
@@ -888,19 +848,16 @@ impl Multibody {
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
link.state
.joint
.update_jacobians(&self.velocities.as_slice());
link.state.local_to_parent = link.state.joint.body_to_parent();
link.state.local_to_world = link.state.local_to_parent;
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
bodies.map_mut_internal(link.rigid_body.0, |poss: &mut RigidBodyPosition| {
poss.next_position = link.state.local_to_world;
bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
rb_pos.next_position = link.local_to_world;
});
if update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
mprops.update_world_mass_properties(&link.state.local_to_world)
mprops.update_world_mass_properties(&link.local_to_world)
});
}
}
@@ -909,16 +866,23 @@ impl Multibody {
for i in 1..self.links.len() {
let (link, parent_link) = self.links.get_mut_with_parent(i);
link.state
.joint
.update_jacobians(&self.velocities.as_slice()[link.assembly_id..]);
link.state.local_to_parent = link.state.joint.body_to_parent();
link.state.local_to_world =
parent_link.state.local_to_world * link.state.local_to_parent;
link.state.parent_to_world = parent_link.state.local_to_world;
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = parent_link.local_to_world * link.local_to_parent;
bodies.map_mut_internal(link.rigid_body.0, |poss: &mut RigidBodyPosition| {
poss.next_position = link.state.local_to_world;
{
let parent_rb_mprops: &RigidBodyMassProps = bodies.index(parent_link.rigid_body.0);
let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
let c0 = parent_link.local_to_world * parent_rb_mprops.local_mprops.local_com;
let c2 = link.local_to_world
* Point::from(link.joint.data.local_frame2.translation.vector);
let c3 = link.local_to_world * rb_mprops.local_mprops.local_com;
link.shift02 = c2 - c0;
link.shift23 = c3 - c2;
}
bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
rb_pos.next_position = link.local_to_world;
});
let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0);
@@ -929,8 +893,8 @@ impl Multibody {
);
if update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
mprops.update_world_mass_properties(&link.state.local_to_world)
bodies.map_mut_internal(link.rigid_body.0, |rb_mprops: &mut RigidBodyMassProps| {
rb_mprops.update_world_mass_properties(&link.local_to_world)
});
}
}
@@ -938,7 +902,7 @@ impl Multibody {
/*
* Compute body jacobians.
*/
self.update_body_next_jacobians(bodies);
self.update_body_jacobians();
}
#[inline]

462
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -1,42 +1,21 @@
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::{
joint, FixedJoint, IntegrationParameters, JointAxesMask, JointData, Multibody, MultibodyLink,
joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink,
RigidBodyVelocity,
};
use crate::math::{
Isometry, JacobianSliceMut, Matrix, Real, Rotation, SpacialVector, Translation, Vector,
ANG_DIM, DIM, SPATIAL_DIM,
Isometry, JacobianSliceMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM,
SPATIAL_DIM,
};
use crate::utils::WCross;
use na::{DVector, DVectorSliceMut};
#[cfg(feature = "dim3")]
use {
crate::utils::WCrossMatrix,
na::{UnitQuaternion, Vector3, VectorSlice3},
};
use na::{UnitQuaternion, Vector3};
#[derive(Copy, Clone, Debug)]
pub struct MultibodyJoint {
pub data: JointData,
pub(crate) coords: SpacialVector<Real>,
pub(crate) joint_rot: Rotation<Real>,
jacobian_v: Matrix<Real>,
jacobian_dot_v: Matrix<Real>,
jacobian_dot_veldiff_v: Matrix<Real>,
}
#[cfg(feature = "dim2")]
fn revolute_locked_axes() -> JointAxesMask {
JointAxesMask::X | JointAxesMask::Y
}
#[cfg(feature = "dim3")]
fn revolute_locked_axes() -> JointAxesMask {
JointAxesMask::X
| JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z
}
impl MultibodyJoint {
@@ -45,9 +24,6 @@ impl MultibodyJoint {
data,
coords: na::zero(),
joint_rot: Rotation::identity(),
jacobian_v: na::zero(),
jacobian_dot_v: na::zero(),
jacobian_dot_veldiff_v: na::zero(),
}
}
@@ -84,76 +60,59 @@ impl MultibodyJoint {
/// The position of the multibody link containing this multibody_joint relative to its parent.
pub fn body_to_parent(&self) -> Isometry<Real> {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
self.data.local_frame1.translation
* self.joint_rot
* self.data.local_frame2.translation.inverse()
} else {
let locked_bits = self.data.locked_axes.bits();
let mut transform = self.joint_rot * self.data.local_frame2.inverse();
let locked_bits = self.data.locked_axes.bits();
let mut transform = self.joint_rot * self.data.local_frame2.inverse();
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
}
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
}
self.data.local_frame1 * transform
}
self.data.local_frame1 * transform
}
/// Integrate the position of this multibody_joint.
pub fn integrate(&mut self, dt: Real, vels: &[Real]) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x_axis();
self.coords[DIM] += vels[0] * dt;
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = 0;
#[cfg(feature = "dim2")]
{
self.joint_rot = Rotation::from_angle(self.coords[DIM]);
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
self.coords[i] += vels[curr_free_dof] * dt;
curr_free_dof += 1;
}
#[cfg(feature = "dim3")]
{
self.joint_rot = Rotation::from_axis_angle(&axis, self.coords[DIM]);
}
} else {
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = 0;
}
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
self.coords[i] += vels[curr_free_dof] * dt;
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
self.coords[DIM + dof_id] += vels[curr_free_dof] * dt;
#[cfg(feature = "dim2")]
{
self.joint_rot = Rotation::new(self.coords[DIM + dof_id]);
}
#[cfg(feature = "dim3")]
3 => {
let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
self.joint_rot = disp * self.joint_rot;
{
self.joint_rot = Rotation::from_axis_angle(
&Vector::ith_axis(dof_id),
self.coords[DIM + dof_id],
);
}
_ => unreachable!(),
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
self.joint_rot = disp * self.joint_rot;
}
_ => unreachable!(),
}
}
@@ -162,278 +121,91 @@ impl MultibodyJoint {
self.integrate(1.0, disp);
}
/// Update the jacobians of this multibody_joint.
pub fn update_jacobians(&mut self, vels: &[Real]) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim2")]
let axis = 1.0;
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x_axis();
let body_shift = self.data.local_frame2.translation.vector;
let shift = self.joint_rot * -body_shift;
let shift_dot_veldiff = axis.gcross(shift);
#[cfg(feature = "dim2")]
{
self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
}
#[cfg(feature = "dim3")]
{
self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
}
self.jacobian_dot_veldiff_v
.column_mut(0)
.copy_from(&axis.gcross(shift_dot_veldiff));
self.jacobian_dot_v
.column_mut(0)
.copy_from(&(axis.gcross(shift_dot_veldiff) * vels[0]));
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let inv_frame2 = self.data.local_frame2.inverse();
let shift = self.joint_rot * inv_frame2.translation.vector;
let angvel =
VectorSlice3::from_slice(&vels[num_free_lin_dofs..num_free_lin_dofs + 3]);
let inv_rotmat2 = inv_frame2.rotation.to_rotation_matrix().into_inner();
self.jacobian_v = inv_rotmat2 * shift.gcross_matrix().transpose();
self.jacobian_dot_v =
inv_rotmat2 * angvel.cross(&shift).gcross_matrix().transpose();
}
_ => unreachable!(),
}
}
}
/// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`.
pub fn jacobian(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim2")]
let axis = 1.0;
pub fn jacobian(&self, transform: &Rotation<Real>, out: &mut JacobianSliceMut<Real>) {
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
let transformed_axis = transform * Vector::ith(i, 1.0);
out.fixed_slice_mut::<DIM, 1>(0, curr_free_dof)
.copy_from(&transformed_axis);
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
#[cfg(feature = "dim2")]
{
out[(DIM, curr_free_dof)] = 1.0;
}
#[cfg(feature = "dim3")]
{
let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
let rotmat = transform.to_rotation_matrix().into_inner();
out.fixed_slice_mut::<ANG_DIM, 1>(DIM, curr_free_dof)
.copy_from(&rotmat.column(dof_id));
}
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x();
let jacobian = RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis);
out.copy_from(jacobian.transformed(transform).as_vector())
} else {
let locked_bits = self.data.locked_axes.bits();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
let transformed_axis = transform * self.data.local_frame1 * Vector::ith(i, 1.0);
out.fixed_slice_mut::<DIM, 1>(0, curr_free_dof)
.copy_from(&transformed_axis);
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let rotmat = transform.rotation.to_rotation_matrix();
out.fixed_slice_mut::<3, 3>(0, curr_free_dof)
.copy_from(&(rotmat * self.jacobian_v));
out.fixed_slice_mut::<3, 3>(3, curr_free_dof)
.copy_from(rotmat.matrix());
}
_ => unreachable!(),
}
}
}
/// Sets in `out` the non-zero entries of the time-derivative of the multibody_joint jacobian transformed by `transform`.
pub fn jacobian_dot(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
let jacobian = RigidBodyVelocity::from_vectors(
self.jacobian_dot_v.column(0).into_owned(),
na::zero(),
);
out.copy_from(jacobian.transformed(transform).as_vector())
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let rotmat = transform.rotation.to_rotation_matrix();
out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
.copy_from(&(rotmat * self.jacobian_dot_v));
}
_ => unreachable!(),
}
}
}
/// Sets in `out` the non-zero entries of the velocity-derivative of the time-derivative of the multibody_joint jacobian transformed by `transform`.
pub fn jacobian_dot_veldiff_mul_coordinates(
&self,
transform: &Isometry<Real>,
acc: &[Real],
out: &mut JacobianSliceMut<Real>,
) {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
let jacobian = RigidBodyVelocity::from_vectors(
self.jacobian_dot_veldiff_v.column(0).into_owned(),
na::zero(),
);
out.copy_from((jacobian.transformed(transform) * acc[0]).as_vector())
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let angvel =
Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
let rotmat = transform.rotation.to_rotation_matrix();
let res = rotmat * angvel.gcross_matrix() * self.jacobian_v;
out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
.copy_from(&res);
}
_ => unreachable!(),
3 => {
let rotmat = transform.to_rotation_matrix();
out.fixed_slice_mut::<3, 3>(3, curr_free_dof)
.copy_from(rotmat.matrix());
}
_ => unreachable!(),
}
}
/// Multiply the multibody_joint jacobian by generalized velocities to obtain the
/// relative velocity of the multibody link containing this multibody_joint.
pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
#[cfg(feature = "dim2")]
let axis = 1.0;
let locked_bits = self.data.locked_axes.bits();
let mut result = RigidBodyVelocity::zero();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
result.linvel += Vector::ith(i, acc[curr_free_dof]);
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
#[cfg(feature = "dim2")]
{
result.angvel += acc[curr_free_dof];
}
#[cfg(feature = "dim3")]
{
let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
result.angvel[dof_id] += acc[curr_free_dof];
}
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
let axis = self.data.local_frame1 * Vector::x();
RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis) * acc[0]
} else {
let locked_bits = self.data.locked_axes.bits();
let mut result = RigidBodyVelocity::zero();
let mut curr_free_dof = 0;
for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 {
result.linvel += self.data.local_frame1 * Vector::ith(i, acc[curr_free_dof]);
curr_free_dof += 1;
}
}
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => { /* No free dofs. */ }
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
let linvel = self.jacobian_v * angvel;
result += RigidBodyVelocity::new(linvel, angvel);
}
_ => unreachable!(),
}
result
}
}
/// Multiply the multibody_joint jacobian by generalized accelerations to obtain the
/// relative acceleration of the multibody link containing this multibody_joint.
pub fn jacobian_dot_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
if self.data.locked_axes == revolute_locked_axes() {
// FIXME: this is a special case for the revolute joint.
// We have the mathematical formulation ready that works in the general case, but its
// implementation will take some time. So let’s make a special case for the alpha
// release and fix is soon after.
RigidBodyVelocity::from_vectors(self.jacobian_dot_v.column(0).into_owned(), na::zero())
* acc[0]
} else {
let locked_bits = self.data.locked_axes.bits();
let locked_ang_bits = locked_bits >> DIM;
let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
match num_free_ang_dofs {
0 => {
/* No free dofs. */
RigidBodyVelocity::zero()
}
1 => {
todo!()
}
2 => {
todo!()
}
#[cfg(feature = "dim3")]
3 => {
let num_free_lin_dofs = self.num_free_lin_dofs();
let angvel =
Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
let linvel = self.jacobian_dot_v * angvel;
RigidBodyVelocity::new(linvel, na::zero())
}
_ => unreachable!(),
3 => {
let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
result.angvel += angvel;
}
_ => unreachable!(),
}
result
}
/// Fill `out` with the non-zero entries of a damping that can be applied by default to ensure a good stability of the multibody_joint.
@@ -445,7 +217,7 @@ impl MultibodyJoint {
for i in DIM..SPATIAL_DIM {
if locked_bits & (1 << i) == 0 {
// This is a free angular DOF.
out[curr_free_dof] = 0.2;
out[curr_free_dof] = 0.1;
curr_free_dof += 1;
}
}

75
src/dynamics/joint/multibody_joint/multibody_link.rs
View File

@@ -1,35 +1,14 @@
use std::ops::{Deref, DerefMut};
use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
use crate::math::{Isometry, Real};
use crate::math::{Isometry, Real, Vector};
use crate::prelude::RigidBodyVelocity;
pub(crate) struct KinematicState {
pub joint: MultibodyJoint,
pub parent_to_world: Isometry<Real>,
// TODO: should this be removed in favor of the rigid-body position?
pub local_to_world: Isometry<Real>,
pub local_to_parent: Isometry<Real>,
}
impl Clone for KinematicState {
fn clone(&self) -> Self {
Self {
joint: self.joint.clone(),
parent_to_world: self.parent_to_world.clone(),
local_to_world: self.local_to_world.clone(),
local_to_parent: self.local_to_parent.clone(),
}
}
}
/// One link of a multibody.
pub struct MultibodyLink {
pub(crate) name: String,
// FIXME: make all those private.
pub(crate) internal_id: usize,
pub(crate) assembly_id: usize,
pub(crate) is_leaf: bool,
pub(crate) parent_internal_id: usize,
pub(crate) rigid_body: RigidBodyHandle,
@@ -37,11 +16,15 @@ pub struct MultibodyLink {
/*
* Change at each time step.
*/
pub(crate) state: KinematicState,
pub joint: MultibodyJoint,
// TODO: should this be removed in favor of the rigid-body position?
pub local_to_world: Isometry<Real>,
pub local_to_parent: Isometry<Real>,
pub shift02: Vector<Real>,
pub shift23: Vector<Real>,
// FIXME: put this on a workspace buffer instead ?
pub(crate) velocity_dot_wrt_joint: RigidBodyVelocity,
pub(crate) velocity_wrt_joint: RigidBodyVelocity,
/// The velocity added by the joint, in world-space.
pub(crate) joint_velocity: RigidBodyVelocity,
}
impl MultibodyLink {
@@ -52,35 +35,27 @@ impl MultibodyLink {
assembly_id: usize,
parent_internal_id: usize,
joint: MultibodyJoint,
parent_to_world: Isometry<Real>,
local_to_world: Isometry<Real>,
local_to_parent: Isometry<Real>,
) -> Self {
let is_leaf = true;
let velocity_dot_wrt_joint = RigidBodyVelocity::zero();
let velocity_wrt_joint = RigidBodyVelocity::zero();
let kinematic_state = KinematicState {
joint,
parent_to_world,
local_to_world,
local_to_parent,
};
let joint_velocity = RigidBodyVelocity::zero();
MultibodyLink {
name: String::new(),
internal_id,
assembly_id,
is_leaf,
parent_internal_id,
state: kinematic_state,
velocity_dot_wrt_joint,
velocity_wrt_joint,
joint,
local_to_world,
local_to_parent,
shift02: na::zero(),
shift23: na::zero(),
joint_velocity,
rigid_body,
}
}
pub fn joint(&self) -> &MultibodyJoint {
&self.state.joint
&self.joint
}
pub fn rigid_body_handle(&self) -> RigidBodyHandle {
@@ -93,18 +68,6 @@ impl MultibodyLink {
self.internal_id == 0
}
/// This link's name.
#[inline]
pub fn name(&self) -> &str {
&self.name
}
/// Sets this link's name.
#[inline]
pub fn set_name(&mut self, name: String) {
self.name = name
}
/// The handle of this multibody link.
#[inline]
pub fn link_id(&self) -> usize {
@@ -123,12 +86,12 @@ impl MultibodyLink {
#[inline]
pub fn local_to_world(&self) -> &Isometry<Real> {
&self.state.local_to_world
&self.local_to_world
}
#[inline]
pub fn local_to_parent(&self) -> &Isometry<Real> {
&self.state.local_to_parent
&self.local_to_parent
}
}