From 5cbf9d41035420a3da02640d80cb9df42bf9358f Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Sat, 5 Oct 2024 22:17:12 -0700 Subject: [PATCH 01/10] Stickman --- examples3d/all_examples3.rs | 3 + examples3d/debug_multibody_sleep3.rs | 286 +++++++++++++++++++++++++++ 2 files changed, 289 insertions(+) create mode 100644 examples3d/debug_multibody_sleep3.rs diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 72a3cf7e6..c45a9314c 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -42,6 +42,7 @@ mod debug_cube_high_mass_ratio3; mod debug_internal_edges3; mod debug_long_chain3; mod debug_multibody_ang_motor_pos3; +mod debug_multibody_sleep3; mod inverse_kinematics3; mod joint_motor_position3; mod keva3; @@ -164,6 +165,8 @@ pub fn main() { "(Debug) multibody ang. motor pos.", debug_multibody_ang_motor_pos3::init_world, ), + + ("00 (Debug) sticcman", debug_multibody_sleep3::init_world) ]; // Lexicographic sort, with stress tests moved at the end of the list. diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs new file mode 100644 index 000000000..563408b85 --- /dev/null +++ b/examples3d/debug_multibody_sleep3.rs @@ -0,0 +1,286 @@ +use std::{f32::consts::{FRAC_PI_2, FRAC_PI_3, FRAC_PI_4, FRAC_PI_6, PI}, num::NonZeroUsize}; + +use nalgebra::Vector3; +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; +use wasm_bindgen::UnwrapThrowExt; + +pub fn init_world(testbed: &mut Testbed) { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + + //ground + let ground_size = 10.; + let ground_height = 1.; + let ground = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height/2., 0.0]); + let ground_body = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); + colliders.insert_with_parent(collider, ground_body, &mut bodies); + + + let radius = 0.1; + let head_radius = 0.3; + let head_total_height = 1.0; + let head_half_height = (head_total_height-head_radius*2.)/2.; + let arm_upper_len = 1.; + let arm_lower_len = 1.25; + let spine_len = 4.; + let num_spine_segments = 2; + let spine_seg_len = spine_len/(1. + num_spine_segments as f32); + let leg_len = 3.05; + let leg_upper_len = leg_len * 0.49; + let leg_lower_len = leg_len*0.51; + let motor_vel = 10f32.to_radians(); + let spine_seg_coll = ColliderBuilder::capsule_y((spine_seg_len/2.)-radius, radius); + + let seg_coll_handle = colliders.insert( + spine_seg_coll.clone() + ); + + let target_vel = 90f32.to_radians(); + let stiffness = 1.; + let damping = 0.; + let spine_root; + let spine_end; + + let mut mbj_handle_opt = None; + + {//spine + let body_spawn_loc = vector![0., leg_len, 0.]; + let fixed = bodies.insert(RigidBodyBuilder::fixed().translation(body_spawn_loc)); + spine_root = bodies.insert(RigidBodyBuilder::dynamic()); + colliders.set_parent(seg_coll_handle, Some(spine_root), &mut bodies); + multibody_joints.insert(fixed, spine_root, FixedJoint::new(), true); + let joint = SphericalJointBuilder::new() + .local_anchor1(point![0., spine_seg_len/2., 0.]) + .local_anchor2(point![0., -spine_seg_len/2., 0.]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping) + .build(); + let mut prev = spine_root; + for _ in 0..num_spine_segments { + let seg_coll_handle = colliders.insert( + spine_seg_coll.clone() + ); + let new_seg = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc)); + colliders.set_parent(seg_coll_handle, Some(new_seg), &mut bodies); + mbj_handle_opt = multibody_joints.insert(prev, new_seg, joint, true); + prev = new_seg; + } + spine_end = prev; + } + + + let joint_collider = ColliderBuilder::ball(radius); + + {//arms + let arm_r_upper; + + let shldx_j = RevoluteJointBuilder::new(Vector::x_axis()) + .local_anchor1(point![0., spine_seg_len/2., 0.]) + .limits([-150f32.to_radians(), 60f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + let shldy_j = RevoluteJointBuilder::new(Vector::y_axis()) + .limits([-90f32.to_radians(), 90f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + + + let upper_arm_coll = ColliderBuilder::capsule_x((arm_upper_len/2.)-radius, radius); + {//right arm + let mut rb_builder = RigidBodyBuilder::dynamic().translation(vector![1., leg_len+spine_len, 1.]); + {//upper arm + let shldz_j = RevoluteJointBuilder::new(Vector::z_axis()) + .local_anchor2(point![-arm_upper_len/2., 0., 0.]) + .limits([-90f32.to_radians(), 90f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + + let shldx = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent(joint_collider.clone(), shldx, &mut bodies); + let shldy = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent(joint_collider.clone(), shldy, &mut bodies); + arm_r_upper = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent( + upper_arm_coll.clone(), + arm_r_upper, + &mut bodies + ); + + multibody_joints.insert(spine_end, shldx, shldx_j, true); + multibody_joints.insert(shldx, shldy, shldy_j, true); + multibody_joints.insert(shldy, arm_r_upper, shldz_j, true).unwrap(); + } + + + {//lower arm + rb_builder = rb_builder.translation(vector![2., leg_len+spine_len, 1.]); + let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) + .local_anchor1(point![arm_upper_len/2., 0., 0.]) + .limits([0., 45f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) + .local_anchor2(point![-arm_lower_len/2., 0., 0.]) + .limits([0., 180f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + + let elbx = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); + let arm_r_lower = bodies.insert(rb_builder.clone()); + let lower_arm_coll = colliders.insert_with_parent( + ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius), + arm_r_lower, + &mut bodies + ); + + multibody_joints.insert(arm_r_upper, elbx, elbx_j, true); + multibody_joints.insert(elbx, arm_r_lower, elby_j, true); + } + } + + + {//left arm + let arm_l_upper; + let rb_builder = RigidBodyBuilder::dynamic().translation(vector![-1., leg_len+spine_len, -1.]); + {//upper + let shldz_j = RevoluteJointBuilder::new(Vector::z_axis()) + .local_anchor2(point![arm_upper_len/2., 0., 0.]) + .limits([-90f32.to_radians(), 90f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + + + let shldx = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent(joint_collider.clone(), shldx, &mut bodies); + let shldy = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent(joint_collider.clone(), shldy, &mut bodies); + arm_l_upper = bodies.insert(rb_builder.clone()); + colliders.insert_with_parent(upper_arm_coll, arm_l_upper, &mut bodies); + + + multibody_joints.insert(spine_end, shldx, shldx_j, true); + multibody_joints.insert(shldx, shldy, shldy_j, true); + multibody_joints.insert(shldy, arm_l_upper, shldz_j, true); + } + + + if false {//lower + let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) + .local_anchor1(point![-arm_upper_len/2., 0., 0.]) + .limits([0., 45f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) + .local_anchor2(point![arm_lower_len/2., 0., 0.]) + .limits([0., 180f32.to_radians()]) + .motor(0., target_vel, stiffness, damping); + + + let elbx = bodies.insert(RigidBodyBuilder::dynamic() + .translation(vector![0., 0., 1.]) + ); + colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); + let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic() + .translation(vector![0., 0., 1.]) + ); + colliders.insert_with_parent( + ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius), + arm_l_lower, + &mut bodies + ); + + + multibody_joints.insert(arm_l_upper, elbx, elbx_j, true); + multibody_joints.insert(elbx, arm_l_lower, elby_j, true); + } + } + } + + {//legs + let mut hipx_j = RevoluteJointBuilder::new(Vector3::x_axis()) + .local_anchor1(point![0., -spine_seg_len/2., 0.]) + .limits([-FRAC_PI_6, FRAC_PI_2]) + .motor(0., target_vel, stiffness, damping); + let mut hipy_j = RevoluteJointBuilder::new(Vector3::y_axis()) + .limits([0., FRAC_PI_2]) + .motor(0., target_vel, stiffness, damping); + let mut hipz_j = RevoluteJointBuilder::new(Vector3::z_axis()) + .local_anchor2(point![0., leg_upper_len/2., 0.]) + .limits([-FRAC_PI_4, 0.]) + .motor(-FRAC_PI_6/2., target_vel, stiffness, damping); + let mut kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) + .local_anchor1(point![0., -leg_upper_len/2., 0.]) + .local_anchor2(point![0., leg_lower_len/2., 0.]) + .limits([-FRAC_PI_4, 0.]) + .motor(0., target_vel, stiffness, damping); + let upper_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); + let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); + + {//right leg + let leg_r_upper; + + let hipx = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); + colliders.insert_with_parent(joint_collider.clone(), hipx, &mut bodies); + let hipy = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); + colliders.insert_with_parent(joint_collider.clone(), hipy, &mut bodies); + leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); + colliders.insert_with_parent(upper_leg_collider.clone(), leg_r_upper, &mut bodies); + + let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); + colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); + + multibody_joints.insert(spine_root, hipx, hipx_j, true); + multibody_joints.insert(hipx, hipy, hipy_j, true); + multibody_joints.insert(hipy, leg_r_upper, hipz_j, true); + multibody_joints.insert(leg_r_upper, leg_l_lower, kneex_j, true); + } + + {//left leg + hipy_j = hipy_j.limits([-FRAC_PI_2, 0.]); + hipz_j = hipz_j + .limits([0., FRAC_PI_4]) + .motor_position(FRAC_PI_6/2., stiffness, damping); + + let hipx = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); + colliders.insert_with_parent(joint_collider.clone(), hipx, &mut bodies); + let hipy = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); + colliders.insert_with_parent(joint_collider.clone(), hipy, &mut bodies); + let leg_l_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); + colliders.insert_with_parent(upper_leg_collider, leg_l_upper, &mut bodies); + + let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); + colliders.insert_with_parent(lower_leg_collider, leg_l_lower, &mut bodies); + + multibody_joints.insert(spine_root, hipx, hipx_j, true); + multibody_joints.insert(hipx, hipy, hipy_j, true); + multibody_joints.insert(hipy, leg_l_upper, hipz_j, true); + let h = multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true).unwrap(); + multibody_joints.get_mut(h).unwrap().0.set_self_contacts_enabled(false); + } + } + + {//head + // let neck_j = SphericalJointBuilder::new() + // .local_anchor1(point![0., spine_seg_len/2., 0.]) + // .local_anchor2(point![0., -(head_half_height+head_radius)/2., 0.]) + // .limits(JointAxis::AngX, [-FRAC_PI_4, FRAC_PI_3]) + // .limits(JointAxis::AngY, [-PI, PI]) + // .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_4]) + // .motor(JointAxis::AngX, 0., motor_vel, 1., 0.) + // .motor(JointAxis::AngY, 0., motor_vel, 1., 0.) + // .motor(JointAxis::AngZ, 0., motor_vel, 1., 0.); + // let head_collider = ColliderBuilder::capsule_y(head_half_height, head_radius); + // let head = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 1.5, -1.5])); + // colliders.insert_with_parent(head_collider, head, &mut bodies); + // multibody_joints.insert(spine_end, head, neck_j, true); + } + + let mb = multibody_joints.get_mut(mbj_handle_opt.unwrap()).unwrap().0; + mb.set_self_contacts_enabled(false); + + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + let integration_params = testbed.integration_parameters_mut(); + integration_params.num_solver_iterations = NonZeroUsize::new(12).unwrap(); + integration_params.num_internal_stabilization_iterations = 4; + testbed.look_at(point![15., 15., 15.], point![0.0, 2., 0.0]); +} From 4414e36695be7af91c2c80ffe96bae167ddfddb5 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Mon, 7 Oct 2024 11:02:17 -0700 Subject: [PATCH 02/10] Use spherical jonit of right leg hip --- examples3d/debug_multibody_sleep3.rs | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 563408b85..4236bf161 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -217,25 +217,28 @@ pub fn init_world(testbed: &mut Testbed) { let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); {//right leg - let leg_r_upper; - let hipx = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); - colliders.insert_with_parent(joint_collider.clone(), hipx, &mut bodies); - let hipy = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); - colliders.insert_with_parent(joint_collider.clone(), hipy, &mut bodies); - leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); + let l_hip = SphericalJointBuilder::new() + .local_anchor1(point![0., -spine_seg_len/2., 0.]) + .local_anchor2(point![0., leg_upper_len/2., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) + .limits(JointAxis::AngY, [-FRAC_PI_2, 0.]) + .limits(JointAxis::AngZ, [-FRAC_PI_4, 0.]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); + + + let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); colliders.insert_with_parent(upper_leg_collider.clone(), leg_r_upper, &mut bodies); - let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); - multibody_joints.insert(spine_root, hipx, hipx_j, true); - multibody_joints.insert(hipx, hipy, hipy_j, true); - multibody_joints.insert(hipy, leg_r_upper, hipz_j, true); + multibody_joints.insert(spine_root, leg_r_upper, l_hip, true); multibody_joints.insert(leg_r_upper, leg_l_lower, kneex_j, true); } - {//left leg + if false {//left leg hipy_j = hipy_j.limits([-FRAC_PI_2, 0.]); hipz_j = hipz_j .limits([0., FRAC_PI_4]) @@ -282,5 +285,5 @@ pub fn init_world(testbed: &mut Testbed) { let integration_params = testbed.integration_parameters_mut(); integration_params.num_solver_iterations = NonZeroUsize::new(12).unwrap(); integration_params.num_internal_stabilization_iterations = 4; - testbed.look_at(point![15., 15., 15.], point![0.0, 2., 0.0]); + testbed.look_at(point![15., 15., -15.], point![0.0, 2., 0.0]); } From 57a7b60bccda51d276ede84edf6f9ee6dc5210d1 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Mon, 7 Oct 2024 12:39:27 -0700 Subject: [PATCH 03/10] Finish stickman body --- examples3d/debug_multibody_sleep3.rs | 121 ++++++++++++++------------- 1 file changed, 62 insertions(+), 59 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 4236bf161..7700f76c5 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -35,6 +35,7 @@ pub fn init_world(testbed: &mut Testbed) { let leg_lower_len = leg_len*0.51; let motor_vel = 10f32.to_radians(); let spine_seg_coll = ColliderBuilder::capsule_y((spine_seg_len/2.)-radius, radius); + let body_spawn_loc = vector![0., 6., 0.]; let seg_coll_handle = colliders.insert( spine_seg_coll.clone() @@ -49,7 +50,6 @@ pub fn init_world(testbed: &mut Testbed) { let mut mbj_handle_opt = None; {//spine - let body_spawn_loc = vector![0., leg_len, 0.]; let fixed = bodies.insert(RigidBodyBuilder::fixed().translation(body_spawn_loc)); spine_root = bodies.insert(RigidBodyBuilder::dynamic()); colliders.set_parent(seg_coll_handle, Some(spine_root), &mut bodies); @@ -88,20 +88,27 @@ pub fn init_world(testbed: &mut Testbed) { .limits([-90f32.to_radians(), 90f32.to_radians()]) .motor(0., target_vel, stiffness, damping); - let upper_arm_coll = ColliderBuilder::capsule_x((arm_upper_len/2.)-radius, radius); + let lower_arm_coll = ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius); + {//right arm let mut rb_builder = RigidBodyBuilder::dynamic().translation(vector![1., leg_len+spine_len, 1.]); {//upper arm + let r_shld_j = SphericalJointBuilder::new() + .local_anchor1(point![0., spine_seg_len/2., 0.]) + .local_anchor2(point![-arm_upper_len/2., 0., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) + .limits(JointAxis::AngY, [-FRAC_PI_2, FRAC_PI_2+FRAC_PI_4]) + .limits(JointAxis::AngZ, [-FRAC_PI_2, FRAC_PI_2]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); + let shldz_j = RevoluteJointBuilder::new(Vector::z_axis()) .local_anchor2(point![-arm_upper_len/2., 0., 0.]) .limits([-90f32.to_radians(), 90f32.to_radians()]) .motor(0., target_vel, stiffness, damping); - let shldx = bodies.insert(rb_builder.clone()); - colliders.insert_with_parent(joint_collider.clone(), shldx, &mut bodies); - let shldy = bodies.insert(rb_builder.clone()); - colliders.insert_with_parent(joint_collider.clone(), shldy, &mut bodies); arm_r_upper = bodies.insert(rb_builder.clone()); colliders.insert_with_parent( upper_arm_coll.clone(), @@ -109,9 +116,7 @@ pub fn init_world(testbed: &mut Testbed) { &mut bodies ); - multibody_joints.insert(spine_end, shldx, shldx_j, true); - multibody_joints.insert(shldx, shldy, shldy_j, true); - multibody_joints.insert(shldy, arm_r_upper, shldz_j, true).unwrap(); + multibody_joints.insert(spine_end, arm_r_upper, r_shld_j, true); } @@ -129,11 +134,11 @@ pub fn init_world(testbed: &mut Testbed) { let elbx = bodies.insert(rb_builder.clone()); colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); let arm_r_lower = bodies.insert(rb_builder.clone()); - let lower_arm_coll = colliders.insert_with_parent( - ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius), - arm_r_lower, - &mut bodies - ); + colliders.insert_with_parent( + lower_arm_coll.clone(), + arm_r_lower, + &mut bodies + ); multibody_joints.insert(arm_r_upper, elbx, elbx_j, true); multibody_joints.insert(elbx, arm_r_lower, elby_j, true); @@ -150,48 +155,47 @@ pub fn init_world(testbed: &mut Testbed) { .limits([-90f32.to_radians(), 90f32.to_radians()]) .motor(0., target_vel, stiffness, damping); + let l_shld_j = SphericalJointBuilder::new() + .local_anchor1(point![0., spine_seg_len/2., 0.]) + .local_anchor2(point![arm_upper_len/2., 0., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) + .limits(JointAxis::AngY, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) + .limits(JointAxis::AngZ, [-FRAC_PI_2, FRAC_PI_2]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); - let shldx = bodies.insert(rb_builder.clone()); - colliders.insert_with_parent(joint_collider.clone(), shldx, &mut bodies); - let shldy = bodies.insert(rb_builder.clone()); - colliders.insert_with_parent(joint_collider.clone(), shldy, &mut bodies); arm_l_upper = bodies.insert(rb_builder.clone()); colliders.insert_with_parent(upper_arm_coll, arm_l_upper, &mut bodies); - multibody_joints.insert(spine_end, shldx, shldx_j, true); - multibody_joints.insert(shldx, shldy, shldy_j, true); - multibody_joints.insert(shldy, arm_l_upper, shldz_j, true); + multibody_joints.insert(spine_end, arm_l_upper, l_shld_j, true); } - if false {//lower - let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) + {//lower + let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) .local_anchor1(point![-arm_upper_len/2., 0., 0.]) - .limits([0., 45f32.to_radians()]) + .limits([-FRAC_PI_2+FRAC_PI_4, 0.]) .motor(0., target_vel, stiffness, damping); - let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) + let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) .local_anchor2(point![arm_lower_len/2., 0., 0.]) - .limits([0., 180f32.to_radians()]) + .limits([-FRAC_PI_2, FRAC_PI_2]) .motor(0., target_vel, stiffness, damping); - let elbx = bodies.insert(RigidBodyBuilder::dynamic() - .translation(vector![0., 0., 1.]) - ); + let elbx = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 0., 1.])); colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); - let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic() - .translation(vector![0., 0., 1.]) - ); - colliders.insert_with_parent( - ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius), - arm_l_lower, - &mut bodies - ); + let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 0., 1.])); + colliders.insert_with_parent( + lower_arm_coll.clone(), + arm_l_lower, + &mut bodies + ); - multibody_joints.insert(arm_l_upper, elbx, elbx_j, true); - multibody_joints.insert(elbx, arm_l_lower, elby_j, true); + multibody_joints.insert(arm_l_upper, elbx, elby_j, true); + multibody_joints.insert(elbx, arm_l_lower, elbx_j, true); } } } @@ -211,22 +215,22 @@ pub fn init_world(testbed: &mut Testbed) { let mut kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) .local_anchor1(point![0., -leg_upper_len/2., 0.]) .local_anchor2(point![0., leg_lower_len/2., 0.]) - .limits([-FRAC_PI_4, 0.]) + .limits([-FRAC_PI_2-FRAC_PI_4, 0.]) .motor(0., target_vel, stiffness, damping); let upper_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); {//right leg - let l_hip = SphericalJointBuilder::new() + let r_hip = SphericalJointBuilder::new() .local_anchor1(point![0., -spine_seg_len/2., 0.]) .local_anchor2(point![0., leg_upper_len/2., 0.]) .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) .limits(JointAxis::AngY, [-FRAC_PI_2, 0.]) - .limits(JointAxis::AngZ, [-FRAC_PI_4, 0.]) + .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_6/2.]) .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); + .motor(JointAxis::AngZ, FRAC_PI_6/2., target_vel, stiffness, damping); let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); @@ -234,31 +238,30 @@ pub fn init_world(testbed: &mut Testbed) { let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); - multibody_joints.insert(spine_root, leg_r_upper, l_hip, true); + multibody_joints.insert(spine_root, leg_r_upper, r_hip, true); multibody_joints.insert(leg_r_upper, leg_l_lower, kneex_j, true); } - if false {//left leg - hipy_j = hipy_j.limits([-FRAC_PI_2, 0.]); - hipz_j = hipz_j - .limits([0., FRAC_PI_4]) - .motor_position(FRAC_PI_6/2., stiffness, damping); + {//left leg + + let l_hip = SphericalJointBuilder::new() + .local_anchor1(point![0., -spine_seg_len/2., 0.]) + .local_anchor2(point![0., leg_upper_len/2., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) + .limits(JointAxis::AngY, [0., FRAC_PI_2]) + .limits(JointAxis::AngZ, [-FRAC_PI_6/2., FRAC_PI_4]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, -FRAC_PI_6/2., target_vel, stiffness, damping); - let hipx = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); - colliders.insert_with_parent(joint_collider.clone(), hipx, &mut bodies); - let hipy = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); - colliders.insert_with_parent(joint_collider.clone(), hipy, &mut bodies); let leg_l_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); colliders.insert_with_parent(upper_leg_collider, leg_l_upper, &mut bodies); let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); colliders.insert_with_parent(lower_leg_collider, leg_l_lower, &mut bodies); - multibody_joints.insert(spine_root, hipx, hipx_j, true); - multibody_joints.insert(hipx, hipy, hipy_j, true); - multibody_joints.insert(hipy, leg_l_upper, hipz_j, true); - let h = multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true).unwrap(); - multibody_joints.get_mut(h).unwrap().0.set_self_contacts_enabled(false); + multibody_joints.insert(spine_root, leg_l_upper, l_hip, true); + multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true); } } @@ -285,5 +288,5 @@ pub fn init_world(testbed: &mut Testbed) { let integration_params = testbed.integration_parameters_mut(); integration_params.num_solver_iterations = NonZeroUsize::new(12).unwrap(); integration_params.num_internal_stabilization_iterations = 4; - testbed.look_at(point![15., 15., -15.], point![0.0, 2., 0.0]); + testbed.look_at(point![15., 15., 15.], point![0.0, 2., 0.0]); } From 3b22ea601e6cebb4c10b3eea17c1f0c7a13025f9 Mon Sep 17 00:00:00 2001 From: Nilson J Vidanapatiranage Date: Mon, 7 Oct 2024 14:15:01 -0700 Subject: [PATCH 04/10] Add neck to stickman --- examples3d/debug_multibody_sleep3.rs | 44 ++++++++++++++++++++-------- 1 file changed, 31 insertions(+), 13 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 7700f76c5..328e1ea2a 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -266,19 +266,37 @@ pub fn init_world(testbed: &mut Testbed) { } {//head - // let neck_j = SphericalJointBuilder::new() - // .local_anchor1(point![0., spine_seg_len/2., 0.]) - // .local_anchor2(point![0., -(head_half_height+head_radius)/2., 0.]) - // .limits(JointAxis::AngX, [-FRAC_PI_4, FRAC_PI_3]) - // .limits(JointAxis::AngY, [-PI, PI]) - // .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_4]) - // .motor(JointAxis::AngX, 0., motor_vel, 1., 0.) - // .motor(JointAxis::AngY, 0., motor_vel, 1., 0.) - // .motor(JointAxis::AngZ, 0., motor_vel, 1., 0.); - // let head_collider = ColliderBuilder::capsule_y(head_half_height, head_radius); - // let head = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 1.5, -1.5])); - // colliders.insert_with_parent(head_collider, head, &mut bodies); - // multibody_joints.insert(spine_end, head, neck_j, true); + + let neck_len = spine_len/8.; + let neck_collider = ColliderBuilder::capsule_y((neck_len/2.)-radius, radius); + let neck = bodies.insert(RigidBodyBuilder::dynamic()); + colliders.insert_with_parent( + neck_collider, + neck, + &mut bodies + ); + let head_collider = ColliderBuilder::capsule_y(head_half_height, head_radius); + let head = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 1.5, -1.5])); + colliders.insert_with_parent(head_collider, head, &mut bodies); + + let spine_to_neck_j = SphericalJointBuilder::new() + .local_anchor1(point![0., spine_seg_len/2., 0.]) + .local_anchor2(point![0., -neck_len/2., 0.]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); + let neck_j = SphericalJointBuilder::new() + .local_anchor1(point![0., neck_len/2., 0.]) + .local_anchor2(point![0., -(head_half_height+head_radius)/2., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_4, FRAC_PI_3]) + .limits(JointAxis::AngY, [-PI, PI]) + .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_4]) + .motor(JointAxis::AngX, 0., motor_vel, 1., 0.) + .motor(JointAxis::AngY, 0., motor_vel, 1., 0.) + .motor(JointAxis::AngZ, 0., motor_vel, 1., 0.); + + multibody_joints.insert(spine_end, neck, spine_to_neck_j, true); + multibody_joints.insert(neck, head, neck_j, true); } let mb = multibody_joints.get_mut(mbj_handle_opt.unwrap()).unwrap().0; From ed140b155d9e09111dbb4544aeee34ca902cc304 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Wed, 9 Oct 2024 06:41:38 -0700 Subject: [PATCH 05/10] Remove unused joints --- examples3d/debug_multibody_sleep3.rs | 31 +--------------------------- 1 file changed, 1 insertion(+), 30 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 328e1ea2a..1ebe52f23 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -3,7 +3,6 @@ use std::{f32::consts::{FRAC_PI_2, FRAC_PI_3, FRAC_PI_4, FRAC_PI_6, PI}, num::No use nalgebra::Vector3; use rapier3d::prelude::*; use rapier_testbed3d::Testbed; -use wasm_bindgen::UnwrapThrowExt; pub fn init_world(testbed: &mut Testbed) { let mut bodies = RigidBodySet::new(); @@ -80,14 +79,6 @@ pub fn init_world(testbed: &mut Testbed) { {//arms let arm_r_upper; - let shldx_j = RevoluteJointBuilder::new(Vector::x_axis()) - .local_anchor1(point![0., spine_seg_len/2., 0.]) - .limits([-150f32.to_radians(), 60f32.to_radians()]) - .motor(0., target_vel, stiffness, damping); - let shldy_j = RevoluteJointBuilder::new(Vector::y_axis()) - .limits([-90f32.to_radians(), 90f32.to_radians()]) - .motor(0., target_vel, stiffness, damping); - let upper_arm_coll = ColliderBuilder::capsule_x((arm_upper_len/2.)-radius, radius); let lower_arm_coll = ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius); @@ -104,11 +95,6 @@ pub fn init_world(testbed: &mut Testbed) { .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); - let shldz_j = RevoluteJointBuilder::new(Vector::z_axis()) - .local_anchor2(point![-arm_upper_len/2., 0., 0.]) - .limits([-90f32.to_radians(), 90f32.to_radians()]) - .motor(0., target_vel, stiffness, damping); - arm_r_upper = bodies.insert(rb_builder.clone()); colliders.insert_with_parent( upper_arm_coll.clone(), @@ -150,10 +136,6 @@ pub fn init_world(testbed: &mut Testbed) { let arm_l_upper; let rb_builder = RigidBodyBuilder::dynamic().translation(vector![-1., leg_len+spine_len, -1.]); {//upper - let shldz_j = RevoluteJointBuilder::new(Vector::z_axis()) - .local_anchor2(point![arm_upper_len/2., 0., 0.]) - .limits([-90f32.to_radians(), 90f32.to_radians()]) - .motor(0., target_vel, stiffness, damping); let l_shld_j = SphericalJointBuilder::new() .local_anchor1(point![0., spine_seg_len/2., 0.]) @@ -201,18 +183,7 @@ pub fn init_world(testbed: &mut Testbed) { } {//legs - let mut hipx_j = RevoluteJointBuilder::new(Vector3::x_axis()) - .local_anchor1(point![0., -spine_seg_len/2., 0.]) - .limits([-FRAC_PI_6, FRAC_PI_2]) - .motor(0., target_vel, stiffness, damping); - let mut hipy_j = RevoluteJointBuilder::new(Vector3::y_axis()) - .limits([0., FRAC_PI_2]) - .motor(0., target_vel, stiffness, damping); - let mut hipz_j = RevoluteJointBuilder::new(Vector3::z_axis()) - .local_anchor2(point![0., leg_upper_len/2., 0.]) - .limits([-FRAC_PI_4, 0.]) - .motor(-FRAC_PI_6/2., target_vel, stiffness, damping); - let mut kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) + let kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) .local_anchor1(point![0., -leg_upper_len/2., 0.]) .local_anchor2(point![0., leg_lower_len/2., 0.]) .limits([-FRAC_PI_2-FRAC_PI_4, 0.]) From a41f753c1de0eeb80bba8128c8975258d0270ab3 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Wed, 9 Oct 2024 21:41:26 -0700 Subject: [PATCH 06/10] Fix crash when removing fixed root --- examples3d/debug_multibody_sleep3.rs | 186 +++++++++++++++------------ 1 file changed, 102 insertions(+), 84 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 1ebe52f23..1637043e9 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -42,17 +42,72 @@ pub fn init_world(testbed: &mut Testbed) { let target_vel = 90f32.to_radians(); let stiffness = 1.; - let damping = 0.; - let spine_root; + let damping = 0.01; let spine_end; + let spine_end_pos; + let mut mbj_handle_opt = None; + + // let fixed = bodies.insert(RigidBodyBuilder::fixed().translation(body_spawn_loc)); + let spine_root = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc)); + colliders.set_parent(seg_coll_handle, Some(spine_root), &mut bodies); + // multibody_joints.insert(fixed, spine_root, FixedJoint::new(), true); + + {//legs + let kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) + .local_anchor1(point![0., -leg_upper_len/2., 0.]) + .local_anchor2(point![0., leg_lower_len/2., 0.]) + .limits([-FRAC_PI_2-FRAC_PI_4, 0.]) + .motor(0., target_vel, stiffness, damping); + let upper_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); + let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); + + {//right leg + let r_hip = SphericalJointBuilder::new() + .local_anchor1(point![0., -spine_seg_len/2., 0.]) + .local_anchor2(point![0., leg_upper_len/2., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) + .limits(JointAxis::AngY, [-FRAC_PI_2, 0.]) + .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_6/2.]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, FRAC_PI_6/2., target_vel, stiffness, damping); + + + let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*2., -leg_upper_len, 0.])); + colliders.insert_with_parent(upper_leg_collider.clone(), leg_r_upper, &mut bodies); + let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*5., -leg_upper_len, 0.])); + colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); + + multibody_joints.insert(spine_root, leg_r_upper, r_hip, true); + multibody_joints.insert(leg_r_upper, leg_l_lower, kneex_j, true); + } + + {//left leg + let l_hip = SphericalJointBuilder::new() + .local_anchor1(point![0., -spine_seg_len/2., 0.]) + .local_anchor2(point![0., leg_upper_len/2., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) + .limits(JointAxis::AngY, [0., FRAC_PI_2]) + .limits(JointAxis::AngZ, [-FRAC_PI_6/2., FRAC_PI_4]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, -FRAC_PI_6/2., target_vel, stiffness, damping); + + let leg_l_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![-radius*2., -leg_upper_len, 0.])); + colliders.insert_with_parent(upper_leg_collider, leg_l_upper, &mut bodies); + + let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![-radius*5., -leg_upper_len, 0.])); + colliders.insert_with_parent(lower_leg_collider, leg_l_lower, &mut bodies); + + multibody_joints.insert(spine_root, leg_l_upper, l_hip, true); + multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true); + } + } + {//spine - let fixed = bodies.insert(RigidBodyBuilder::fixed().translation(body_spawn_loc)); - spine_root = bodies.insert(RigidBodyBuilder::dynamic()); - colliders.set_parent(seg_coll_handle, Some(spine_root), &mut bodies); - multibody_joints.insert(fixed, spine_root, FixedJoint::new(), true); let joint = SphericalJointBuilder::new() .local_anchor1(point![0., spine_seg_len/2., 0.]) .local_anchor2(point![0., -spine_seg_len/2., 0.]) @@ -61,19 +116,21 @@ pub fn init_world(testbed: &mut Testbed) { .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping) .build(); let mut prev = spine_root; - for _ in 0..num_spine_segments { - let seg_coll_handle = colliders.insert( - spine_seg_coll.clone() + for i in 0..num_spine_segments { + let seg_coll_handle = colliders.insert(spine_seg_coll.clone()); + let new_seg = bodies.insert(RigidBodyBuilder::dynamic() + .translation(body_spawn_loc + vector![0., (i as f32 + 1.)*(spine_seg_len*1.1), 0.]) ); - let new_seg = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc)); - colliders.set_parent(seg_coll_handle, Some(new_seg), &mut bodies); + colliders.set_parent(seg_coll_handle, Some(new_seg), &mut bodies); mbj_handle_opt = multibody_joints.insert(prev, new_seg, joint, true); prev = new_seg; } spine_end = prev; + + let spine_end_y = (num_spine_segments as f32)*(spine_seg_len*1.1) + spine_seg_len/2.; + spine_end_pos = body_spawn_loc + vector![0., spine_end_y, 0.]; } - let joint_collider = ColliderBuilder::ball(radius); {//arms @@ -82,8 +139,11 @@ pub fn init_world(testbed: &mut Testbed) { let upper_arm_coll = ColliderBuilder::capsule_x((arm_upper_len/2.)-radius, radius); let lower_arm_coll = ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius); + {//right arm - let mut rb_builder = RigidBodyBuilder::dynamic().translation(vector![1., leg_len+spine_len, 1.]); + let mut rb_builder = RigidBodyBuilder::dynamic(); + + let upper_arm_pos = spine_end_pos + vector![arm_upper_len/2. + radius, 0., 0.]; {//upper arm let r_shld_j = SphericalJointBuilder::new() .local_anchor1(point![0., spine_seg_len/2., 0.]) @@ -95,7 +155,7 @@ pub fn init_world(testbed: &mut Testbed) { .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); - arm_r_upper = bodies.insert(rb_builder.clone()); + arm_r_upper = bodies.insert(rb_builder.clone().translation(upper_arm_pos)); colliders.insert_with_parent( upper_arm_coll.clone(), arm_r_upper, @@ -105,7 +165,6 @@ pub fn init_world(testbed: &mut Testbed) { multibody_joints.insert(spine_end, arm_r_upper, r_shld_j, true); } - {//lower arm rb_builder = rb_builder.translation(vector![2., leg_len+spine_len, 1.]); let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) @@ -116,10 +175,15 @@ pub fn init_world(testbed: &mut Testbed) { .local_anchor2(point![-arm_lower_len/2., 0., 0.]) .limits([0., 180f32.to_radians()]) .motor(0., target_vel, stiffness, damping); - - let elbx = bodies.insert(rb_builder.clone()); + + let elb_pos = upper_arm_pos + vector![arm_upper_len/2. + radius*2., 0., 0.]; + let elbx = bodies.insert(rb_builder.clone() + .translation(elb_pos) + ); colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); - let arm_r_lower = bodies.insert(rb_builder.clone()); + let arm_r_lower = bodies.insert(rb_builder.clone() + .translation(elb_pos + vector![arm_lower_len/2. + radius*2., 0., 0.]) + ); colliders.insert_with_parent( lower_arm_coll.clone(), arm_r_lower, @@ -135,8 +199,9 @@ pub fn init_world(testbed: &mut Testbed) { {//left arm let arm_l_upper; let rb_builder = RigidBodyBuilder::dynamic().translation(vector![-1., leg_len+spine_len, -1.]); + + let upper_arm_pos = spine_end_pos - vector![arm_upper_len/2. + radius, 0., 0.]; {//upper - let l_shld_j = SphericalJointBuilder::new() .local_anchor1(point![0., spine_seg_len/2., 0.]) .local_anchor2(point![arm_upper_len/2., 0., 0.]) @@ -147,14 +212,13 @@ pub fn init_world(testbed: &mut Testbed) { .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); - arm_l_upper = bodies.insert(rb_builder.clone()); + arm_l_upper = bodies.insert(rb_builder.clone().translation(upper_arm_pos)); colliders.insert_with_parent(upper_arm_coll, arm_l_upper, &mut bodies); multibody_joints.insert(spine_end, arm_l_upper, l_shld_j, true); } - {//lower let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) .local_anchor1(point![-arm_upper_len/2., 0., 0.]) @@ -166,9 +230,12 @@ pub fn init_world(testbed: &mut Testbed) { .motor(0., target_vel, stiffness, damping); - let elbx = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 0., 1.])); + let elb_pos = upper_arm_pos - vector![arm_upper_len/2. + radius*2., 0., 0.]; + let elbx = bodies.insert(RigidBodyBuilder::dynamic().translation(elb_pos)); colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); - let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 0., 1.])); + let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic() + .translation(elb_pos - vector![arm_lower_len/2. + radius*2., 0., 0.]) + ); colliders.insert_with_parent( lower_arm_coll.clone(), arm_l_lower, @@ -182,72 +249,23 @@ pub fn init_world(testbed: &mut Testbed) { } } - {//legs - let kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) - .local_anchor1(point![0., -leg_upper_len/2., 0.]) - .local_anchor2(point![0., leg_lower_len/2., 0.]) - .limits([-FRAC_PI_2-FRAC_PI_4, 0.]) - .motor(0., target_vel, stiffness, damping); - let upper_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); - let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); - - {//right leg - - let r_hip = SphericalJointBuilder::new() - .local_anchor1(point![0., -spine_seg_len/2., 0.]) - .local_anchor2(point![0., leg_upper_len/2., 0.]) - .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) - .limits(JointAxis::AngY, [-FRAC_PI_2, 0.]) - .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_6/2.]) - .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngZ, FRAC_PI_6/2., target_vel, stiffness, damping); - - - let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); - colliders.insert_with_parent(upper_leg_collider.clone(), leg_r_upper, &mut bodies); - let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 5., 1.])); - colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); - - multibody_joints.insert(spine_root, leg_r_upper, r_hip, true); - multibody_joints.insert(leg_r_upper, leg_l_lower, kneex_j, true); - } - - {//left leg - - let l_hip = SphericalJointBuilder::new() - .local_anchor1(point![0., -spine_seg_len/2., 0.]) - .local_anchor2(point![0., leg_upper_len/2., 0.]) - .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) - .limits(JointAxis::AngY, [0., FRAC_PI_2]) - .limits(JointAxis::AngZ, [-FRAC_PI_6/2., FRAC_PI_4]) - .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngZ, -FRAC_PI_6/2., target_vel, stiffness, damping); - - let leg_l_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); - colliders.insert_with_parent(upper_leg_collider, leg_l_upper, &mut bodies); - - let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![1., 5., 1.])); - colliders.insert_with_parent(lower_leg_collider, leg_l_lower, &mut bodies); - - multibody_joints.insert(spine_root, leg_l_upper, l_hip, true); - multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true); - } - } - {//head let neck_len = spine_len/8.; + let neck_pos = spine_end_pos + vector![0., neck_len/2. + radius, 0.]; let neck_collider = ColliderBuilder::capsule_y((neck_len/2.)-radius, radius); - let neck = bodies.insert(RigidBodyBuilder::dynamic()); - colliders.insert_with_parent( - neck_collider, - neck, - &mut bodies - ); + let neck = bodies.insert(RigidBodyBuilder::dynamic().translation(neck_pos)); + colliders.insert_with_parent( + neck_collider, + neck, + &mut bodies + ); + + let head_pos = neck_pos + vector![0., neck_len/2. + head_total_height/2. + radius, 0.]; let head_collider = ColliderBuilder::capsule_y(head_half_height, head_radius); - let head = bodies.insert(RigidBodyBuilder::dynamic().translation(vector![0., 1.5, -1.5])); + let head = bodies.insert(RigidBodyBuilder::dynamic() + .translation(head_pos) + ); colliders.insert_with_parent(head_collider, head, &mut bodies); let spine_to_neck_j = SphericalJointBuilder::new() From ceea27eb43d312d153f0163522591a9270f6109e Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Wed, 9 Oct 2024 21:56:29 -0700 Subject: [PATCH 07/10] Decrease number solver iterations --- examples3d/debug_multibody_sleep3.rs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 1637043e9..c75646500 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -293,7 +293,6 @@ pub fn init_world(testbed: &mut Testbed) { testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); let integration_params = testbed.integration_parameters_mut(); - integration_params.num_solver_iterations = NonZeroUsize::new(12).unwrap(); - integration_params.num_internal_stabilization_iterations = 4; + integration_params.num_solver_iterations = NonZeroUsize::new(6).unwrap(); testbed.look_at(point![15., 15., 15.], point![0.0, 2., 0.0]); } From d46cf963683c0e32af37489e80f6e306ccba8f00 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Thu, 10 Oct 2024 10:40:09 -0700 Subject: [PATCH 08/10] Give joints ball colliders --- examples3d/debug_multibody_sleep3.rs | 79 ++++++++++++++++++++-------- 1 file changed, 57 insertions(+), 22 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index c75646500..45037290e 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -36,9 +36,7 @@ pub fn init_world(testbed: &mut Testbed) { let spine_seg_coll = ColliderBuilder::capsule_y((spine_seg_len/2.)-radius, radius); let body_spawn_loc = vector![0., 6., 0.]; - let seg_coll_handle = colliders.insert( - spine_seg_coll.clone() - ); + let joint_collider = ColliderBuilder::ball(radius); let target_vel = 90f32.to_radians(); let stiffness = 1.; @@ -52,7 +50,16 @@ pub fn init_world(testbed: &mut Testbed) { // let fixed = bodies.insert(RigidBodyBuilder::fixed().translation(body_spawn_loc)); let spine_root = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc)); - colliders.set_parent(seg_coll_handle, Some(spine_root), &mut bodies); + colliders.insert_with_parent( + spine_seg_coll.clone(), + spine_root, + &mut bodies + ); + colliders.insert_with_parent( + joint_collider.clone().translation(vector![0., -spine_seg_len/2., 0.]), + spine_root, + &mut bodies + ); // multibody_joints.insert(fixed, spine_root, FixedJoint::new(), true); {//legs @@ -78,11 +85,16 @@ pub fn init_world(testbed: &mut Testbed) { let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*2., -leg_upper_len, 0.])); colliders.insert_with_parent(upper_leg_collider.clone(), leg_r_upper, &mut bodies); - let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*5., -leg_upper_len, 0.])); - colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); + let leg_r_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*5., -leg_upper_len, 0.])); + colliders.insert_with_parent(lower_leg_collider.clone(), leg_r_lower, &mut bodies); + colliders.insert_with_parent( + joint_collider.clone().translation(vector![0., leg_lower_len/2., 0.]), + leg_r_lower, + &mut bodies + ); multibody_joints.insert(spine_root, leg_r_upper, r_hip, true); - multibody_joints.insert(leg_r_upper, leg_l_lower, kneex_j, true); + multibody_joints.insert(leg_r_upper, leg_r_lower, kneex_j, true); } {//left leg @@ -101,6 +113,11 @@ pub fn init_world(testbed: &mut Testbed) { let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![-radius*5., -leg_upper_len, 0.])); colliders.insert_with_parent(lower_leg_collider, leg_l_lower, &mut bodies); + colliders.insert_with_parent( + joint_collider.clone().translation(vector![0., leg_lower_len/2., 0.]), + leg_l_lower, + &mut bodies + ); multibody_joints.insert(spine_root, leg_l_upper, l_hip, true); multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true); @@ -117,11 +134,20 @@ pub fn init_world(testbed: &mut Testbed) { .build(); let mut prev = spine_root; for i in 0..num_spine_segments { - let seg_coll_handle = colliders.insert(spine_seg_coll.clone()); let new_seg = bodies.insert(RigidBodyBuilder::dynamic() .translation(body_spawn_loc + vector![0., (i as f32 + 1.)*(spine_seg_len*1.1), 0.]) ); - colliders.set_parent(seg_coll_handle, Some(new_seg), &mut bodies); + colliders.insert_with_parent( + spine_seg_coll.clone(), + new_seg, + &mut bodies + ); + colliders.insert_with_parent( + joint_collider.clone().translation(vector![0., -spine_seg_len/2., 0.]), + new_seg, + &mut bodies + ); + mbj_handle_opt = multibody_joints.insert(prev, new_seg, joint, true); prev = new_seg; } @@ -131,7 +157,6 @@ pub fn init_world(testbed: &mut Testbed) { spine_end_pos = body_spawn_loc + vector![0., spine_end_y, 0.]; } - let joint_collider = ColliderBuilder::ball(radius); {//arms let arm_r_upper; @@ -180,15 +205,19 @@ pub fn init_world(testbed: &mut Testbed) { let elbx = bodies.insert(rb_builder.clone() .translation(elb_pos) ); - colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); let arm_r_lower = bodies.insert(rb_builder.clone() .translation(elb_pos + vector![arm_lower_len/2. + radius*2., 0., 0.]) ); - colliders.insert_with_parent( - lower_arm_coll.clone(), - arm_r_lower, - &mut bodies - ); + colliders.insert_with_parent( + lower_arm_coll.clone(), + arm_r_lower, + &mut bodies + ); + colliders.insert_with_parent( + joint_collider.clone().translation(vector![-arm_lower_len/2., 0., 0.]), + arm_r_lower, + &mut bodies + ); multibody_joints.insert(arm_r_upper, elbx, elbx_j, true); multibody_joints.insert(elbx, arm_r_lower, elby_j, true); @@ -232,15 +261,21 @@ pub fn init_world(testbed: &mut Testbed) { let elb_pos = upper_arm_pos - vector![arm_upper_len/2. + radius*2., 0., 0.]; let elbx = bodies.insert(RigidBodyBuilder::dynamic().translation(elb_pos)); - colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); + // colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic() .translation(elb_pos - vector![arm_lower_len/2. + radius*2., 0., 0.]) ); - colliders.insert_with_parent( - lower_arm_coll.clone(), - arm_l_lower, - &mut bodies - ); + colliders.insert_with_parent( + lower_arm_coll.clone(), + arm_l_lower, + &mut bodies + ); + colliders.insert_with_parent( + + joint_collider.clone().translation(vector![arm_lower_len/2., 0., 0.]), + arm_l_lower, + &mut bodies + ); multibody_joints.insert(arm_l_upper, elbx, elby_j, true); From 1693b84417ca5ce538dee8a341fa1c20d4e08529 Mon Sep 17 00:00:00 2001 From: Nilson J Vidanapatiranage Date: Fri, 11 Oct 2024 13:06:50 -0700 Subject: [PATCH 09/10] Switch some chunks of code around a bit --- examples3d/debug_multibody_sleep3.rs | 181 +++++++++++++-------------- 1 file changed, 90 insertions(+), 91 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 45037290e..1dc3b8ae4 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -48,7 +48,6 @@ pub fn init_world(testbed: &mut Testbed) { let mut mbj_handle_opt = None; - // let fixed = bodies.insert(RigidBodyBuilder::fixed().translation(body_spawn_loc)); let spine_root = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc)); colliders.insert_with_parent( spine_seg_coll.clone(), @@ -60,7 +59,6 @@ pub fn init_world(testbed: &mut Testbed) { spine_root, &mut bodies ); - // multibody_joints.insert(fixed, spine_root, FixedJoint::new(), true); {//legs let kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) @@ -71,31 +69,8 @@ pub fn init_world(testbed: &mut Testbed) { let upper_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); - {//right leg - let r_hip = SphericalJointBuilder::new() - .local_anchor1(point![0., -spine_seg_len/2., 0.]) - .local_anchor2(point![0., leg_upper_len/2., 0.]) - .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) - .limits(JointAxis::AngY, [-FRAC_PI_2, 0.]) - .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_6/2.]) - .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngZ, FRAC_PI_6/2., target_vel, stiffness, damping); - - - let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*2., -leg_upper_len, 0.])); - colliders.insert_with_parent(upper_leg_collider.clone(), leg_r_upper, &mut bodies); - let leg_r_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*5., -leg_upper_len, 0.])); - colliders.insert_with_parent(lower_leg_collider.clone(), leg_r_lower, &mut bodies); - colliders.insert_with_parent( - joint_collider.clone().translation(vector![0., leg_lower_len/2., 0.]), - leg_r_lower, - &mut bodies - ); - - multibody_joints.insert(spine_root, leg_r_upper, r_hip, true); - multibody_joints.insert(leg_r_upper, leg_r_lower, kneex_j, true); - } + let leg_l_lower; + let leg_r_lower; {//left leg let l_hip = SphericalJointBuilder::new() @@ -109,10 +84,10 @@ pub fn init_world(testbed: &mut Testbed) { .motor(JointAxis::AngZ, -FRAC_PI_6/2., target_vel, stiffness, damping); let leg_l_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![-radius*2., -leg_upper_len, 0.])); - colliders.insert_with_parent(upper_leg_collider, leg_l_upper, &mut bodies); + colliders.insert_with_parent(upper_leg_collider.clone(), leg_l_upper, &mut bodies); - let leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![-radius*5., -leg_upper_len, 0.])); - colliders.insert_with_parent(lower_leg_collider, leg_l_lower, &mut bodies); + leg_l_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![-radius*5., -leg_upper_len, 0.])); + colliders.insert_with_parent(lower_leg_collider.clone(), leg_l_lower, &mut bodies); colliders.insert_with_parent( joint_collider.clone().translation(vector![0., leg_lower_len/2., 0.]), leg_l_lower, @@ -122,6 +97,32 @@ pub fn init_world(testbed: &mut Testbed) { multibody_joints.insert(spine_root, leg_l_upper, l_hip, true); multibody_joints.insert(leg_l_upper, leg_l_lower, kneex_j, true); } + + {//right leg + let r_hip = SphericalJointBuilder::new() + .local_anchor1(point![0., -spine_seg_len/2., 0.]) + .local_anchor2(point![0., leg_upper_len/2., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2, FRAC_PI_2]) + .limits(JointAxis::AngY, [-FRAC_PI_2, 0.]) + .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_6/2.]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, FRAC_PI_6/2., target_vel, stiffness, damping); + + + let leg_r_upper = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*2., -leg_upper_len, 0.])); + colliders.insert_with_parent(upper_leg_collider, leg_r_upper, &mut bodies); + leg_r_lower = bodies.insert(RigidBodyBuilder::dynamic().translation(body_spawn_loc+vector![radius*5., -leg_upper_len, 0.])); + colliders.insert_with_parent(lower_leg_collider, leg_r_lower, &mut bodies); + colliders.insert_with_parent( + joint_collider.clone().translation(vector![0., leg_lower_len/2., 0.]), + leg_r_lower, + &mut bodies + ); + + multibody_joints.insert(spine_root, leg_r_upper, r_hip, true); + multibody_joints.insert(leg_r_upper, leg_r_lower, kneex_j, true); + } } {//spine @@ -157,14 +158,70 @@ pub fn init_world(testbed: &mut Testbed) { spine_end_pos = body_spawn_loc + vector![0., spine_end_y, 0.]; } - {//arms let arm_r_upper; let upper_arm_coll = ColliderBuilder::capsule_x((arm_upper_len/2.)-radius, radius); let lower_arm_coll = ColliderBuilder::capsule_x((arm_lower_len/2.)-radius, radius); + {//left arm + let arm_l_upper; + let rb_builder = RigidBodyBuilder::dynamic().translation(vector![-1., leg_len+spine_len, -1.]); + + let upper_arm_pos = spine_end_pos - vector![arm_upper_len/2. + radius, 0., 0.]; + {//upper + let l_shld_j = SphericalJointBuilder::new() + .local_anchor1(point![0., spine_seg_len/2., 0.]) + .local_anchor2(point![arm_upper_len/2., 0., 0.]) + .limits(JointAxis::AngX, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) + .limits(JointAxis::AngY, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) + .limits(JointAxis::AngZ, [-FRAC_PI_2, FRAC_PI_2]) + .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) + .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); + + arm_l_upper = bodies.insert(rb_builder.clone().translation(upper_arm_pos)); + colliders.insert_with_parent(upper_arm_coll.clone(), arm_l_upper, &mut bodies); + + + multibody_joints.insert(spine_end, arm_l_upper, l_shld_j, true); + } + + {//lower + let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) + .local_anchor1(point![-arm_upper_len/2., 0., 0.]) + .limits([-FRAC_PI_2+FRAC_PI_4, 0.]) + .motor(0., target_vel, stiffness, damping); + let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) + .local_anchor2(point![arm_lower_len/2., 0., 0.]) + .limits([-FRAC_PI_2, FRAC_PI_2]) + .motor(0., target_vel, stiffness, damping); + + + let elb_pos = upper_arm_pos - vector![arm_upper_len/2. + radius*2., 0., 0.]; + let elbx = bodies.insert(RigidBodyBuilder::dynamic().translation(elb_pos)); + // colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); + let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic() + .translation(elb_pos - vector![arm_lower_len/2. + radius*2., 0., 0.]) + ); + colliders.insert_with_parent( + lower_arm_coll.clone(), + arm_l_lower, + &mut bodies + ); + colliders.insert_with_parent( + + joint_collider.clone().translation(vector![arm_lower_len/2., 0., 0.]), + arm_l_lower, + &mut bodies + ); + + multibody_joints.insert(arm_l_upper, elbx, elby_j, true); + multibody_joints.insert(elbx, arm_l_lower, elbx_j, true); + } + } + {//right arm let mut rb_builder = RigidBodyBuilder::dynamic(); @@ -182,7 +239,7 @@ pub fn init_world(testbed: &mut Testbed) { arm_r_upper = bodies.insert(rb_builder.clone().translation(upper_arm_pos)); colliders.insert_with_parent( - upper_arm_coll.clone(), + upper_arm_coll, arm_r_upper, &mut bodies ); @@ -209,7 +266,7 @@ pub fn init_world(testbed: &mut Testbed) { .translation(elb_pos + vector![arm_lower_len/2. + radius*2., 0., 0.]) ); colliders.insert_with_parent( - lower_arm_coll.clone(), + lower_arm_coll, arm_r_lower, &mut bodies ); @@ -224,64 +281,6 @@ pub fn init_world(testbed: &mut Testbed) { } } - - {//left arm - let arm_l_upper; - let rb_builder = RigidBodyBuilder::dynamic().translation(vector![-1., leg_len+spine_len, -1.]); - - let upper_arm_pos = spine_end_pos - vector![arm_upper_len/2. + radius, 0., 0.]; - {//upper - let l_shld_j = SphericalJointBuilder::new() - .local_anchor1(point![0., spine_seg_len/2., 0.]) - .local_anchor2(point![arm_upper_len/2., 0., 0.]) - .limits(JointAxis::AngX, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) - .limits(JointAxis::AngY, [-FRAC_PI_2-FRAC_PI_4, FRAC_PI_2]) - .limits(JointAxis::AngZ, [-FRAC_PI_2, FRAC_PI_2]) - .motor(JointAxis::AngX, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngY, 0., target_vel, stiffness, damping) - .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); - - arm_l_upper = bodies.insert(rb_builder.clone().translation(upper_arm_pos)); - colliders.insert_with_parent(upper_arm_coll, arm_l_upper, &mut bodies); - - - multibody_joints.insert(spine_end, arm_l_upper, l_shld_j, true); - } - - {//lower - let elby_j = RevoluteJointBuilder::new(Vector::y_axis()) - .local_anchor1(point![-arm_upper_len/2., 0., 0.]) - .limits([-FRAC_PI_2+FRAC_PI_4, 0.]) - .motor(0., target_vel, stiffness, damping); - let elbx_j = RevoluteJointBuilder::new(Vector::x_axis()) - .local_anchor2(point![arm_lower_len/2., 0., 0.]) - .limits([-FRAC_PI_2, FRAC_PI_2]) - .motor(0., target_vel, stiffness, damping); - - - let elb_pos = upper_arm_pos - vector![arm_upper_len/2. + radius*2., 0., 0.]; - let elbx = bodies.insert(RigidBodyBuilder::dynamic().translation(elb_pos)); - // colliders.insert_with_parent(joint_collider.clone(), elbx, &mut bodies); - let arm_l_lower = bodies.insert(RigidBodyBuilder::dynamic() - .translation(elb_pos - vector![arm_lower_len/2. + radius*2., 0., 0.]) - ); - colliders.insert_with_parent( - lower_arm_coll.clone(), - arm_l_lower, - &mut bodies - ); - colliders.insert_with_parent( - - joint_collider.clone().translation(vector![arm_lower_len/2., 0., 0.]), - arm_l_lower, - &mut bodies - ); - - - multibody_joints.insert(arm_l_upper, elbx, elby_j, true); - multibody_joints.insert(elbx, arm_l_lower, elbx_j, true); - } - } } {//head From 6d5df529c7a1acc40056cea2c3b150cc21b67b89 Mon Sep 17 00:00:00 2001 From: Nilson J Vidanapatiranage Date: Fri, 11 Oct 2024 13:56:18 -0700 Subject: [PATCH 10/10] Proportions --- examples3d/debug_multibody_sleep3.rs | 33 ++++++++++++++-------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/examples3d/debug_multibody_sleep3.rs b/examples3d/debug_multibody_sleep3.rs index 1dc3b8ae4..56c91f805 100644 --- a/examples3d/debug_multibody_sleep3.rs +++ b/examples3d/debug_multibody_sleep3.rs @@ -20,16 +20,19 @@ pub fn init_world(testbed: &mut Testbed) { colliders.insert_with_parent(collider, ground_body, &mut bodies); + //geometry parameters let radius = 0.1; - let head_radius = 0.3; - let head_total_height = 1.0; - let head_half_height = (head_total_height-head_radius*2.)/2.; - let arm_upper_len = 1.; - let arm_lower_len = 1.25; - let spine_len = 4.; + let spine_len = 2.; + let arm_len = spine_len*1.333; + let arm_upper_len = arm_len*0.45; + let arm_lower_len = arm_len - arm_upper_len; + let leg_len = spine_len*1.9; let num_spine_segments = 2; + let neck_len = spine_len/6.; + let head_diameter = (spine_len/2. + neck_len/2.)*0.95; + + let head_radius = head_diameter/2.; let spine_seg_len = spine_len/(1. + num_spine_segments as f32); - let leg_len = 3.05; let leg_upper_len = leg_len * 0.49; let leg_lower_len = leg_len*0.51; let motor_vel = 10f32.to_radians(); @@ -60,6 +63,9 @@ pub fn init_world(testbed: &mut Testbed) { &mut bodies ); + + let leg_l_lower; + let leg_r_lower; {//legs let kneex_j = RevoluteJointBuilder::new(Vector3::x_axis()) .local_anchor1(point![0., -leg_upper_len/2., 0.]) @@ -69,9 +75,6 @@ pub fn init_world(testbed: &mut Testbed) { let upper_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); let lower_leg_collider = ColliderBuilder::capsule_y((leg_upper_len/2.)-radius, radius); - let leg_l_lower; - let leg_r_lower; - {//left leg let l_hip = SphericalJointBuilder::new() .local_anchor1(point![0., -spine_seg_len/2., 0.]) @@ -284,8 +287,6 @@ pub fn init_world(testbed: &mut Testbed) { } {//head - - let neck_len = spine_len/8.; let neck_pos = spine_end_pos + vector![0., neck_len/2. + radius, 0.]; let neck_collider = ColliderBuilder::capsule_y((neck_len/2.)-radius, radius); let neck = bodies.insert(RigidBodyBuilder::dynamic().translation(neck_pos)); @@ -295,8 +296,8 @@ pub fn init_world(testbed: &mut Testbed) { &mut bodies ); - let head_pos = neck_pos + vector![0., neck_len/2. + head_total_height/2. + radius, 0.]; - let head_collider = ColliderBuilder::capsule_y(head_half_height, head_radius); + let head_pos = neck_pos + vector![0., neck_len/2. + head_radius + radius, 0.]; + let head_collider = ColliderBuilder::ball(head_radius); let head = bodies.insert(RigidBodyBuilder::dynamic() .translation(head_pos) ); @@ -310,7 +311,7 @@ pub fn init_world(testbed: &mut Testbed) { .motor(JointAxis::AngZ, 0., target_vel, stiffness, damping); let neck_j = SphericalJointBuilder::new() .local_anchor1(point![0., neck_len/2., 0.]) - .local_anchor2(point![0., -(head_half_height+head_radius)/2., 0.]) + .local_anchor2(point![0., -head_radius, 0.]) .limits(JointAxis::AngX, [-FRAC_PI_4, FRAC_PI_3]) .limits(JointAxis::AngY, [-PI, PI]) .limits(JointAxis::AngZ, [-FRAC_PI_4, FRAC_PI_4]) @@ -328,5 +329,5 @@ pub fn init_world(testbed: &mut Testbed) { testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); let integration_params = testbed.integration_parameters_mut(); integration_params.num_solver_iterations = NonZeroUsize::new(6).unwrap(); - testbed.look_at(point![15., 15., 15.], point![0.0, 2., 0.0]); + testbed.look_at(point![0., body_spawn_loc.y, 15.], point![0.0, body_spawn_loc.y, 0.0]); }