use super::errors::*;
use super::joint::*;
use super::node::*;
use na::{Isometry3, RealField};
use nalgebra as na;
use simba::scalar::SubsetOf;
use std::fmt::{self, Display};
use std::ops::Deref;
#[derive(Debug)]
pub struct Chain<T: RealField> {
nodes: Vec<Node<T>>,
movable_nodes: Vec<Node<T>>,
dof: usize,
}
impl<T: RealField + SubsetOf<f64>> Chain<T> {
fn fmt_with_indent_level(
&self,
node: &Node<T>,
level: usize,
f: &mut fmt::Formatter,
) -> fmt::Result {
if self.nodes.iter().any(|joint| joint == node) {
writeln!(f, "{}{}", " ".repeat(level), node)?;
}
for c in node.children().iter() {
self.fmt_with_indent_level(c, level + 1, f)?
}
Ok(())
}
}
impl<T: RealField + SubsetOf<f64>> Display for Chain<T> {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
self.fmt_with_indent_level(&self.iter().next().unwrap(), 0, f)
}
}
impl<T: RealField + SubsetOf<f64>> Chain<T> {
#[allow(clippy::needless_pass_by_value)]
pub fn from_root(root_joint: Node<T>) -> Self {
let nodes = root_joint.iter_descendants().collect::<Vec<_>>();
let movable_nodes = nodes
.iter()
.filter(|joint| joint.joint().is_movable())
.cloned()
.collect::<Vec<_>>();
Chain {
dof: movable_nodes.len(),
nodes,
movable_nodes,
}
}
pub fn from_end(end_joint: &Node<T>) -> Chain<T> {
let mut nodes = end_joint.iter_ancestors().collect::<Vec<_>>();
nodes.reverse();
let movable_nodes = nodes
.iter()
.filter(|joint| joint.joint().is_movable())
.cloned()
.collect::<Vec<_>>();
Chain {
dof: movable_nodes.len(),
movable_nodes,
nodes,
}
}
pub fn set_origin(&self, pose: na::Isometry3<T>) {
self.nodes[0].set_origin(pose)
}
pub fn origin(&self) -> na::Isometry3<T> {
self.nodes[0].origin()
}
pub fn iter(&self) -> impl Iterator<Item = &Node<T>> {
self.nodes.iter()
}
pub fn iter_joints(&self) -> impl Iterator<Item = JointRefGuard<T>> {
self.movable_nodes.iter().map(|node| node.joint())
}
pub fn iter_links(&self) -> impl Iterator<Item = LinkRefGuard<T>> {
self.nodes.iter().filter_map(|node| {
if node.link().is_some() {
Some(LinkRefGuard { guard: node.lock() })
} else {
None
}
})
}
pub fn dof(&self) -> usize {
self.dof
}
pub fn find(&self, joint_name: &str) -> Option<&Node<T>> {
self.iter().find(|joint| joint.joint().name == joint_name)
}
pub fn joint_positions(&self) -> Vec<T> {
self.iter_joints()
.map(|joint| {
joint
.joint_position()
.expect("Must be a bug: movable joint must have position")
})
.collect()
}
pub fn set_joint_positions(&self, positions_vec: &[T]) -> Result<(), Error> {
if positions_vec.len() != self.dof {
return Err(Error::SizeMismatchError {
input: positions_vec.len(),
required: self.dof,
});
}
for (joint, position) in self.movable_nodes.iter().zip(positions_vec.iter()) {
joint.set_joint_position(*position)?;
}
Ok(())
}
pub fn set_joint_positions_clamped(&self, positions_vec: &[T]) {
for (joint, position) in self.movable_nodes.iter().zip(positions_vec.iter()) {
joint.set_joint_position_clamped(*position);
}
}
#[inline]
pub fn set_joint_positions_unchecked(&self, positions_vec: &[T]) {
for (joint, position) in self.movable_nodes.iter().zip(positions_vec.iter()) {
joint.set_joint_position_unchecked(*position);
}
}
pub fn update_transforms(&self) -> Vec<Isometry3<T>> {
self.iter()
.map(|node| {
let parent_transform = node.parent_world_transform().expect("cache must exist");
let trans = parent_transform * node.joint().local_transform();
node.joint().set_world_transform(trans);
trans
})
.collect()
}
pub fn update_velocities(&self) -> Vec<Velocity<T>> {
self.update_transforms();
self.iter()
.map(|node| {
let parent_transform = node
.parent_world_transform()
.expect("transform cache must exist");
let parent_velocity = node
.parent_world_velocity()
.expect("velocity cache must exist");
let velocity = match node.joint().joint_type {
JointType::Fixed => parent_velocity,
JointType::Rotational { axis } => {
let parent = node.parent().expect("parent must exist");
let parent_vel = parent.joint().origin().translation.vector;
Velocity::from_parts(
parent_velocity.translation
+ parent_velocity.rotation.cross(
&(parent_transform.rotation.to_rotation_matrix() * parent_vel),
),
parent_velocity.rotation
+ node
.world_transform()
.expect("cache must exist")
.rotation
.to_rotation_matrix()
* (axis.into_inner() * node.joint().joint_velocity().unwrap()),
)
}
JointType::Linear { axis } => Velocity::from_parts(
parent_velocity.translation
+ node
.world_transform()
.expect("cache must exist")
.rotation
.to_rotation_matrix()
* (axis.into_inner() * node.joint().joint_velocity().unwrap()),
parent_velocity.rotation,
),
};
node.joint().set_world_velocity(velocity);
velocity
})
.collect()
}
pub fn update_link_transforms(&self) {
self.update_transforms();
self.iter().for_each(|node| {
let parent_transform = node.parent_world_transform().expect("cache must exist");
let mut node_mut = node.lock();
if let Some(ref mut link) = node_mut.link {
let inertial_trans = parent_transform * link.inertial.origin();
link.inertial.set_world_transform(inertial_trans);
for c in &mut link.collisions {
let c_trans = parent_transform * c.origin();
c.set_world_transform(c_trans);
}
for v in &mut link.visuals {
let v_trans = parent_transform * v.origin();
v.set_world_transform(v_trans);
}
}
});
}
}
impl<T> Clone for Chain<T>
where
T: RealField + SubsetOf<f64>,
{
fn clone(&self) -> Self {
if self.nodes.is_empty() {
return Self {
nodes: vec![],
movable_nodes: vec![],
dof: 0,
};
}
assert!(self.nodes[0].is_root());
let mut new_nodes = self
.nodes
.iter()
.map(|n| {
let node = Node::new(n.joint().clone());
node.set_link(n.link().clone());
node
})
.collect::<Vec<_>>();
for i in 0..new_nodes.len() {
if let Some(p) = self.nodes[i].parent() {
let parent_index = self.nodes.iter().position(|x| *x == p).unwrap();
new_nodes[i].set_parent(&new_nodes[parent_index]);
}
if let Some(m) = self.nodes[i].mimic_parent() {
let parent_index = self.nodes.iter().position(|x| *x == m).unwrap();
new_nodes[i].set_mimic_parent(
&new_nodes[parent_index],
self.nodes[i].lock().mimic.clone().unwrap(),
);
}
}
assert!(new_nodes[0].is_root());
Chain::from_root(new_nodes.remove(0))
}
}
#[derive(Debug)]
pub struct SerialChain<T: RealField> {
inner: Chain<T>,
}
impl<T> SerialChain<T>
where
T: RealField + SubsetOf<f64>,
{
pub fn new_unchecked(inner: Chain<T>) -> Self {
Self { inner }
}
pub fn try_new(inner: Chain<T>) -> Option<Self> {
{
let num = inner.iter().count();
for node in inner.iter().take(num - 1) {
if node.children().len() != 1 {
return None;
}
}
}
Some(Self { inner })
}
pub fn from_end(end_joint: &Node<T>) -> SerialChain<T> {
SerialChain {
inner: Chain::from_end(end_joint),
}
}
pub fn unwrap(self) -> Chain<T> {
self.inner
}
pub fn end_transform(&self) -> Isometry3<T> {
self.iter().fold(Isometry3::identity(), |trans, joint| {
trans * joint.joint().local_transform()
})
}
}
impl<T> Clone for SerialChain<T>
where
T: RealField + SubsetOf<f64>,
{
fn clone(&self) -> Self {
Self {
inner: self.inner.clone(),
}
}
}
impl<T> Display for SerialChain<T>
where
T: RealField + SubsetOf<f64>,
{
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
self.inner.fmt(f)
}
}
impl<T> Deref for SerialChain<T>
where
T: RealField,
{
type Target = Chain<T>;
fn deref(&self) -> &Self::Target {
&self.inner
}
}
#[test]
fn test_chain0() {
use super::joint::*;
use super::node::*;
use na;
let joint0 = NodeBuilder::new()
.name("j0")
.translation(na::Translation3::new(0.0, 0.1, 0.0))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint1 = NodeBuilder::new()
.translation(na::Translation3::new(0.0, 0.1, 0.1))
.name("j1")
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint2 = NodeBuilder::new()
.name("j2")
.translation(na::Translation3::new(0.0, 0.1, 0.1))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint3 = NodeBuilder::new()
.name("j3")
.translation(na::Translation3::new(0.0, 0.1, 0.2))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint4 = NodeBuilder::new()
.name("j4")
.translation(na::Translation3::new(0.0, 0.1, 0.1))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint5 = NodeBuilder::new()
.name("j5")
.translation(na::Translation3::new(0.0, 0.1, 0.1))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
joint1.set_parent(&joint0);
joint2.set_parent(&joint1);
joint3.set_parent(&joint2);
joint4.set_parent(&joint0);
joint5.set_parent(&joint4);
let names = joint0
.iter_descendants()
.map(|joint| joint.joint().name.clone())
.collect::<Vec<_>>();
println!("{}", joint0);
assert_eq!(names.len(), 6);
println!("names = {:?}", names);
let positions = joint0
.iter_descendants()
.map(|node| node.joint().joint_position())
.collect::<Vec<_>>();
println!("positions = {:?}", positions);
fn get_z(joint: &Node<f32>) -> f32 {
match joint.parent_world_transform() {
Some(iso) => iso.translation.vector.z,
None => 0.0f32,
}
}
let poses = joint0
.iter_descendants()
.map(|joint| get_z(&joint))
.collect::<Vec<_>>();
println!("poses = {:?}", poses);
let _ = joint0
.iter_ancestors()
.map(|joint| joint.set_joint_position(-0.5))
.collect::<Vec<_>>();
let positions = joint0
.iter_descendants()
.map(|node| node.joint().joint_position())
.collect::<Vec<_>>();
println!("positions = {:?}", positions);
let poses = joint0
.iter_descendants()
.map(|joint| get_z(&joint))
.collect::<Vec<_>>();
println!("poses = {:?}", poses);
let arm = Chain::from_end(&joint3);
assert_eq!(arm.joint_positions().len(), 4);
println!("{:?}", arm.joint_positions());
}
#[test]
fn test_mimic() {
use super::joint::*;
use super::node::*;
use na;
let joint0 = NodeBuilder::new()
.name("j0")
.translation(na::Translation3::new(0.0, 0.1, 0.0))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint1 = NodeBuilder::new()
.name("joint1")
.translation(na::Translation3::new(0.0, 0.1, 0.1))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
let joint2 = NodeBuilder::new()
.name("joint2")
.translation(na::Translation3::new(0.0, 0.1, 0.1))
.joint_type(JointType::Rotational {
axis: na::Vector3::y_axis(),
})
.into_node();
joint1.set_parent(&joint0);
joint2.set_parent(&joint1);
joint2.set_mimic_parent(&joint1, Mimic::new(2.0, 0.5));
let arm = Chain::from_root(joint0);
assert_eq!(arm.joint_positions().len(), 3);
println!("{:?}", arm.joint_positions());
let positions = vec![0.1, 0.2, 0.3];
arm.set_joint_positions(&positions).unwrap();
let positions = arm.joint_positions();
assert_eq!(positions[0], 0.1);
assert_eq!(positions[1], 0.2);
assert_eq!(positions[2], 0.9);
}