From fb20d72ee29de9311a81aec6eb9f02fd2aa35fc4 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 20 Feb 2022 12:55:00 +0100 Subject: Joint API and joint motors improvements --- examples3d/joints3.rs | 282 +++++++++++++++++++++----------------------------- 1 file changed, 118 insertions(+), 164 deletions(-) (limited to 'examples3d/joints3.rs') diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 02aa5f9..8b2c258 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -11,20 +11,17 @@ fn create_prismatic_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, origin.z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { let z = origin.z + (i + 1) as f32 * shift; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![origin.x, origin.y, z]) - .build(); + let rigid_body = + RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { @@ -33,10 +30,10 @@ fn create_prismatic_joints( UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) }; - let mut prism = JointData::prismatic(axis) + let mut prism = GenericJoint::prismatic(axis) .local_anchor1(point![0.0, 0.0, shift]) .local_anchor2(point![0.0, 0.0, 0.0]) - .limit_axis(JointAxis::X, [-2.0, 2.0]); + .limits(JointAxis::X, [-2.0, 2.0]); impulse_joints.insert(curr_parent, curr_child, prism); @@ -54,20 +51,17 @@ fn create_actuated_prismatic_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, origin.z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { let z = origin.z + (i + 1) as f32 * shift; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![origin.x, origin.y, z]) - .build(); + let rigid_body = + RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { @@ -76,13 +70,13 @@ fn create_actuated_prismatic_joints( UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) }; - let mut prism = JointData::prismatic(axis) + let mut prism = GenericJoint::prismatic(axis) .local_anchor1(point![0.0, 0.0, 0.0]) .local_anchor2(point![0.0, 0.0, -shift]); if i == 1 { prism = prism - .limit_axis(JointAxis::X, [-Real::MAX, 5.0]) + .limits(JointAxis::X, [-Real::MAX, 5.0]) .motor_velocity(JointAxis::X, 1.0, 1.0) // We set a max impulse so that the motor doesn't fight // the limits with large forces. @@ -95,7 +89,7 @@ fn create_actuated_prismatic_joints( // We set a max impulse so that the motor doesn't fight // the limits with large forces. .motor_max_impulse(JointAxis::X, 0.7) - .limit_axis(JointAxis::X, [-2.0, 5.0]); + .limits(JointAxis::X, [-2.0, 5.0]); } impulse_joints.insert(curr_parent, curr_child, prism); @@ -114,11 +108,9 @@ fn create_revolute_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, 0.0]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { @@ -133,11 +125,9 @@ fn create_revolute_joints( let mut handles = [curr_parent; 4]; for k in 0..4 { - let rigid_body = RigidBodyBuilder::new_dynamic() - .position(positions[k]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]); handles[k] = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handles[k], bodies); } @@ -166,63 +156,41 @@ fn create_revolute_joints_with_limits( impulse_joints: &mut ImpulseJointSet, origin: Point, ) { - let ground = bodies.insert( - RigidBodyBuilder::new_static() - .translation(origin.coords) - .build(), - ); + let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); - let platform1 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), - platform1, - bodies, - ); + let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords)); + colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies); let shift = vector![0.0, 0.0, 6.0]; - let platform2 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), - platform2, - bodies, - ); + let platform2 = + bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift)); + colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies); let z = Vector::z_axis(); - let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]); + let mut joint1 = RevoluteJoint::new(z).limits(JointAxis::X, [-0.2, 0.2]); impulse_joints.insert(ground, platform1, joint1); let mut joint2 = RevoluteJoint::new(z) .local_anchor2(shift.into()) - .limit_axis(JointAxis::Z, [-0.2, 0.2]); + .limits(JointAxis::Z, [-0.2, 0.2]); impulse_joints.insert(platform1, platform2, joint2); // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. let cuboid_body1 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + vector![-2.0, 4.0, 0.0]) - .build(), + RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]), ); colliders.insert_with_parent( - ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0), cuboid_body1, bodies, ); let cuboid_body2 = bodies.insert( RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]) - .build(), + .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]), ); colliders.insert_with_parent( - ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0), cuboid_body2, bodies, ); @@ -254,22 +222,20 @@ fn create_fixed_joints( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![ - origin.x + fk * shift, - origin.y, - origin.z + fi * shift - ]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + origin.x + fk * shift, + origin.y, + origin.z + fi * shift + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical joint. if i > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = JointData::fixed().local_anchor2(point![0.0, 0.0, -shift]); + let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -277,7 +243,7 @@ fn create_fixed_joints( if k > 0 { let parent_index = body_handles.len() - 1; let parent_handle = body_handles[parent_index]; - let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = GenericJoint::fixed().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -308,17 +274,19 @@ fn create_ball_joints( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, 0.0, fi * shift * 2.0]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + fk * shift, + 0.0, + fi * shift * 2.0 + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build(); + let collider = ColliderBuilder::capsule_z(rad * 1.25, rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -326,7 +294,7 @@ fn create_ball_joints( if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = GenericJoint::ball().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -343,45 +311,29 @@ fn create_ball_joints_with_limits( ) { let shift = vector![0.0, 0.0, 3.0]; - let ground = bodies.insert( - RigidBodyBuilder::new_static() - .translation(origin.coords) - .build(), - ); + let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); let ball1 = bodies.insert( RigidBodyBuilder::new_dynamic() .translation(origin.coords + shift) - .linvel(vector![20.0, 20.0, 0.0]) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), - ball1, - bodies, + .linvel(vector![20.0, 20.0, 0.0]), ); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies); - let ball2 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift * 2.0) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), - ball2, - bodies, - ); + let ball2 = + bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0)); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies); - let mut joint1 = JointData::ball() + let mut joint1 = GenericJoint::ball() .local_anchor2(Point::from(-shift)) - .limit_axis(JointAxis::X, [-0.2, 0.2]) - .limit_axis(JointAxis::Y, [-0.2, 0.2]); + .limits(JointAxis::X, [-0.2, 0.2]) + .limits(JointAxis::Y, [-0.2, 0.2]); impulse_joints.insert(ground, ball1, joint1); - let mut joint2 = JointData::ball() + let mut joint2 = GenericJoint::ball() .local_anchor2(Point::from(-shift)) - .limit_axis(JointAxis::X, [-0.3, 0.3]) - .limit_axis(JointAxis::Y, [-0.3, 0.3]); + .limits(JointAxis::X, [-0.3, 0.3]) + .limits(JointAxis::Y, [-0.3, 0.3]); impulse_joints.insert(ball1, ball2, joint2); } @@ -418,26 +370,28 @@ fn create_actuated_revolute_joints( let rigid_body = RigidBodyBuilder::new(status) .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift]) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) - .build(); + ; let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build(); + let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad); colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { - let mut joint = joint_template.clone(); + let mut joint = joint_template + .clone() + .motor_model(MotorModel::AccelerationBased); if i % 3 == 1 { - joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1); + joint.set_motor_velocity(JointAxis::AngX, -20.0, 0.1); } else if i == num - 1 { let stiffness = 0.2; let damping = 1.0; - joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping); + jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping); } if i == 1 { joint.local_frame2.translation.vector.y = 2.0; - joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1); + joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1); } impulse_joints.insert(parent_handle, child_handle, joint); @@ -458,7 +412,7 @@ fn create_actuated_ball_joints( let shift = 2.0; // We will reuse this base configuration for all the impulse_joints here. - let joint_template = JointData::ball().local_anchor1(point![0.0, 0.0, shift]); + let joint_template = GenericJoint::ball().local_anchor1(point![0.0, 0.0, shift]); let mut parent_handle = RigidBodyHandle::invalid(); @@ -477,10 +431,10 @@ fn create_actuated_ball_joints( let rigid_body = RigidBodyBuilder::new(status) .translation(vector![origin.x, origin.y, origin.z + fi * shift]) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) - .build(); + ; let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build(); + let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad); colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { @@ -516,40 +470,40 @@ pub fn init_world(testbed: &mut Testbed) { let mut impulse_joints = ImpulseJointSet::new(); let multibody_joints = MultibodyJointSet::new(); - create_prismatic_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![20.0, 5.0, 0.0], - 4, - ); - create_actuated_prismatic_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![25.0, 5.0, 0.0], - 4, - ); - create_revolute_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![20.0, 0.0, 0.0], - 3, - ); - create_revolute_joints_with_limits( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![34.0, 0.0, 0.0], - ); - create_fixed_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![0.0, 10.0, 0.0], - 10, - ); + // create_prismatic_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![20.0, 5.0, 0.0], + // 4, + // ); + // create_actuated_prismatic_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![25.0, 5.0, 0.0], + // 4, + // ); + // create_revolute_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![20.0, 0.0, 0.0], + // 3, + // ); + // create_revolute_joints_with_limits( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![34.0, 0.0, 0.0], + // ); + // create_fixed_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![0.0, 10.0, 0.0], + // 10, + // ); create_actuated_revolute_joints( &mut bodies, &mut colliders, @@ -557,20 +511,20 @@ pub fn init_world(testbed: &mut Testbed) { point![20.0, 10.0, 0.0], 6, ); - create_actuated_ball_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![13.0, 10.0, 0.0], - 3, - ); - create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15); - create_ball_joints_with_limits( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![-5.0, 0.0, 0.0], - ); + // create_actuated_ball_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![13.0, 10.0, 0.0], + // 3, + // ); + // create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15); + // create_ball_joints_with_limits( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![-5.0, 0.0, 0.0], + // ); /* * Set up the testbed. -- cgit