openrr / k Goto Github PK
View Code? Open in Web Editor NEWk: Kinematics Library for rust-lang
License: Apache License 2.0
k: Kinematics Library for rust-lang
License: Apache License 2.0
First of all, thanks for making this great library!
I was looking to use this library with a new collision detection library parry3d which replaces ncollide3d
, but it seems that the minimum supported version of nalgebra there is 0.29.
I figured I would look into what it would take to update the k
library to match this newer version of nalgebra, and it seems that the most prominent difference is that nalgebra 0.29 uses a RealField that no longer implements the Copy
trait. This means that in several locations, fields need to be cloned now, or things need to be refactored slightly. Overall it doesn't seem like too big a change. I might be able to help out a bit, if this is of interest to you.
Example line where Copy is no longer supported:
Line 498 in 0d1f0d0
This was due to the limitation that a feature cannot have the same name as a dependency, but namespaced features (dep:
syntax) added in 1.60 allow to work around this limitation.
Line 14 in 2a0d352
nalgebra still uses a feature called "serde-serialize" too, but as far as I know, "serde" feature is more common and it seems that the nalgebra maintainer also wants to rename it. dimforge/nalgebra#868 (comment)
Hi,
Im investigating your library with the aim of building a kinematic model for a 5 axis cnc machine. Im starting off very basic with 1 then 2, etc dof. My 1 dof test seems to work but my 2 dof test does not converge.
Could you shed some light on what I'm doing wrong?
I've included a bare bones reproduction of the code...Apologies as this is more of a question than an issue.
fn test_kinematics() {
// Works...
test_kinematics_1_dof();
// Errors...
// thread 'main' panicked at 'called `Result::unwrap()` on an `Err`
// value: NotConvergedError { num_tried: 10, position_diff:
// Matrix { data: [0.0009765625, 0.0009765625, 0.0] },
// rotation_diff: Matrix { data: [0.0, 0.0, 0.0] } }'
test_kinematics_2_dof();
}
fn test_kinematics_1_dof() {
let machine_frame = k::Node::new(
k::NodeBuilder::new()
.name("machine_frame")
.translation(na::Translation3::new(0.0, 0.0, 0.0))
.joint_type(k::JointType::Fixed)
.finalize(),
);
let x_axis_linear = k::Node::new(
k::NodeBuilder::new()
.name("x_axis_linear")
.translation(na::Translation3::new(0.0, 0.0, 0.0))
.joint_type(k::JointType::Linear {
axis: na::Vector3::x_axis(),
})
.finalize(),
);
x_axis_linear.set_parent(&machine_frame);
let chain = k::Chain::from_root(machine_frame);
// Initial position
let origin_positions_vec = vec![0.0];
chain.set_joint_positions(&origin_positions_vec).unwrap();
println!("initial positions = {:?}", chain.joint_positions());
// Forward kinematics
let target_positions_vec = vec![1.0];
let workpiece = chain.find("x_axis_linear").unwrap();
chain.update_transforms();
let mut target = x_axis_linear.world_transform().unwrap();
println!("initial target pos = {:?}", target.translation);
println!("move x: {:?}", target_positions_vec[0]);
target.translation.vector.x += target_positions_vec[0];
// Inverse kinematics
let solver = k::JacobianIKSolver::default();
let arm = k::SerialChain::from_end(workpiece);
let mut constraints = k::Constraints::default();
constraints.rotation_x = false;
constraints.rotation_y = false;
constraints.rotation_z = false;
// constraints.position_x = false;
constraints.position_y = false;
constraints.position_z = false;
solver
.solve_with_constraints(&arm, &target, &constraints)
.unwrap();
println!("solved positions = {:?}", chain.joint_positions());
chain.update_transforms();
let solved_pose = workpiece.world_transform().unwrap();
println!("solved target positions = {:?}", solved_pose.translation);
}
fn test_kinematics_2_dof() {
let machine_frame = k::Node::new(
k::NodeBuilder::new()
.name("machine_frame")
.translation(na::Translation3::new(0.0, 0.0, 0.0))
.joint_type(k::JointType::Fixed)
.finalize(),
);
let x_axis_linear = k::Node::new(
k::NodeBuilder::new()
.name("x_axis_linear")
.translation(na::Translation3::new(0.0, 0.0, 0.0))
.joint_type(k::JointType::Linear {
axis: na::Vector3::x_axis(),
})
.finalize(),
);
let y_axis_linear = k::Node::new(
k::NodeBuilder::new()
.name("y_axis_linear")
.translation(na::Translation3::new(0.0, 0.0, 0.0))
.joint_type(k::JointType::Linear {
axis: na::Vector3::y_axis(),
})
.finalize(),
);
x_axis_linear.set_parent(&machine_frame);
y_axis_linear.set_parent(&x_axis_linear);
let chain = k::Chain::from_root(machine_frame);
// Initial position
let origin_positions_vec = vec![0.0, 0.0];
chain.set_joint_positions(&origin_positions_vec).unwrap();
println!("initial positions = {:?}", chain.joint_positions());
// Forward kinematics
let target_positions_vec = vec![1.0, 1.0];
let workpiece = chain.find("y_axis_linear").unwrap();
chain.update_transforms();
let mut target = y_axis_linear.world_transform().unwrap();
println!("initial target pos = {:?}", target.translation);
println!("move x: {:?}", target_positions_vec[0]);
println!("move y: {:?}", target_positions_vec[1]);
target.translation.vector.x += target_positions_vec[0];
target.translation.vector.y += target_positions_vec[1];
// Inverse kinematics
let solver = k::JacobianIKSolver::default();
let arm = k::SerialChain::from_end(workpiece);
let mut constraints = k::Constraints::default();
constraints.rotation_x = false;
constraints.rotation_y = false;
constraints.rotation_z = false;
// constraints.position_x = false;
// constraints.position_y = false;
constraints.position_z = false;
solver
.solve_with_constraints(&arm, &target, &constraints)
.unwrap();
println!("solved angles = {:?}", chain.joint_positions());
chain.update_transforms();
let solved_pose = workpiece.world_transform().unwrap();
println!("solved target pos = {:?}", solved_pose.translation);
}
Hi,
I'm just learning your great crate k. I tried to build the example on README page. It crashed on
// chain.update_transforms();
let solved_pose = target_link.world_transform().unwrap();
Why do you put the line before it in comment?
I see you're loading URDF files primarily and k::Chain
certainly sounds like it doesn't support loops.
Are closed-loop kinematic systems in scope for this project? Do you know what it might take to implement support for them?
We're currently exploring SolveSpace and rapier, but neither seems exactly suited to our problem (solving forward kinematics for closed-loop n-bar systems).
I wanted to use k
for a forward kinematics solver for a human pose. In order to accomplish this I would need the ability to have ball joints, such as the joints on your hip and spine. Could this functionality be added? I know its possible by using three rotational joints but thinking about permutations of rotations, etc, is confusing to me and I'm not sure I'm up to the challenge as someone unfamiliar with all of this.
Hi,
I am still new to robotics and rust. I am trying to make a simple chain with 2 segments that are connected via three nodes. I am modifying the example code. I can't really seem to figure out what I am doing wrong. Sorry if this is a silly question. Is what I am trying to do even possible?
fn create_joint_with_link_array() -> k::Node<f32> {
let fixed: k::Node<f32> = JointBuilder::new()
.name("fixed")
.joint_type(JointType::Fixed)
.translation(Translation3::new(0.0, 0.0, 0.6))
.finalize()
.into();
let l0: k::Node<f32> = JointBuilder::new()
.name("shoulder_pitch")
.joint_type(JointType::Rotational {
axis: Vector3::y_axis(),
})
.translation(Translation3::new(0.0, 0.1, 0.0))
.finalize()
.into();
let l1: k::Node<f32> = JointBuilder::new()
.name("shoulder_roll")
.joint_type(JointType::Rotational {
axis: Vector3::x_axis(),
})
.translation(Translation3::new(0.0, 0.1, 0.0))
.finalize()
.into();
connect![fixed => l0 => l1];
fixed
}
fn create_cubes(window: &mut Window) -> Vec<SceneNode> {
let mut c_fixed = window.add_cube(0.05, 0.05, 0.05);
c_fixed.set_color(0.2, 0.2, 0.2);
let mut c0 = window.add_cube(0.1, 0.1, 0.1);
c0.set_color(1.0, 0.0, 1.0);
let mut c1 = window.add_cube(0.1, 0.1, 0.1);
c1.set_color(1.0, 0.0, 0.0);
vec![c_fixed, c0, c1]
}
fn main() {
let root = create_joint_with_link_array();
let arm = k::SerialChain::new_unchecked(k::Chain::from_root(root));
let mut window = Window::new("k ui");
window.set_light(Light::StickToCamera);
let mut cubes = create_cubes(&mut window);
let angles = vec![0.2, 0.2];
arm.set_joint_positions(&angles).unwrap();
let base_rot = Isometry3::from_parts(
Translation3::new(0.0, 0.0, -0.6),
UnitQuaternion::from_euler_angles(0.0, -1.57, -1.57),
);
arm.iter().next().unwrap().set_origin(
base_rot
* Isometry3::from_parts(
Translation3::new(1.0, 0.0, 0.6),
UnitQuaternion::from_euler_angles(0.5, 0.0, 0.0),
),
);
arm.update_transforms();
let end = arm.find("shoulder_roll").unwrap();
let mut target = end.world_transform().unwrap().clone();
let mut c_t = window.add_sphere(0.05);
c_t.set_color(1.0, 0.2, 0.2);
let eye = Point3::new(0.0f32, 1.0, 3.0);
let at = Point3::new(0.0f32, 0.2, 0.0);
let mut arc_ball = ArcBall::new(eye, at);
let solver = JacobianIKSolver::default();
let _ = create_ground(&mut window);
while window.render_with_camera(&mut arc_ball) {
for mut event in window.events().iter() {
match event.value {
WindowEvent::Key(code, Action::Release, _) => {
match code {
Key::R => {
// reset
arm.set_joint_positions(&angles).unwrap();
arm.update_transforms();
target = end.world_transform().unwrap().clone();
}
Key::Z => target.translation.vector[2] += 0.1,
Key::X => target.translation.vector[2] -= 0.1,
Key::Right => target.translation.vector[0] -= 0.1,
Key::Left => target.translation.vector[0] += 0.1,
Key::Up => target.translation.vector[1] += 0.1,
Key::Down => target.translation.vector[1] -= 0.1,
Key::S => {
// arm.set_joint_positions(&angles).unwrap();
println!("{:?}", arm.joint_positions());
},
_ => {}
}
event.inhibited = true // override the default keyboard handler
}
_ => {}
}
}
let mut constraints = k::Constraints::default();
constraints.rotation_x = false;
solver.solve(&arm, &target)
.unwrap_or_else(|err| {
println!("Err: {}", err);
});
// solver
// .solve_with_constraints(&arm, &target, &constraints)
//
c_t.set_local_transformation(target.clone());
for (i, trans) in arm.update_transforms().iter().enumerate() {
cubes[i].set_local_transformation(trans.clone());
}
}
}
error:
Err: ik precondition error "Input Dof=2, must be greater than 6"
Thanks!
Same as openrr/openrr#812.
If we follow the guide below,
https://github.com/rust-lang/rfcs/blob/master/text/0430-finalizing-naming-conventions.md
We should use Ik
instead of IK
. (UUID should be Uuid)
Sometimes we want to fix some joints when solving IK.
For example, don't use wheel base for small manipulation.
There is an iter_joints()
function, which works great for quickly iterating over all units -- but the lack of a mut
variant means that you can't repurpose your ref for writing back to the state. This complicates some use cases considerably -- e.g., a UI that lets you interactively probe the state of a limb and set poses.
Hey all,
I recently noticed in my code that I am returning a vector of NaN values when using the center_of_mass
function. The joint values I am specifying are all valid, and it seems to have transforms for all the links, so I am not sure exactly what might be happening. Here is the URDF I am using, in case that helps. Any ideas what might be causing this?
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from src/universal_robot/ur_description/urdf/ur3.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="ur3_robot">
<!-- links: main serial chain -->
<link name="base_link">
<visual>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0.0"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0.04"/>
<geometry>
<cylinder length="0.09" radius="0.065"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0030531654454" ixy="0.0" ixz="0.0" iyy="0.0030531654454" iyz="0.0" izz="0.005625"/>
</inertial>
</link>
<link name="shoulder_link">
<visual>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
<geometry>
<cylinder length="0.11" radius="0.065"/>
</geometry>
</collision>
<inertial>
<mass value="2.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.008093163429399999" ixy="0.0" ixz="0.0" iyy="0.008093163429399999" iyz="0.0" izz="0.005625"/>
</inertial>
</link>
<link name="upper_arm_link">
<visual>
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.12"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/upperarm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="-0.127 0 0.12"/>
<geometry>
<cylinder length="0.16" radius="0.05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 -1.5707963267948966" xyz="0 0 0.125"/>
<geometry>
<cylinder length="0.12" radius="0.05"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 -1.5707963267948966" xyz="-0.245 0 0.125"/>
<geometry>
<cylinder length="0.12" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="3.42"/>
<origin rpy="0 1.5707963267948966 0" xyz="-0.121825 0.0 0.12"/>
<inertia ixx="0.021728483221103233" ixy="0.0" ixz="0.0" iyy="0.021728483221103233" iyz="0.0" izz="0.00961875"/>
</inertial>
</link>
<link name="forearm_link">
<visual>
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.027"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="-0.07 0 0.027"/>
<geometry>
<cylinder length="0.21" radius="0.04"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 -1.5707963267948966" xyz="-0.215 0 0.027"/>
<geometry>
<cylinder length="0.095" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="1.26"/>
<origin rpy="0 1.5707963267948966 0" xyz="-0.1066 0.0 0.027"/>
<inertia ixx="0.0065445675821719194" ixy="0.0" ixz="0.0" iyy="0.0065445675821719194" iyz="0.0" izz="0.00354375"/>
</inertial>
</link>
<link name="wrist_1_link">
<visual>
<!-- TODO: Move this to a parameter -->
<origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.104"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.095" radius="0.035"/>
</geometry>
</collision>
<inertial>
<mass value="0.8"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.002084999166" ixy="0.0" ixz="0.0" iyy="0.002084999166" iyz="0.0" izz="0.00225"/>
</inertial>
</link>
<link name="wrist_2_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.08535"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.008 0.01"/>
<geometry>
<cylinder length="0.1" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.8"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.002084999166" ixy="0.0" ixz="0.0" iyy="0.002084999166" iyz="0.0" izz="0.00225"/>
</inertial>
</link>
<link name="wrist_3_link">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.01 -0.081"/>
<geometry>
<mesh filename="package://ur_description/meshes/ur3/visual/wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.01 -0.015"/>
<geometry>
<cylinder length="0.04" radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.35"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.02"/>
<inertia ixx="0.00013626661215999998" ixy="0.0" ixz="0.0" iyy="0.00013626661215999998" iyz="0.0" izz="0.0001792"/>
</inertial>
</link>
<!-- joints: main serial chain -->
<joint name="shoulder_pan_joint" type="revolute">
<parent link="base_link_inertia"/>
<child link="shoulder_link"/>
<origin rpy="0 0 0" xyz="0 0 0.1519"/>
<axis xyz="0 0 1"/>
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="shoulder_lift_joint" type="revolute">
<parent link="shoulder_link"/>
<child link="upper_arm_link"/>
<origin rpy="1.570796327 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="56.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.141592653589793"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin rpy="0 0 0" xyz="-0.24365 0 0"/>
<axis xyz="0 0 1"/>
<limit effort="28.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.141592653589793"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="wrist_1_joint" type="revolute">
<parent link="forearm_link"/>
<child link="wrist_1_link"/>
<origin rpy="0 0 0" xyz="-0.21325 0 0.11235"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="wrist_2_joint" type="revolute">
<parent link="wrist_1_link"/>
<child link="wrist_2_link"/>
<origin rpy="1.570796327 0 0" xyz="0 -0.08535 -1.750557762378351e-11"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
<dynamics damping="0" friction="0"/>
</joint>
<joint name="wrist_3_joint" type="revolute">
<parent link="wrist_2_link"/>
<child link="wrist_3_link"/>
<origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0819 -1.679797079540562e-11"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.283185307179586"/>
<dynamics damping="0" friction="0"/>
</joint>
<!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
<link name="base"/>
<joint name="base_link-base_fixed_joint" type="fixed">
<!-- Note the rotation over Z of pi radians: as base_link is REP-103
aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
to correctly align 'base' with the 'Base' coordinate system of
the UR controller.
-->
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base_link"/>
<child link="base"/>
</joint>
<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
<link name="flange"/>
<joint name="wrist_3-flange" type="fixed">
<parent link="wrist_3_link"/>
<child link="flange"/>
<origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
</joint>
<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
<link name="tool0"/>
<joint name="flange-tool0" type="fixed">
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
<origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0 0 0"/>
<parent link="flange"/>
<child link="tool0"/>
</joint>
<joint name="world_mapping" type="fixed">
<!-- default toolframe: X+ left, Y+ up, Z+ front -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
</robot>
It would be useful to be able to Serialize/Deserialize an entire Joint
, SerialChain
, etc, for easier usage in use cases involving persistence. As it stands today, even with the serde
feature enabled, k
can only serialize/deserialize a very small amount of the API's structs.
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.