Struct k::Chain [−][src]
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>
[src]
pub fn from_root(root_joint: Node<T>) -> Self
[src]
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>
[src]
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>
[src]
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>
[src]
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>)
[src]
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>
[src]
Get the Chain
’s origin
pub fn iter(&self) -> impl Iterator<Item = &Node<T>>
[src]
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>>
[src]
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>>
[src]
Iterate for links
pub fn dof(&self) -> usize
[src]
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>>
[src]
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 joint_positions(&self) -> Vec<T>ⓘ
[src]
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>
[src]
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])
[src]
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])
[src]
Fast, but without check, dangerous set_joint_positions
pub fn update_transforms(&self) -> Vec<Isometry3<T>>ⓘ
[src]
Update world_transform() of the joints
pub fn update_velocities(&self) -> Vec<Velocity<T>>ⓘ
[src]
Update world_velocity() of the joints
pub fn update_link_transforms(&self)
[src]
Update transforms of the links
impl<T> Chain<T> where
T: RealField + SubsetOf<f64>,
[src]
T: RealField + SubsetOf<f64>,
Trait Implementations
impl<T> Clone for Chain<T> where
T: RealField + SubsetOf<f64>,
[src]
T: RealField + SubsetOf<f64>,
fn clone(&self) -> Self
[src]
pub fn clone_from(&mut self, source: &Self)
1.0.0[src]
impl<T: Debug + RealField> Debug for Chain<T>
[src]
impl<T: RealField + SubsetOf<f64>> Display for Chain<T>
[src]
impl<'a, T> From<&'a Robot> for Chain<T> where
T: RealField + SubsetOf<f64>,
[src]
T: RealField + SubsetOf<f64>,
impl<T> From<Robot> for Chain<T> where
T: RealField + SubsetOf<f64>,
[src]
T: RealField + SubsetOf<f64>,
Auto Trait Implementations
impl<T> RefUnwindSafe for Chain<T>
impl<T> Send for Chain<T>
impl<T> Sync for Chain<T>
impl<T> Unpin for Chain<T>
impl<T> UnwindSafe for Chain<T>
Blanket Implementations
impl<T> Any for T where
T: 'static + ?Sized,
[src]
T: 'static + ?Sized,
impl<T> Borrow<T> for T where
T: ?Sized,
[src]
T: ?Sized,
impl<T> BorrowMut<T> for T where
T: ?Sized,
[src]
T: ?Sized,
pub fn borrow_mut(&mut self) -> &mut T
[src]
impl<T> From<T> for T
[src]
impl<T, U> Into<U> for T where
U: From<T>,
[src]
U: From<T>,
impl<T> Same<T> for T
type Output = T
Should always be Self
impl<SS, SP> SupersetOf<SS> for SP where
SS: SubsetOf<SP>,
[src]
SS: SubsetOf<SP>,
pub fn to_subset(&self) -> Option<SS>
[src]
pub fn is_in_subset(&self) -> bool
[src]
pub fn to_subset_unchecked(&self) -> SS
[src]
pub fn from_subset(element: &SS) -> SP
[src]
impl<T> ToOwned for T where
T: Clone,
[src]
T: Clone,
type Owned = T
The resulting type after obtaining ownership.
pub fn to_owned(&self) -> T
[src]
pub fn clone_into(&self, target: &mut T)
[src]
impl<T> ToString for T where
T: Display + ?Sized,
[src]
T: Display + ?Sized,
impl<T, U> TryFrom<U> for T where
U: Into<T>,
[src]
U: Into<T>,
type Error = Infallible
The type returned in the event of a conversion error.
pub fn try_from(value: U) -> Result<T, <T as TryFrom<U>>::Error>
[src]
impl<T, U> TryInto<U> for T where
U: TryFrom<T>,
[src]
U: TryFrom<T>,