Expanded Rust support

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"
+}

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"
+}

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"
+}

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": [

resources/pr2/pr2_spherized.urdf

@@ -2341,7 +2341,7 @@
     <parent link="r_gripper_palm_link" />
     <child link="r_gripper_l_finger_link" />
     <axis xyz="0. 0. 1." />
-    <origin xyz="0.07691 0.01 0.0" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.07691 0.01 0.0" rpy="0.0 0.0 0.548" />
   </joint>
   <joint name="r_gripper_r_finger_joint" type="fixed">
     <dynamics damping="0.02" />
@@ -2350,7 +2350,7 @@
     <parent link="r_gripper_palm_link" />
     <child link="r_gripper_r_finger_link" />
     <axis xyz=" 0.  0. -1." />
-    <origin xyz="0.07691 -0.01 0.0" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.07691 -0.01 0.0" rpy="0.0 0.0 -0.548" />
   </joint>
   <joint name="r_gripper_l_finger_tip_joint" type="fixed">
     <dynamics damping="0.001" />
@@ -2565,7 +2565,7 @@
     <parent link="l_gripper_palm_link" />
     <child link="l_gripper_l_finger_link" />
     <axis xyz="0. 0. 1." />
-    <origin xyz="0.07691 0.01 0.0" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.07691 0.01 0.0" rpy="0.0 0.0 0.548" />
   </joint>
   <joint name="l_gripper_r_finger_joint" type="fixed">
     <dynamics damping="0.02" />
@@ -2574,7 +2574,7 @@
     <parent link="l_gripper_palm_link" />
     <child link="l_gripper_r_finger_link" />
     <axis xyz=" 0.  0. -1." />
-    <origin xyz="0.07691 -0.01 0.0" rpy="0.0 0.0 0.0" />
+    <origin xyz="0.07691 -0.01 0.0" rpy="0.0 0.0 -0.548" />
   </joint>
   <joint name="l_gripper_l_finger_tip_joint" type="fixed">
     <dynamics damping="0.001" />

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"
+}

resources/templates/fk_template.rs

@@ -4,42 +4,204 @@
     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<const L: usize>(x: &super::ConfigurationBlock<L>, environment: &World3d<f32>) -> 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<const L: usize> = [Simd<f32, L>; {{name}}::DIM];
+
+impl Robot<DIM, f32> 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<const L: usize>(
+        &self,
+        cfgs: &Block<{ Self::DIM }, L, f32>,
+        spheres: &mut Vec<Ball<3, Simd<f32, L>>>,
+    ) {
+        sphere_fk(&cfgs.0, spheres);
+    }
+}
+
+impl<W> BlockValidate<DIM, f32, W> for {{name}}
+where
+    W: Collide3<f32>,
+{
+    fn is_valid<const L: usize>(&self, cfgs: &Block<{ Self::DIM }, L, f32>, world: &W) -> bool
+    where
+        Simd<f32, L>: SimdArithmetic<f32, L>,
+    {
+        fkcc(&cfgs.0, world)
+    }
+}
+
+impl Attach<DIM, f32> for {{name}}
+{
+    fn eefk<const L: usize>(&self, cfgs: &Block<DIM, L, f32>) -> Isometry<Simd<f32, L>, 3> {
+        eefk(&cfgs.0)
+    }
+}
+
+impl<W> AttachValidate<DIM, f32, W> for {{name}}
+where
+    W: Collide3<f32>,
+{
+    fn is_valid_attach<const L: usize>(
+        &self,
+        cfgs: &Block<DIM, L, f32>,
+        world: &W,
+        attachment: &mut PosedAttachment<f32, L>,
+    ) -> bool
+    where
+        Simd<f32, L>: crate::SimdArithmetic<f32, L>,
+    {
+        attach_fkcc(&cfgs.0, world, attachment)
+    }
+}
+
+
+fn fkcc<const L: usize>(x: &ConfigurationBlock<L>, environment: &impl Collide3<f32>) -> bool {
     let mut v = [Simd::splat(0.0); {{ccfk_code_vars}}];
     let mut y = [Simd::splat(0.0); {{ccfk_code_output}}];
 
     {{ccfk_code}}
     {% include "ccfk" %}
     true
 }
+
+
+fn attach_fkcc<const L: usize>(
+    x: &ConfigurationBlock<L>,
+    environment: &impl Collide3<f32>,
+    attachment: &mut PosedAttachment<f32, L>,
+) -> 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<const L: usize>(x: &ConfigurationBlock<L>, spheres: &mut Vec<Ball<3, Simd<f32, L>>>)
+{
+    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<const L: usize>(x: &ConfigurationBlock<L>) -> Isometry<Simd<f32, L>, 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)
+}

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"
+}