diff --git a/resources/baxter_rust.json b/resources/baxter_rust.json
new file mode 100644
index 0000000..7dde18a
--- /dev/null
+++ b/resources/baxter_rust.json
@@ -0,0 +1,13 @@
+{
+ "name": "Baxter",
+ "urdf": "baxter/baxter_spherized.urdf",
+ "srdf": "baxter/baxter.srdf",
+ "end_effector": "right_gripper",
+ "resolution": 64,
+ "template": "templates/fk_template.rs",
+ "subtemplates": [
+ { "name": "ccfk", "template": "templates/ccfk_template.rs" }
+ ],
+ "output": "baxter_fk.rs",
+ "language": "rust"
+}
diff --git a/resources/fetch_rust.json b/resources/fetch_rust.json
new file mode 100644
index 0000000..4db4a79
--- /dev/null
+++ b/resources/fetch_rust.json
@@ -0,0 +1,13 @@
+{
+ "name": "Fetch",
+ "urdf": "fetch/fetch_spherized.urdf",
+ "srdf": "fetch/fetch.srdf",
+ "end_effector": "gripper_link",
+ "resolution": 32,
+ "template": "templates/fk_template.rs",
+ "subtemplates": [
+ { "name": "ccfk", "template": "templates/ccfk_template.rs" }
+ ],
+ "output": "fetch_fk.rs",
+ "language": "rust"
+}
diff --git a/resources/panda_rust.json b/resources/panda_rust.json
new file mode 100644
index 0000000..4ffc44c
--- /dev/null
+++ b/resources/panda_rust.json
@@ -0,0 +1,13 @@
+{
+ "name": "Panda",
+ "urdf": "panda/panda_spherized.urdf",
+ "srdf": "panda/panda.srdf",
+ "end_effector": "panda_grasptarget",
+ "resolution": 32,
+ "template": "templates/fk_template.rs",
+ "subtemplates": [
+ { "name": "ccfk", "template": "templates/ccfk_template.rs" }
+ ],
+ "output": "panda_fk.rs",
+ "language": "rust"
+}
diff --git a/resources/pr2.json b/resources/pr2.json
index 776581c..bb3d9e5 100644
--- a/resources/pr2.json
+++ b/resources/pr2.json
@@ -2,7 +2,7 @@
"name": "Pr2",
"urdf": "pr2/pr2_spherized.urdf",
"srdf": "pr2/pr2.srdf",
- "end_effector": "r_gripper_palm_link",
+ "end_effector": "r_gripper_tool_frame",
"resolution": 32,
"template": "templates/fk_template.hh",
"subtemplates": [
diff --git a/resources/pr2/pr2_spherized.urdf b/resources/pr2/pr2_spherized.urdf
index ceed41f..c63d504 100644
--- a/resources/pr2/pr2_spherized.urdf
+++ b/resources/pr2/pr2_spherized.urdf
@@ -2341,7 +2341,7 @@
-
+
@@ -2350,7 +2350,7 @@
-
+
@@ -2565,7 +2565,7 @@
-
+
@@ -2574,7 +2574,7 @@
-
+
diff --git a/resources/pr2_rust.json b/resources/pr2_rust.json
new file mode 100644
index 0000000..5e61a91
--- /dev/null
+++ b/resources/pr2_rust.json
@@ -0,0 +1,13 @@
+{
+ "name": "Pr2",
+ "urdf": "pr2/pr2_spherized.urdf",
+ "srdf": "pr2/pr2_simple.srdf",
+ "end_effector": "r_gripper_tool_frame",
+ "resolution": 32,
+ "template": "templates/fk_template.rs",
+ "subtemplates": [
+ { "name": "ccfk", "template": "templates/ccfk_template.rs" }
+ ],
+ "output": "pr2_fk.rs",
+ "language": "rust"
+}
diff --git a/resources/templates/fk_template.rs b/resources/templates/fk_template.rs
index 1b277f4..426d780 100644
--- a/resources/templates/fk_template.rs
+++ b/resources/templates/fk_template.rs
@@ -4,38 +4,116 @@
clippy::unreadable_literal,
clippy::collapsible_if,
clippy::excessive_precision,
- clippy::suspicious_operation_groupings
)]
+#![allow(clippy::assign_op_pattern)]
+#![feature(portable_simd)]
use core::simd::Simd;
use carom_core::{
- cos, env::World3d, sin, sphere_environment_in_collision, sphere_sphere_self_collision,
+ Attach, AttachValidate, Ball, Block, BlockValidate, Collide3, PosedAttachment, Robot, SimdArithmetic, cos, sin,
+ sphere_environment_in_collision, sphere_sphere_self_collision, Isometry,
};
-pub const DIM: usize = {{n_q}};
+#[derive(Clone, Copy, Debug)]
+pub struct {{name}};
-pub const JOINT_NAMES: [&str; DIM] = ["{{join(joint_names, "\", \"")}}"];
-pub const END_EFFECTOR_NAME: &str = "{{end_effector}}";
+const DIM: usize = {{n_q}};
-pub const BOUND_LOWER: [f32; DIM] = [{{join(bound_lower, ", ")}}];
+impl {{name}} {
+ /// The configuration-space dimension of this robot.
+ pub const DIM: usize = DIM;
-pub const BOUND_SCALE: [f32; DIM] = [{{join(bound_range, ", ")}}];
+ pub const BOUNDS: [[f32; DIM]; 2] = [
+ Self::BOUND_LOWER,
+ make_upper(Self::BOUND_LOWER, Self::BOUND_SCALE),
+ ];
-pub const RESOLUTION: usize = {{resolution}};
+ pub const STEP_SIZE: f32 = 1.0 / {{resolution}}f32;
-pub const MIN_RADIUS: f32 = {{min_radius}};
-pub const MAX_RADIUS: f32 = {{max_radius}};
+ pub const JOINT_NAMES: [&str; DIM] = ["{{join(joint_names, "\", \"")}}"];
+ pub const END_EFFECTOR_NAME: &str = "{{end_effector}}";
-#[expect(
- clippy::too_many_lines,
- clippy::cognitive_complexity,
- clippy::unreadable_literal,
- clippy::collapsible_if,
- clippy::excessive_precision,
- clippy::suspicious_operation_groupings
-)]
-pub fn fkcc(x: &super::ConfigurationBlock, environment: &World3d) -> bool {
+ const BOUND_LOWER: [f32; DIM] = [{{join(bound_lower, ", ")}}];
+ const BOUND_SCALE: [f32; DIM] = [{{join(bound_range, ", ")}}];
+
+ pub const MIN_RADIUS: f32 = {{min_radius}};
+ pub const MAX_RADIUS: f32 = {{max_radius}};
+}
+
+const fn make_upper(lower: [f32; DIM], scale: [f32; DIM]) -> [f32; DIM] {
+ let mut ret = [0.0; DIM];
+ let mut i = 0;
+ while i < DIM {
+ ret[i] = lower[i] + scale[i];
+ i += 1;
+ }
+ ret
+}
+
+type ConfigurationBlock = [Simd; {{name}}::DIM];
+
+impl Robot for {{name}}
+{
+ fn bounds(&self) -> [[f32; DIM]; 2] {
+ Self::BOUNDS
+ }
+
+ fn name(&self) -> &'static str {
+ "{{lower(name)}}"
+ }
+
+ fn joint_names(&self) -> &[&str; DIM] {
+ &Self::JOINT_NAMES
+ }
+
+ fn sphere_fk(
+ &self,
+ cfgs: &Block<{ Self::DIM }, L, f32>,
+ spheres: &mut Vec>>,
+ ) {
+ sphere_fk(&cfgs.0, spheres);
+ }
+}
+
+impl BlockValidate for {{name}}
+where
+ W: Collide3,
+{
+ fn is_valid(&self, cfgs: &Block<{ Self::DIM }, L, f32>, world: &W) -> bool
+ where
+ Simd: SimdArithmetic,
+ {
+ fkcc(&cfgs.0, world)
+ }
+}
+
+impl Attach for {{name}}
+{
+ fn eefk(&self, cfgs: &Block) -> Isometry, 3> {
+ eefk(&cfgs.0)
+ }
+}
+
+impl AttachValidate for {{name}}
+where
+ W: Collide3,
+{
+ fn is_valid_attach(
+ &self,
+ cfgs: &Block,
+ world: &W,
+ attachment: &mut PosedAttachment,
+ ) -> bool
+ where
+ Simd: crate::SimdArithmetic,
+ {
+ attach_fkcc(&cfgs.0, world, attachment)
+ }
+}
+
+
+fn fkcc(x: &ConfigurationBlock, environment: &impl Collide3) -> bool {
let mut v = [Simd::splat(0.0); {{ccfk_code_vars}}];
let mut y = [Simd::splat(0.0); {{ccfk_code_output}}];
@@ -43,3 +121,87 @@ pub fn fkcc(x: &super::ConfigurationBlock, environment: &Worl
{% include "ccfk" %}
true
}
+
+
+fn attach_fkcc(
+ x: &ConfigurationBlock,
+ environment: &impl Collide3,
+ attachment: &mut PosedAttachment,
+) -> bool {
+ let mut v = [Simd::splat(0.0); {{ccfkee_code_vars}}];
+ let mut y = [Simd::splat(0.0); {{ccfkee_code_output}}];
+
+ {{ccfkee_code}}
+ {% include "ccfk" %}
+
+ // attaching at {{ end_effector }}
+ attachment.set_pose(&Isometry::from_carom_buf(
+ *y[{{ccfkee_code_output - 12}}..{{ccfkee_code_output}}].as_array().unwrap()
+ ));
+
+ //
+ // attachment vs. environment collisions
+ //
+ if attachment.environment_collision(environment)
+ {
+ return false;
+ }
+
+ //
+ // attachment vs. robot collisions
+ //
+
+ {% for i in range(length(end_effector_collisions)) %}
+ {% set link_index = at(end_effector_collisions, i) %}
+ {% set link_bs = at(bounding_sphere_index, link_index) %}
+ {% set link_spheres = at(per_link_spheres, link_index) %}
+
+ // Attachment vs. {{ at(link_names, link_index )}}
+ if attachment.sphere_collision(
+ y[{{(n_spheres + link_bs) * 4 + 0}}],
+ y[{{(n_spheres + link_bs) * 4 + 1}}],
+ y[{{(n_spheres + link_bs) * 4 + 2}}],
+ y[{{(n_spheres + link_bs) * 4 + 3}}]
+ ) {
+ {% for j in range(length(link_spheres)) %}
+ {% set sphere_index = at(link_spheres, j) %}
+ if attachment.sphere_collision(
+ y[{{sphere_index * 4 + 0}}],
+ y[{{sphere_index * 4 + 1}}],
+ y[{{sphere_index * 4 + 2}}],
+ y[{{sphere_index * 4 + 3}}]
+ ) {
+ return false;
+ }
+ {% endfor %}
+ }
+ {% endfor %}
+
+ true
+}
+
+fn sphere_fk(x: &ConfigurationBlock, spheres: &mut Vec>>)
+{
+ let mut v = [Simd::splat(0.0); {{spherefk_code_vars}}];
+ let mut y = [Simd::splat(0.0); {{spherefk_code_output}}];
+
+ {{spherefk_code}}
+
+ for &[x, y, z, r] in y.as_chunks().0 {
+ spheres.push(Ball {
+ pos: [x, y, z],
+ r,
+ });
+ }
+}
+
+
+fn eefk(x: &ConfigurationBlock) -> Isometry, 3>
+{
+ let mut v = [Simd::splat(0.0); {{eefk_code_vars}}];
+ let mut y = [Simd::splat(0.0); {{eefk_code_output}}];
+
+ {{eefk_code}}
+
+ Isometry::from_carom_buf(y)
+}
diff --git a/resources/ur10e_rust.json b/resources/ur10e_rust.json
new file mode 100644
index 0000000..b8e3c89
--- /dev/null
+++ b/resources/ur10e_rust.json
@@ -0,0 +1,12 @@
+{
+ "name": "Ur10e",
+ "urdf": "ur10e/ur_10e_spherized.urdf",
+ "end_effector": "robotiq_85_base_link",
+ "resolution": 32,
+ "template": "templates/fk_template.rs",
+ "subtemplates": [
+ { "name": "ccfk", "template": "templates/ccfk_template.rs" }
+ ],
+ "output": "ur10e_fk.rs",
+ "language": "rust"
+}
diff --git a/resources/ur5/ur5_spherized_no_offset.urdf b/resources/ur5/ur5_spherized_no_offset.urdf
new file mode 100644
index 0000000..d0cdb21
--- /dev/null
+++ b/resources/ur5/ur5_spherized_no_offset.urdf
@@ -0,0 +1,758 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/resources/ur5_rust.json b/resources/ur5_rust.json
new file mode 100644
index 0000000..d16cbc2
--- /dev/null
+++ b/resources/ur5_rust.json
@@ -0,0 +1,13 @@
+{
+ "name": "Ur5",
+ "urdf": "ur5/ur5_spherized_no_offset.urdf",
+ "srdf": "ur5/ur5.srdf",
+ "end_effector": "robotiq_85_base_link",
+ "resolution": 32,
+ "template": "templates/fk_template.rs",
+ "subtemplates": [
+ { "name": "ccfk", "template": "templates/ccfk_template.rs" }
+ ],
+ "output": "ur5_fk.rs",
+ "language": "rust"
+}