Struct k::Chain

source · [−]

pub struct Chain<T: RealField> { /* private fields */ }

Expand description

Kinematic Chain using Node

Examples

use k::*;
use k::prelude::*;

let l0 = NodeBuilder::new()
    .name("joint_pitch0")
    .translation(Translation3::new(0.0, 0.0, 0.1))
    .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
    .into_node();
let l1 = NodeBuilder::new()
    .name("joint_pitch1")
    .translation(Translation3::new(0.0, 0.0, 0.5))
    .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
    .into_node();
let l2 = NodeBuilder::new()
    .name("hand")
    .translation(Translation3::new(0.0, 0.0, 0.5))
    .joint_type(JointType::Fixed)
    .into_node();

// Sequential joints structure
connect![l0 => l1 => l2];

let mut tree = Chain::from_root(l0);
assert_eq!(tree.dof(), 2);

// Get joint positions
let positions = tree.joint_positions();
assert_eq!(positions.len(), 2);
assert_eq!(positions[0], 0.0);
assert_eq!(positions[1], 0.0);

// Get the initial joint transforms
let transforms = tree.update_transforms();
assert_eq!(transforms.len(), 3);
assert_eq!(transforms[0].translation.vector.z, 0.1);
assert_eq!(transforms[1].translation.vector.z, 0.6);
assert_eq!(transforms[2].translation.vector.z, 1.1);
for t in transforms {
    println!("before: {}", t);
}

// Set joint positions
tree.set_joint_positions(&vec![1.0, 2.0]).unwrap();
let positions = tree.joint_positions();
assert_eq!(positions[0], 1.0);
assert_eq!(positions[1], 2.0);

// Get the result of forward kinematics
let transforms = tree.update_transforms();
assert_eq!(transforms.len(), 3);
for t in transforms {
    println!("after: {}", t);
}

Implementations

source

impl<T: RealField + SubsetOf<f64>> Chain<T>

source

pub fn from_root(root_joint: Node<T>) -> Self

Create Chain from root joint

Examples

use k;

let l0 = k::NodeBuilder::new().into_node();
let l1 = k::NodeBuilder::new().into_node();
l1.set_parent(&l0);
let tree = k::Chain::<f32>::from_root(l0);

source

pub fn from_end(end_joint: &Node<T>) -> Chain<T>

Create Chain from end joint. It has any branches.

Do not discard root joint before create Chain. If you want to get Chain, unwrap() this, it is safe.

Examples

use k::*;

fn create_tree_from_end() -> Chain<f64> {
  let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
  let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
  l1.set_parent(&l0);
  Chain::from_end(&l1) // ok, because root is stored in `Chain`
}

let tree = create_tree_from_end(); // no problem

source

pub fn from_nodes(nodes: Vec<Node<T>>) -> Chain<T>

Create Chain from nodes.

This method is public, but it is for professional use.

Examples

use k::*;

let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
l1.set_parent(&l0);
let chain = Chain::<f64>::from_nodes(vec![l0, l1]);

source

pub fn from_end_to_root(end_joint: &Node<T>, root_joint: &Node<T>) -> Chain<T>

Create Chain from end node and root node, without any branches. The root node is included in the chain.

Examples

use k::*;

let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
let l2 = Node::new(Joint::new("fixed2", JointType::Fixed));
let l3 = Node::new(Joint::new("fixed3", JointType::Fixed));
l1.set_parent(&l0);
l2.set_parent(&l1);
l3.set_parent(&l2);
let chain = Chain::<f32>::from_end_to_root(&l2, &l1);

assert!(chain.find("fixed0").is_none()); // not included
assert!(chain.find("fixed1").is_some());
assert!(chain.find("fixed2").is_some());
assert!(chain.find("fixed3").is_none()); // not included

source

pub fn set_origin(&self, pose: Isometry3<T>)

Set the Chain’s origin

Examples

use k::*;

let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
l1.set_parent(&l0);
let c = Chain::<f32>::from_end(&l1);
let mut o = c.origin();
assert!(o.translation.vector[0].abs() < 0.000001);
o.translation.vector[0] = 1.0;
c.set_origin(o);
assert!((o.translation.vector[0] - 1.0).abs() < 0.000001);

source

pub fn origin(&self) -> Isometry3<T>

Get the Chain’s origin

source

pub fn iter(&self) -> impl Iterator<Item = &Node<T>>

Iterate for all joint nodes

The order is from parent to children. You can assume that parent is already iterated.

Examples

use k::*;

let l0 = Node::new(Joint::new("fixed0", JointType::Fixed));
let l1 = Node::new(Joint::new("fixed1", JointType::Fixed));
l1.set_parent(&l0);
let tree = Chain::<f64>::from_root(l0);
let names = tree.iter().map(|node| node.joint().name.to_owned()).collect::<Vec<_>>();
assert_eq!(names.len(), 2);
assert_eq!(names[0], "fixed0");
assert_eq!(names[1], "fixed1");

source

pub fn iter_joints(&self) -> impl Iterator<Item = JointRefGuard<'_, T>>

Iterate for movable joints

Fixed joints are ignored. If you want to manipulate on Fixed, use iter() instead of iter_joints()

source

pub fn iter_links(&self) -> impl Iterator<Item = LinkRefGuard<'_, T>>

Iterate for links

source

pub fn dof(&self) -> usize

Calculate the degree of freedom

Examples

use k::*;
let l0 = NodeBuilder::new()
    .joint_type(JointType::Fixed)
    .finalize()
    .into();
let l1 : Node<f64> = NodeBuilder::new()
    .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
    .finalize()
    .into();
l1.set_parent(&l0);
let tree = Chain::from_root(l0);
assert_eq!(tree.dof(), 1);

source

pub fn find(&self, joint_name: &str) -> Option<&Node<T>>

Find the joint by name

Examples

use k::*;

let l0 = Node::new(NodeBuilder::new()
    .name("fixed")
    .finalize());
let l1 = Node::new(NodeBuilder::new()
    .name("pitch1")
    .translation(Translation3::new(0.0, 0.1, 0.0))
    .joint_type(JointType::Rotational{axis: Vector3::y_axis()})
    .finalize());
l1.set_parent(&l0);
let tree = Chain::from_root(l0);
let j = tree.find("pitch1").unwrap();
j.set_joint_position(0.5).unwrap();
assert_eq!(j.joint_position().unwrap(), 0.5);