k
: Kinematics library for rust-lang
k
has below functionalities.
- Forward kinematics
- Inverse kinematics
- URDF Loader
k
uses nalgebra as math library.
See Document and examples/ for more details.
Requirements to build examples
sudo apt install g++ cmake xorg-dev libglu1-mesa-dev
IK example with GUI
cargo run --release --example interactive_ik
Push below keys to move the end of the manipulator.
f
: forward
b
: backward
p
: up
n
: down
l
: left
r
: right
z
: reset the manipulator state.
Create link tree from urdf and solve IK
use k::prelude::*;
fn main() {
let chain = k::Chain::<f32>::from_urdf_file("urdf/sample.urdf").unwrap();
println!("chain: {}", chain);
let angles = vec![0.2, 0.2, 0.0, -1.0, 0.0, 0.0, 0.2, 0.2, 0.0, -1.0, 0.0, 0.0];
chain.set_joint_positions(&angles).unwrap();
println!("initial angles={:?}", chain.joint_positions());
let target_link = chain.find("l_wrist_pitch").unwrap();
chain.update_transforms();
let mut target = target_link.world_transform().unwrap();
println!("initial target pos = {}", target.translation);
println!("move z: +0.1");
target.translation.vector.z += 0.1;
let solver = k::JacobianIkSolver::default();
let arm = k::SerialChain::from_end(target_link);
solver.solve(&arm, &target).unwrap();
println!("solved angles={:?}", chain.joint_positions());
chain.update_transforms();
let solved_pose = target_link.world_transform().unwrap();
println!("solved target pos = {}", solved_pose.translation);
}
Structure of API
Top level interface is Chain
struct. It contains Node
s and they have the relations between nodes (parent/children).
Actual data (joint angle(position), transform between nodes) is stored in Joint
object inside nodes.
You can get local/world transform of nodes. See below figure to understand what is the node's local_transform()
and world_transform()
.