rapier3d_urdf/lib.rs
1//! ## URDF loader for the Rapier physics engine
2//!
3//! Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. The `rapier3d-urdf`
4//! crate lets you convert an URDF file into a set of rigid-bodies, colliders, and joints, for usage with the
5//! `rapier3d` physics engine.
6//!
7//! ## Optional cargo features
8//!
9//! - `stl`: enables loading STL meshes referenced by the URDF file.
10//! - `collada`: enables loading Collada (`.dae`) meshes referenced by the URDF file.
11//! - `wavefront`: enables loading Wavefront (`.obj`) meshes referenced by the URDF file.
12//!
13//! ## Limitations
14//!
15//! Are listed below some known limitations you might want to be aware of before picking this library. Contributions to
16//! improve
17//! these elements are very welcome!
18//!
19//! - Mesh file types are limited. Contributions are welcome. You may check the `rapier3d-meshloader`
20//! repository for an example of mesh loader.
21//! - When inserting joints as multibody joints, they will be reset to their neutral position (all coordinates = 0).
22//! - The following fields are currently ignored:
23//! - `Joint::dynamics`
24//! - `Joint::limit.effort` / `limit.velocity`
25//! - `Joint::mimic`
26//! - `Joint::safety_controller`
27
28#![warn(missing_docs)]
29
30use na::RealField;
31use rapier3d::{
32 dynamics::{
33 GenericJoint, GenericJointBuilder, ImpulseJointHandle, ImpulseJointSet, JointAxesMask,
34 JointAxis, MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBody,
35 RigidBodyBuilder, RigidBodyHandle, RigidBodySet, RigidBodyType,
36 },
37 geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet, SharedShape, TriMeshFlags},
38 glamx::EulerRot,
39 math::{Pose, Real, Rotation, Vector},
40 na,
41};
42use std::collections::HashMap;
43use std::path::Path;
44use urdf_rs::{Geometry, Inertial, Joint, Pose as UrdfPose, Robot};
45
46#[cfg(doc)]
47use rapier3d::dynamics::Multibody;
48
49bitflags::bitflags! {
50 /// Options applied to multibody joints created from the URDF joints.
51 #[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
52 pub struct UrdfMultibodyOptions: u8 {
53 /// If this flag is set, the created multibody joint will be marked as kinematic.
54 ///
55 /// A kinematic joint is entirely controlled by the user (it is not affected by any force).
56 /// This particularly useful if you intend to control the robot through inverse-kinematics.
57 const JOINTS_ARE_KINEMATIC = 0b0001;
58 /// If enabled, any contact between two links belonging to the same generated multibody robot will
59 /// be ignored.
60 ///
61 /// This is useful if the generated colliders are known to be overlapping (e.g. if creating colliders
62 /// from visual meshes was enabled) or that collision detection is not needed a computationally
63 /// expensive (e.g. if any of these colliders is a high-quality triangle mesh).
64 const DISABLE_SELF_CONTACTS = 0b0010;
65 }
66}
67
68/// The index of an urdf link.
69pub type LinkId = usize;
70
71/// A set of configurable options for loading URDF files.
72#[derive(Clone, Debug)]
73pub struct UrdfLoaderOptions {
74 /// If `true` one collider will be created for each **collision** shape from the urdf file (default: `true`).
75 pub create_colliders_from_collision_shapes: bool,
76 /// If `true` one collider will be created for each **visual** shape from the urdf file (default: `false`).
77 ///
78 /// Note that visual shapes are usually significantly higher-resolution than collision shapes.
79 /// Most of the time they might also overlap, or generate a lot of contacts due to them being
80 /// thin triangle meshes.
81 ///
82 /// So if this option is set to `true`, it is recommended to also keep
83 /// [`UrdfLoaderOptions::enable_joint_collisions`] set to `false`. If the model is then added
84 /// to the physics sets using multibody joints, it is recommended to call
85 /// [`UrdfRobot::insert_using_multibody_joints`] with the [`UrdfMultibodyOptions::DISABLE_SELF_CONTACTS`]
86 /// flag enabled.
87 pub create_colliders_from_visual_shapes: bool,
88 /// If `true`, the mass properties (center-of-mass, mass, and angular inertia) read from the urdf
89 /// file will be added to the corresponding rigid-body (default: `true`).
90 ///
91 /// Note that by default, all colliders created will be given a density of 0.0, meaning that,
92 /// by default, the imported mass properties are the only ones added to the created rigid-bodies.
93 /// To give colliders a non-zero density, see [`UrdfLoaderOptions::collider_blueprint`].
94 pub apply_imported_mass_props: bool,
95 /// If `true`, collisions between two links sharing a joint will be disabled (default: `false`).
96 ///
97 /// It is strongly recommended to leave this to `false` unless you are certain adjacent links
98 /// colliders don’t overlap.
99 pub enable_joint_collisions: bool,
100 /// If `true`, the rigid-body at the root of the kinematic chains will be initialized as [`RigidBodyType::Fixed`]
101 /// (default: `false`).
102 pub make_roots_fixed: bool,
103 /// This is the set of flags set on all the loaded triangle meshes (default: [`TriMeshFlags::all`]).
104 ///
105 /// Note that the default enables all the flags. This is operating under the assumption that the provided
106 /// mesh are generally well-formed and properly oriented (2-manifolds with outward normals).
107 pub trimesh_flags: TriMeshFlags,
108 /// The transform appended to every created rigid-bodies (default: `Pose::IDENTITY`).
109 pub shift: Pose,
110 /// A description of the collider properties that need to be applied to every collider created
111 /// by the loader (default: `ColliderBuilder::default().density(0.0)`).
112 ///
113 /// This collider builder will be used for initializing every collider created by the loader.
114 /// The shape specified by this builder isn’t important and will be replaced by the shape read
115 /// from the urdf file.
116 ///
117 /// Note that by default, the collider is given a density of 0.0 so that it doesn’t contribute
118 /// to its parent rigid-body’s mass properties (since they should be already provided by the
119 /// urdf file assuming the [`UrdfLoaderOptions::apply_imported_mass_props`] wasn’t set `false`).
120 pub collider_blueprint: ColliderBuilder,
121 /// A description of the rigid-body properties that need to be applied to every rigid-body
122 /// created by the loader (default: `RigidBodyBuilder::dynamic()`).
123 ///
124 /// This rigid-body builder will be used for initializing every rigid-body created by the loader.
125 /// The rigid-body type is not important as it will always be set to [`RigidBodyType::Dynamic`]
126 /// for non-root links. Root links will be set to [`RigidBodyType::Fixed`] instead of
127 /// [`RigidBodyType::Dynamic`] if the [`UrdfLoaderOptions::make_roots_fixed`] is set to `true`.
128 pub rigid_body_blueprint: RigidBodyBuilder,
129}
130
131impl Default for UrdfLoaderOptions {
132 fn default() -> Self {
133 Self {
134 create_colliders_from_collision_shapes: true,
135 create_colliders_from_visual_shapes: false,
136 apply_imported_mass_props: true,
137 enable_joint_collisions: false,
138 make_roots_fixed: false,
139 trimesh_flags: TriMeshFlags::all(),
140 shift: Pose::IDENTITY,
141 collider_blueprint: ColliderBuilder::default().density(0.0),
142 rigid_body_blueprint: RigidBodyBuilder::dynamic(),
143 }
144 }
145}
146
147/// An urdf link loaded as a rapier [`RigidBody`] and its [`Collider`]s.
148#[derive(Clone, Debug)]
149pub struct UrdfLink {
150 /// The rigid-body created for this link.
151 pub body: RigidBody,
152 /// All the colliders build from the URDF visual and/or collision shapes (if the corresponding
153 /// [`UrdfLoaderOptions`] option is enabled).
154 pub colliders: Vec<Collider>,
155}
156
157/// An urdf joint loaded as a rapier [`GenericJoint`].
158#[derive(Clone, Debug)]
159pub struct UrdfJoint {
160 /// The rapier version for the corresponding urdf joint.
161 pub joint: GenericJoint,
162 /// Index of the rigid-body (from the [`UrdfRobot`] array) at the first
163 /// endpoint of this joint.
164 pub link1: LinkId,
165 /// Index of the rigid-body (from the [`UrdfRobot`] array) at the second
166 /// endpoint of this joint.
167 pub link2: LinkId,
168}
169
170/// A robot represented as a set of rapier rigid-bodies, colliders, and joints.
171#[derive(Clone, Debug)]
172pub struct UrdfRobot {
173 /// The bodies and colliders loaded from the urdf file.
174 ///
175 /// This vector matches the order of [`Robot::links`].
176 pub links: Vec<UrdfLink>,
177 /// The joints loaded from the urdf file.
178 ///
179 /// This vector matches the order of [`Robot::joints`].
180 pub joints: Vec<UrdfJoint>,
181}
182
183/// Handle of a joint read from the URDF file and inserted into rapier’s `ImpulseJointSet`
184/// or a `MultibodyJointSet`.
185pub struct UrdfJointHandle<JointHandle> {
186 /// The inserted joint handle.
187 pub joint: JointHandle,
188 /// The handle of the first rigid-body attached by this joint.
189 pub link1: RigidBodyHandle,
190 /// The handle of the second rigid-body attached by this joint.
191 pub link2: RigidBodyHandle,
192}
193
194/// The handles related to a link read from the URDF file and inserted into Rapier’s
195/// `RigidBodySet` and `ColliderSet`.
196pub struct UrdfLinkHandle {
197 /// Handle of the inserted link’s rigid-body.
198 pub body: RigidBodyHandle,
199 /// Handle of the colliders attached to [`Self::body`].
200 pub colliders: Vec<ColliderHandle>,
201}
202
203/// Handles to all the Rapier objects created when inserting this robot into Rapier’s
204/// `RigidBodySet`, `ColliderSet`, `ImpulseJointSet`, `MultibodyJointSet`.
205pub struct UrdfRobotHandles<JointHandle> {
206 /// The handles related to each URDF robot link.
207 pub links: Vec<UrdfLinkHandle>,
208 /// The handles related to each URDF robot joint.
209 pub joints: Vec<UrdfJointHandle<JointHandle>>,
210}
211
212impl UrdfRobot {
213 /// Parses a URDF file and returns both the rapier objects (`UrdfRobot`) and the original urdf
214 /// structures (`Robot`). Both structures are arranged the same way, with matching indices for each part.
215 ///
216 /// If the URDF file references external meshes, they will be loaded automatically if the format
217 /// is supported. The format is detected from the file’s extension. All the mesh formats are
218 /// disabled by default and can be enabled through cargo features (e.g. the `stl` feature of
219 /// this crate enabled loading referenced meshes in stl format).
220 ///
221 /// # Parameters
222 /// - `path`: the path of the URDF file.
223 /// - `options`: customize the creation of rapier objects from the URDF description.
224 /// - `mesh_dir`: the base directory containing the meshes referenced by the URDF file. When
225 /// a mesh reference is found in the URDF file, this `mesh_dir` is appended
226 /// to the file path. If `mesh_dir` is `None` then the mesh directory is assumed
227 /// to be the same directory as the one containing the URDF file.
228 pub fn from_file(
229 path: impl AsRef<Path>,
230 options: UrdfLoaderOptions,
231 mesh_dir: Option<&Path>,
232 ) -> anyhow::Result<(Self, Robot)> {
233 let path = path.as_ref().canonicalize()?;
234 let mesh_dir = mesh_dir
235 .or_else(|| path.parent())
236 .unwrap_or_else(|| Path::new("./"));
237 let robot = urdf_rs::read_file(&path)?;
238 Ok((Self::from_robot(&robot, options, mesh_dir), robot))
239 }
240
241 /// Parses a string in URDF format and returns both the rapier objects (`UrdfRobot`) and the original urdf
242 /// structures (`Robot`). Both structures are arranged the same way, with matching indices for each part.
243 ///
244 /// If the URDF file references external meshes, they will be loaded automatically if the format
245 /// is supported. The format is detected from the file’s extension. All the mesh formats are
246 /// disabled by default and can be enabled through cargo features (e.g. the `stl` feature of
247 /// this crate enabled loading referenced meshes in stl format).
248 ///
249 /// # Parameters
250 /// - `str`: the string content of an URDF file.
251 /// - `options`: customize the creation of rapier objects from the URDF description.
252 /// - `mesh_dir`: the base directory containing the meshes referenced by the URDF file. When
253 /// a mesh reference is found in the URDF file, this `mesh_dir` is appended
254 /// to the file path.
255 pub fn from_str(
256 str: &str,
257 options: UrdfLoaderOptions,
258 mesh_dir: &Path,
259 ) -> anyhow::Result<(Self, Robot)> {
260 let robot = urdf_rs::read_from_string(str)?;
261 Ok((Self::from_robot(&robot, options, mesh_dir), robot))
262 }
263
264 /// From an already loaded urdf file as a `Robot`, this creates the matching rapier objects
265 /// (`UrdfRobot`). Both structures are arranged the same way, with matching indices for each part.
266 ///
267 /// If the URDF file references external meshes, they will be loaded automatically if the format
268 /// is supported. The format is detected mostly from the file’s extension. All the mesh formats are
269 /// disabled by default and can be enabled through cargo features (e.g. the `stl` feature of
270 /// this crate enabled loading referenced meshes in stl format).
271 ///
272 /// # Parameters
273 /// - `robot`: the robot loaded from an URDF file.
274 /// - `options`: customize the creation of rapier objects from the URDF description.
275 /// - `mesh_dir`: the base directory containing the meshes referenced by the URDF file. When
276 /// a mesh reference is found in the URDF file, this `mesh_dir` is appended
277 /// to the file path.
278 pub fn from_robot(robot: &Robot, options: UrdfLoaderOptions, mesh_dir: &Path) -> Self {
279 let mut name_to_link_id = HashMap::new();
280 let mut link_is_root = vec![true; robot.links.len()];
281 let mut links: Vec<_> = robot
282 .links
283 .iter()
284 .enumerate()
285 .map(|(id, link)| {
286 name_to_link_id.insert(&link.name, id);
287 let mut colliders = vec![];
288 if options.create_colliders_from_collision_shapes {
289 colliders.extend(link.collision.iter().flat_map(|co| {
290 urdf_to_colliders(&options, mesh_dir, &co.geometry, &co.origin)
291 }))
292 }
293 if options.create_colliders_from_visual_shapes {
294 colliders.extend(link.visual.iter().flat_map(|vis| {
295 urdf_to_colliders(&options, mesh_dir, &vis.geometry, &vis.origin)
296 }))
297 }
298 let mut body = urdf_to_rigid_body(&options, &link.inertial);
299 let new_pos = options.shift * body.position();
300 body.set_position(new_pos, false);
301 UrdfLink { body, colliders }
302 })
303 .collect();
304 let joints: Vec<_> = robot
305 .joints
306 .iter()
307 .map(|joint| {
308 let link1 = name_to_link_id[&joint.parent.link];
309 let link2 = name_to_link_id[&joint.child.link];
310 let pose1 = *links[link1].body.position();
311 let rb2 = &mut links[link2].body;
312 let joint = urdf_to_joint(&options, joint, &pose1, rb2);
313 link_is_root[link2] = false;
314
315 UrdfJoint {
316 joint,
317 link1,
318 link2,
319 }
320 })
321 .collect();
322
323 if options.make_roots_fixed {
324 for (link, is_root) in links.iter_mut().zip(link_is_root.into_iter()) {
325 if is_root {
326 link.body.set_body_type(RigidBodyType::Fixed, false)
327 }
328 }
329 }
330
331 Self { links, joints }
332 }
333
334 /// Inserts all the robots elements to the rapier rigid-body, collider, and impulse joint, sets.
335 ///
336 /// Joints are represented as impulse joints. This implies that joint constraints are simulated
337 /// in full coordinates using impulses. For a reduced-coordinates approach, see
338 /// [`UrdfRobot::insert_using_multibody_joints`].
339 pub fn insert_using_impulse_joints(
340 self,
341 rigid_body_set: &mut RigidBodySet,
342 collider_set: &mut ColliderSet,
343 joint_set: &mut ImpulseJointSet,
344 ) -> UrdfRobotHandles<ImpulseJointHandle> {
345 let links: Vec<_> = self
346 .links
347 .into_iter()
348 .map(|link| {
349 let body = rigid_body_set.insert(link.body);
350 let colliders = link
351 .colliders
352 .into_iter()
353 .map(|co| collider_set.insert_with_parent(co, body, rigid_body_set))
354 .collect();
355 UrdfLinkHandle { body, colliders }
356 })
357 .collect();
358 let joints: Vec<_> = self
359 .joints
360 .into_iter()
361 .map(|joint| {
362 let link1 = links[joint.link1].body;
363 let link2 = links[joint.link2].body;
364 let joint = joint_set.insert(link1, link2, joint.joint, false);
365 UrdfJointHandle {
366 joint,
367 link1,
368 link2,
369 }
370 })
371 .collect();
372
373 UrdfRobotHandles { links, joints }
374 }
375
376 /// Inserts all the robots elements to the rapier rigid-body, collider, and multibody joint, sets.
377 ///
378 /// Joints are represented as multibody joints. This implies that the robot as a whole can be
379 /// accessed as a single [`Multibody`] from the [`MultibodyJointSet`]. That multibody uses reduced
380 /// coordinates for modeling joints, meaning that it will be very close to the way they are usually
381 /// represented for robotics applications. Multibodies also support inverse kinematics.
382 pub fn insert_using_multibody_joints(
383 self,
384 rigid_body_set: &mut RigidBodySet,
385 collider_set: &mut ColliderSet,
386 joint_set: &mut MultibodyJointSet,
387 multibody_options: UrdfMultibodyOptions,
388 ) -> UrdfRobotHandles<Option<MultibodyJointHandle>> {
389 let links: Vec<_> = self
390 .links
391 .into_iter()
392 .map(|link| {
393 let body = rigid_body_set.insert(link.body);
394 let colliders = link
395 .colliders
396 .into_iter()
397 .map(|co| collider_set.insert_with_parent(co, body, rigid_body_set))
398 .collect();
399 UrdfLinkHandle { body, colliders }
400 })
401 .collect();
402 let joints: Vec<_> = self
403 .joints
404 .into_iter()
405 .map(|joint| {
406 let link1 = links[joint.link1].body;
407 let link2 = links[joint.link2].body;
408 let joint =
409 if multibody_options.contains(UrdfMultibodyOptions::JOINTS_ARE_KINEMATIC) {
410 joint_set.insert_kinematic(link1, link2, joint.joint, false)
411 } else {
412 joint_set.insert(link1, link2, joint.joint, false)
413 };
414
415 if let Some(joint) = joint {
416 let (multibody, _) = joint_set.get_mut(joint).unwrap_or_else(|| unreachable!());
417 multibody.set_self_contacts_enabled(
418 !multibody_options.contains(UrdfMultibodyOptions::DISABLE_SELF_CONTACTS),
419 );
420 }
421
422 UrdfJointHandle {
423 joint,
424 link1,
425 link2,
426 }
427 })
428 .collect();
429
430 UrdfRobotHandles { links, joints }
431 }
432
433 /// Appends a transform to all the rigid-bodie of this robot.
434 pub fn append_transform(&mut self, transform: &Pose) {
435 for link in &mut self.links {
436 let new_pos = transform * link.body.position();
437 link.body.set_position(new_pos, true);
438 }
439 }
440}
441
442#[rustfmt::skip]
443fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> RigidBody {
444 let origin = urdf_to_pose(&inertial.origin);
445 let mut builder = options.rigid_body_blueprint.clone();
446 builder.body_type = RigidBodyType::Dynamic;
447
448 if options.apply_imported_mass_props {
449 builder = builder.additional_mass_properties(MassProperties::with_inertia_matrix(
450 origin.translation,
451 inertial.mass.value as Real,
452 // See http://wiki.ros.org/urdf/Tutorials/Adding%20Physical%20and%20Collision%20Properties%20to%20a%20URDF%20Model#Inertia
453 na::Matrix3::new(
454 inertial.inertia.ixx as Real, inertial.inertia.ixy as Real, inertial.inertia.ixz as Real,
455 inertial.inertia.ixy as Real, inertial.inertia.iyy as Real, inertial.inertia.iyz as Real,
456 inertial.inertia.ixz as Real, inertial.inertia.iyz as Real,inertial.inertia.izz as Real,
457 ).into(),
458 ))
459 }
460
461 builder.build()
462}
463
464fn urdf_to_colliders(
465 options: &UrdfLoaderOptions,
466 _mesh_dir: &Path, // Unused if none of the meshloader features is enabled.
467 geometry: &Geometry,
468 origin: &UrdfPose,
469) -> Vec<Collider> {
470 let mut shape_transform = Pose::IDENTITY;
471
472 let mut colliders = Vec::new();
473
474 match &geometry {
475 Geometry::Box { size } => {
476 colliders.push(SharedShape::cuboid(
477 size[0] as Real / 2.0,
478 size[1] as Real / 2.0,
479 size[2] as Real / 2.0,
480 ));
481 }
482 Geometry::Cylinder { radius, length } => {
483 // This rotation will make the cylinder Z-up as per the URDF spec,
484 // instead of rapier’s default Y-up.
485 shape_transform = Pose::rotation(Vector::X * Real::frac_pi_2());
486 colliders.push(SharedShape::cylinder(
487 *length as Real / 2.0,
488 *radius as Real,
489 ));
490 }
491 Geometry::Capsule { radius, length } => {
492 colliders.push(SharedShape::capsule_z(
493 *length as Real / 2.0,
494 *radius as Real,
495 ));
496 }
497 Geometry::Sphere { radius } => {
498 colliders.push(SharedShape::ball(*radius as Real));
499 }
500 #[cfg(not(feature = "__meshloader_is_enabled"))]
501 Geometry::Mesh { .. } => {
502 log::error!(
503 "Mesh loading is disabled by default. Enable one of the format features (`stl`, `collada`, `wavefront`) of `rapier3d-urdf` for mesh support."
504 );
505 }
506 #[cfg(feature = "__meshloader_is_enabled")]
507 Geometry::Mesh { filename, scale } => {
508 let full_path = _mesh_dir.join(filename);
509 let scale = scale
510 .map(|s| Vector::new(s[0] as Real, s[1] as Real, s[2] as Real))
511 .unwrap_or_else(|| Vector::splat(1.0));
512
513 let Ok(loaded_mesh) = rapier3d_meshloader::load_from_path(
514 full_path,
515 &rapier3d::prelude::MeshConverter::TriMeshWithFlags(options.trimesh_flags),
516 scale,
517 ) else {
518 return Vec::new();
519 };
520 colliders.append(
521 &mut loaded_mesh
522 .into_iter()
523 .filter_map(|x| x.map(|s| s.shape).ok())
524 .collect(),
525 );
526 }
527 }
528
529 colliders
530 .drain(..)
531 .map(move |shape| {
532 let mut builder = options.collider_blueprint.clone();
533 builder.shape = shape;
534 builder
535 .position(urdf_to_pose(origin) * shape_transform)
536 .build()
537 })
538 .collect()
539}
540
541fn urdf_to_pose(pose: &UrdfPose) -> Pose {
542 Pose::from_parts(
543 Vector::new(
544 pose.xyz[0] as Real,
545 pose.xyz[1] as Real,
546 pose.xyz[2] as Real,
547 ),
548 Rotation::from_euler(
549 EulerRot::XYZ,
550 pose.rpy[0] as Real,
551 pose.rpy[1] as Real,
552 pose.rpy[2] as Real,
553 ),
554 )
555}
556
557fn urdf_to_joint(
558 options: &UrdfLoaderOptions,
559 joint: &Joint,
560 pose1: &Pose,
561 link2: &mut RigidBody,
562) -> GenericJoint {
563 let locked_axes = match joint.joint_type {
564 urdf_rs::JointType::Fixed => JointAxesMask::LOCKED_FIXED_AXES,
565 urdf_rs::JointType::Continuous | urdf_rs::JointType::Revolute => {
566 JointAxesMask::LOCKED_REVOLUTE_AXES
567 }
568 urdf_rs::JointType::Floating => JointAxesMask::empty(),
569 urdf_rs::JointType::Planar => JointAxesMask::ANG_AXES | JointAxesMask::LIN_X,
570 urdf_rs::JointType::Prismatic => JointAxesMask::LOCKED_PRISMATIC_AXES,
571 urdf_rs::JointType::Spherical => JointAxesMask::LOCKED_SPHERICAL_AXES,
572 };
573 let joint_to_parent = urdf_to_pose(&joint.origin);
574 let joint_axis = Vector::new(
575 joint.axis.xyz[0] as Real,
576 joint.axis.xyz[1] as Real,
577 joint.axis.xyz[2] as Real,
578 )
579 .normalize_or_zero();
580
581 link2.set_position(pose1 * joint_to_parent, false);
582
583 let mut builder = GenericJointBuilder::new(locked_axes)
584 .local_anchor1(joint_to_parent.translation)
585 .contacts_enabled(options.enable_joint_collisions);
586
587 if joint_axis != Vector::ZERO {
588 builder = builder
589 .local_axis1(joint_to_parent.rotation * joint_axis)
590 .local_axis2(joint_axis);
591 }
592
593 match joint.joint_type {
594 urdf_rs::JointType::Prismatic => {
595 builder = builder.limits(
596 JointAxis::LinX,
597 [joint.limit.lower as Real, joint.limit.upper as Real],
598 )
599 }
600 urdf_rs::JointType::Revolute => {
601 builder = builder.limits(
602 JointAxis::AngX,
603 [joint.limit.lower as Real, joint.limit.upper as Real],
604 )
605 }
606 _ => {}
607 }
608
609 // TODO: the following fields are currently ignored:
610 // - Joint::dynamics
611 // - Joint::limit.effort / limit.velocity
612 // - Joint::mimic
613 // - Joint::safety_controller
614 builder.build()
615}