Struct k::Chain

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

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

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

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);

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

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]);

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

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);

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

Get the Chain’s origin

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");

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()

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

Iterate for links

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);

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);

pub fn find_link(&self, link_name: &str) -> Option<&Node<T>>

Find the joint by link name

Examples

use k::*;

let l0 = Node::new(NodeBuilder::new()
    .name("fixed")
    .finalize());
l0.set_link(Some(link::LinkBuilder::new().name("link0").finalize()));
let mut 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);
l1.set_link(Some(link::LinkBuilder::new().name("link1").finalize()));
let tree = Chain::from_root(l0);
let joint_name = tree.find_link("link1").unwrap()
.joint().name.clone();
assert_eq!(joint_name, "pitch1");

pub fn joint_positions(&self) -> Vec<T>

Get the positions of the joints

FixedJoint is ignored. the length is the same with dof()

pub fn set_joint_positions(&self, positions_vec: &[T]) -> Result<(), Error>

Set the positions of the joints

FixedJoints are ignored. the input number must be equal with dof()

pub fn set_joint_positions_clamped(&self, positions_vec: &[T])

Set the clamped positions of the joints

This function is safe, in contrast to set_joint_positions_unchecked.

pub fn set_joint_positions_unchecked(&self, positions_vec: &[T])

Fast, but without check, dangerous set_joint_positions

pub fn update_transforms(&self) -> Vec<Isometry3<T>>

Update world_transform() of the joints

pub fn update_velocities(&self) -> Vec<Velocity<T>>

Update world_velocity() of the joints

pub fn update_link_transforms(&self)

Update transforms of the links

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

pub fn from_urdf_file<P>(path: P) -> Result<Self, UrdfError>where
    P: AsRef<Path>,

Trait Implementations

impl<T> Clone for Chain<T>where
    T: RealField + SubsetOf<f64>,

fn clone(&self) -> Self

Returns a copy of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl<T: Debug + RealField> Debug for Chain<T>

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

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

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl<'a, T> From<&'a Robot> for Chain<T>where
    T: RealField + SubsetOf<f64>,

fn from(robot: &Robot) -> Self

Converts to this type from the input type.

impl<T> From<Robot> for Chain<T>where
    T: RealField + SubsetOf<f64>,

fn from(robot: Robot) -> Self

Converts to this type from the input type.