Module k::urdf
[−]
[src]
Load URDF format and create k::JointWithLinTree
k::urdf
uses urdf-rs to load urdf model.
k::urdf
converts urdf_rs::Robot
to k::LinkTree
or k::LinkStar
Examples
use k::InverseKinematicsSolver; use k::KinematicChain; let robot = k::urdf::create_tree_from_file("urdf/sample.urdf").unwrap(); let mut arms = k::create_kinematic_chains(&robot); // set joint angles let angles = vec![0.8, 0.2, 0.0, -1.5, 0.0, -0.3]; arms[0].set_joint_angles(&angles).unwrap(); // get the transform of the end of the manipulator (forward kinematics) let mut target = arms[0].calc_end_transform(); target.translation.vector[2] += 0.1; let solver = k::JacobianIKSolverBuilder::new().finalize(); // solve and move the manipulator angles solver.solve(&mut arms[0], &target) .unwrap_or_else(|err| { println!("Err: {}", err); 0.0f32 }); println!("angles={:?}", arms[0].get_joint_angles());
Functions
create_star |
Create |
create_star_from_file |
Create |
create_tree |
Create |
create_tree_from_file |
Create |
quaternion_from | |
translation_from |