diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index f3abee9..3b853c9 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -12,7 +12,7 @@ env: jobs: build: - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v2 @@ -21,5 +21,7 @@ jobs: git submodule update --init cargo build --verbose - name: Check Format - run: cargo fmt -- --check + run: cargo fmt -- --check + - name: Run Tests + run: cargo test -p rubullet -- --test-threads=1 diff --git a/.gitmodules b/.gitmodules index 893e404..9ac4407 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "bullet3/libbullet3"] - path = bullet3/libbullet3 +[submodule "rubullet-sys/bullet3/libbullet3"] + path = rubullet-sys/bullet3/libbullet3 url = https://github.com/bulletphysics/bullet3.git diff --git a/Cargo.toml b/Cargo.toml index 3726405..1fa2982 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,16 +1,5 @@ -[package] -name = "rubullet" -version = "0.1.0" -authors = ["Nathan Kent "] -edition = "2018" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -nalgebra = "0.21.0" - -[build-dependencies] -cmake = "0.1.42" - -[dev-dependencies] -easy-error = "0.3.1" +[workspace] +members = [ + "rubullet", + "rubullet-sys", + ] diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..36883bc --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Nathan Kent, Marco Boneberger + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.adoc b/README.adoc deleted file mode 100644 index 3c935e7..0000000 --- a/README.adoc +++ /dev/null @@ -1,11 +0,0 @@ -= RuBullet - -This is a Rust implementation of PyBullet. -In other words, it uses the Bullet C API in order to expose a functionality that is similar to PyBullet. -Development is ongoing and functionality is currently limited. - -== Status - -This is currently highly experimental and largely composed of hacks. -Additionally, while this does expose a safe API, the internals are a large heap of unsafe code. -It can serve as a starting point for future work but should not be used in production now. diff --git a/README.md b/README.md new file mode 100644 index 0000000..36d0197 --- /dev/null +++ b/README.md @@ -0,0 +1,79 @@ +![GitHub Workflow Status](https://img.shields.io/github/workflow/status/neachdainn/rubullet/Rust) +# RuBullet + +RuBullet is a Rust implementation of [PyBullet](https://pybullet.org/). +In other words, it uses the [Bullet3](https://github.com/bulletphysics/bullet3) +C API in order to expose a functionality that is similar to PyBullet. +Development is ongoing and functionality is currently limited. + +## Status +Right now RuBullet should cover most of the basic use cases. It can: +* Create a PhysicsClient in either Direct or Gui mode +* Load models from URDF, SDF or MuJoCo files +* Create own models within the simulation +* Control robots in position, velocity or torque mode +* Calculate inverse dynamics, inverse kinematics, jacobians and mass matrices +* Render camera images +* Read information about links and joints +* Change linear and angular damping of joints +* Create GUI sliders, button or put debugging text or lines in the simulation +* Get keyboard and mouse events + +Things which are not implemented yet: +* Collision Detection Queries +* Virtual Reality +* Connect via SHARED_MEMORY, UDP or TCP +* Plugins +* Deformables and Cloth +* Logging and saving states +* Creating constraints +* Set physics engine parameters +* Change other dynamics parameters apart from linear and angular damping + +The API is unstable and subject to change. + +# Example +```rust +use std::{thread, time::Duration}; + +use anyhow::Result; +use nalgebra::{Isometry3, Vector3}; +use rubullet::*; + +fn main() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + + physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + physics_client.set_gravity(Vector3::new(0.0, 0.0, -10.0))?; + + let _plane_id = physics_client.load_urdf("plane.urdf", Default::default())?; + + let cube_start_position = Isometry3::translation(0.0, 0.0, 1.0); + let box_id = physics_client.load_urdf( + "r2d2.urdf", + UrdfOptions { + base_transform: cube_start_position, + ..Default::default() + }, + )?; + + for _ in 0..10000 { + physics_client.step_simulation()?; + thread::sleep(Duration::from_micros(4167)); + } + + let cube_transform = physics_client.get_base_transform(box_id)?; + println!("{}", cube_transform); + + Ok(()) +} +``` + +## Bug reports and Merge Requests +The current development happens as a part of marcbone's master thesis. Therefore, merge requests can not be accepted until +July 5, 2021. We are disabling merge requests until then which sadly also disables issues. If you find any bugs or have suggestions please write an email to +one of the maintainers. + +## License +RuBullet is licensed under MIT + diff --git a/bullet3/libbullet3 b/bullet3/libbullet3 deleted file mode 160000 index 830f0a9..0000000 --- a/bullet3/libbullet3 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 830f0a9565b1829a07e21e2f16be2aa9966bd28c diff --git a/rubullet-sys/Cargo.toml b/rubullet-sys/Cargo.toml new file mode 100644 index 0000000..dd3ae38 --- /dev/null +++ b/rubullet-sys/Cargo.toml @@ -0,0 +1,20 @@ +[package] +name = "rubullet-sys" +version = "0.1.0-alpha" +authors = ["Nathan Kent ", "Marco Boneberger "] +edition = "2018" +license = "MIT" +repository = "https://github.com/neachdainn/rubullet" +description = "Compiles bullet3 and exposes rust bindings to the C API" +categories =["science::robotics","simulation"] +keywords =["pybullet","bullet","bullet3","physics","robotics"] +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[lib] +name = "rubullet_sys" +src = "src/lib.rs" + + +[build-dependencies] +cmake = "0.1.42" + diff --git a/build.rs b/rubullet-sys/build.rs similarity index 88% rename from build.rs rename to rubullet-sys/build.rs index 312be4f..f635a55 100644 --- a/build.rs +++ b/rubullet-sys/build.rs @@ -2,9 +2,11 @@ fn main() { // PyBullet does not enable `BT_THREADSAFE`. I assume this is because of the GIL - PyBullet does // nothing without the GIL locked. We'll make the same guarantee by using Rust's ownership // model. - let dst = cmake::Config::new("bullet3") - //.very_verbose(true) - .build(); + let mut config = &mut cmake::Config::new("bullet3"); + if config.get_profile() != "Release" { + config = config.profile("RelWithDebInfo"); + } + let dst = config.build(); println!("cargo:rustc-link-search=native={}/lib", dst.display()); println!("cargo:rustc-link-lib=static=BulletExampleBrowserLib"); diff --git a/bullet3/CMakeLists.txt b/rubullet-sys/bullet3/CMakeLists.txt similarity index 100% rename from bullet3/CMakeLists.txt rename to rubullet-sys/bullet3/CMakeLists.txt diff --git a/rubullet-sys/bullet3/libbullet3 b/rubullet-sys/bullet3/libbullet3 new file mode 160000 index 0000000..df09fd9 --- /dev/null +++ b/rubullet-sys/bullet3/libbullet3 @@ -0,0 +1 @@ +Subproject commit df09fd9ed37e365ceae884ca7f620b61607dae2e diff --git a/rubullet-sys/src/lib.rs b/rubullet-sys/src/lib.rs new file mode 100644 index 0000000..67c8150 --- /dev/null +++ b/rubullet-sys/src/lib.rs @@ -0,0 +1,1256 @@ +//! Foreign function interface for Bullet C API. +#![allow(non_camel_case_types, non_snake_case, clippy::upper_case_acronyms)] +use std::os::raw::{c_char, c_int, c_uchar}; +use std::ptr::NonNull; + +#[repr(C)] +pub struct b3PhysicsClientHandle__ { + _unused: c_int, +} +pub type b3PhysicsClientHandle = NonNull; + +#[repr(C)] +pub struct b3SharedMemoryCommandHandle__ { + _unused: c_int, +} +pub type b3SharedMemoryCommandHandle = *mut b3SharedMemoryCommandHandle__; + +#[repr(C)] +pub struct b3SharedMemoryStatusHandle__ { + _unused: c_int, +} +pub type b3SharedMemoryStatusHandle = *mut b3SharedMemoryStatusHandle__; +pub const MAX_SDF_BODIES: u32 = 512; +extern "C" { + pub fn b3ConnectPhysicsDirect() -> Option; + pub fn b3CreateInProcessPhysicsServerAndConnect( + argc: c_int, + argv: *mut *mut c_char, + ) -> Option; + pub fn b3CreateInProcessPhysicsServerAndConnectMainThread( + argc: c_int, + argv: *mut *mut c_char, + ) -> Option; + pub fn b3DisconnectSharedMemory(physClient: b3PhysicsClientHandle); + + pub fn b3InitConfigureOpenGLVisualizer( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + pub fn b3ConfigureOpenGLVisualizerSetVisualizationFlags( + commandHandle: b3SharedMemoryCommandHandle, + flag: c_int, + enabled: c_int, + ); + pub fn b3CanSubmitCommand(physClient: b3PhysicsClientHandle) -> c_int; + pub fn b3SubmitClientCommandAndWaitStatus( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + ) -> b3SharedMemoryStatusHandle; + + pub fn b3GetStatusType(statusHandle: b3SharedMemoryStatusHandle) -> c_int; + + pub fn b3GetStatusActualState( + statusHandle: b3SharedMemoryStatusHandle, + bodyUniqueId: *mut c_int, + numDegreeOfFreedomQ: *mut c_int, + numDegreeOfFreedomU: *mut c_int, + rootLocalInertialFrame: *mut *const f64, + actualStateQ: *mut *const f64, + actualStateQdot: *mut *const f64, + jointReactionForces: *mut *const f64, + ) -> c_int; + + pub fn b3GetStatusBodyIndices( + statusHandle: b3SharedMemoryStatusHandle, + bodyIndicesOut: *mut c_int, + bodyIndicesCapacity: c_int, + ) -> c_int; + + pub fn b3GetStatusBodyIndex(statusHandle: b3SharedMemoryStatusHandle) -> c_int; + + pub fn b3InitSyncBodyInfoCommand( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + pub fn b3InitSyncUserDataCommand( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitPhysicsParamCommand( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + pub fn b3PhysicsParamSetGravity( + commandHandle: b3SharedMemoryCommandHandle, + gravx: f64, + gravy: f64, + gravz: f64, + ) -> c_int; + pub fn b3PhysicsParamSetTimeStep( + commandHandle: b3SharedMemoryCommandHandle, + time_step: f64, + ) -> c_int; + pub fn b3PhysicsParamSetRealTimeSimulation( + commandHandle: b3SharedMemoryCommandHandle, + enableRealTimeSimulation: c_int, + ) -> c_int; + pub fn b3InitStepSimulationCommand( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitResetSimulationCommand( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitResetSimulationCommand2( + commandHandle: b3SharedMemoryCommandHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitResetSimulationSetFlags( + commandHandle: b3SharedMemoryCommandHandle, + flags: c_int, + ) -> c_int; + + pub fn b3SetAdditionalSearchPath( + physClient: b3PhysicsClientHandle, + path: *const c_char, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3LoadUrdfCommandInit( + physClient: b3PhysicsClientHandle, + urdfFileName: *const c_char, + ) -> b3SharedMemoryCommandHandle; + pub fn b3LoadUrdfCommandSetFlags( + commandHandle: b3SharedMemoryCommandHandle, + flags: c_int, + ) -> c_int; + pub fn b3LoadUrdfCommandSetStartPosition( + commandHandle: b3SharedMemoryCommandHandle, + startPosX: f64, + startPosY: f64, + startPosZ: f64, + ) -> c_int; + pub fn b3LoadUrdfCommandSetStartOrientation( + commandHandle: b3SharedMemoryCommandHandle, + startOrnX: f64, + startOrnY: f64, + startOrnZ: f64, + startOrnW: f64, + ) -> c_int; + pub fn b3LoadUrdfCommandSetUseMultiBody( + commandHandle: b3SharedMemoryCommandHandle, + useMultiBody: c_int, + ) -> c_int; + pub fn b3LoadUrdfCommandSetUseFixedBase( + commandHandle: b3SharedMemoryCommandHandle, + useFixedBase: c_int, + ) -> c_int; + pub fn b3LoadUrdfCommandSetGlobalScaling( + commandHandle: b3SharedMemoryCommandHandle, + globalScaling: f64, + ) -> c_int; + + pub fn b3RequestActualStateCommandInit( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + pub fn b3RequestActualStateCommandComputeLinkVelocity( + commandHandle: b3SharedMemoryCommandHandle, + computeLinkVelocity: c_int, + ) -> c_int; + pub fn b3RequestActualStateCommandComputeForwardKinematics( + commandHandle: b3SharedMemoryCommandHandle, + computeForwardKinematics: c_int, + ) -> c_int; + pub fn b3GetJointState( + physClient: b3PhysicsClientHandle, + statusHandle: b3SharedMemoryStatusHandle, + jointIndex: c_int, + state: *mut b3JointSensorState, + ) -> c_int; + pub fn b3GetJointStateMultiDof( + physClient: b3PhysicsClientHandle, + statusHandle: b3SharedMemoryStatusHandle, + jointIndex: c_int, + state: *mut b3JointSensorState2, + ) -> c_int; + pub fn b3GetLinkState( + physClient: b3PhysicsClientHandle, + statusHandle: b3SharedMemoryStatusHandle, + linkIndex: c_int, + state: *mut b3LinkState, + ) -> c_int; + pub fn b3InitChangeDynamicsInfo( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + pub fn b3ChangeDynamicsInfoSetLinearDamping( + commandHandle: b3SharedMemoryCommandHandle, + bodyUniqueId: c_int, + linear_damping: f64, + ) -> c_int; + pub fn b3ChangeDynamicsInfoSetAngularDamping( + commandHandle: b3SharedMemoryCommandHandle, + bodyUniqueId: c_int, + linear_damping: f64, + ) -> c_int; + + pub fn b3InitRemoveBodyCommand( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + + #[doc = "return the total number of bodies in the simulation"] + pub fn b3GetNumBodies(physClient: b3PhysicsClientHandle) -> c_int; + + #[doc = " return the body unique id, given the index in range [0 , b3GetNumBodies() )"] + pub fn b3GetBodyUniqueId(physClient: b3PhysicsClientHandle, serialIndex: c_int) -> c_int; + + #[doc = "given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h"] + pub fn b3GetBodyInfo( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + info: *mut b3BodyInfo, + ) -> c_int; + + pub fn b3GetNumJoints(physClient: b3PhysicsClientHandle, bodyUniqueId: c_int) -> c_int; + pub fn b3GetJointInfo( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + jointIndex: c_int, + jointInfo: *mut b3JointInfo, + ); + pub fn b3CreatePoseCommandInit( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + pub fn b3CreatePoseCommandSetBasePosition( + commandHandle: b3SharedMemoryCommandHandle, + startPosX: f64, + startPosY: f64, + startPosZ: f64, + ) -> c_int; + + pub fn b3CreatePoseCommandSetBaseOrientation( + commandHandle: b3SharedMemoryCommandHandle, + startOrnX: f64, + startOrnY: f64, + startOrnZ: f64, + startOrnW: f64, + ) -> c_int; + pub fn b3CreatePoseCommandSetBaseLinearVelocity( + commandHandle: b3SharedMemoryCommandHandle, + linVel: *const f64, + ) -> c_int; + + pub fn b3CreatePoseCommandSetBaseAngularVelocity( + commandHandle: b3SharedMemoryCommandHandle, + angVel: *const f64, + ) -> c_int; + pub fn b3CreatePoseCommandSetJointPosition( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + jointIndex: c_int, + jointPosition: f64, + ) -> c_int; + pub fn b3CreatePoseCommandSetJointVelocity( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + jointIndex: c_int, + jointVelocity: f64, + ) -> c_int; + + pub fn b3ComputeDofCount(physClient: b3PhysicsClientHandle, bodyUniqueId: c_int) -> c_int; + + pub fn b3LoadMJCFCommandInit( + physClient: b3PhysicsClientHandle, + fileName: *const c_char, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3LoadMJCFCommandInit2( + commandHandle: b3SharedMemoryCommandHandle, + fileName: *const c_char, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3LoadMJCFCommandSetFlags(commandHandle: b3SharedMemoryCommandHandle, flags: c_int); + + pub fn b3CalculateInverseDynamicsCommandInit2( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + jointPositionsQ: *const f64, + dofCountQ: c_int, + jointVelocitiesQdot: *const f64, + jointAccelerations: *const f64, + dofCountQdot: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3CalculateInverseDynamicsSetFlags( + commandHandle: b3SharedMemoryCommandHandle, + flags: c_int, + ); + pub fn b3GetStatusInverseDynamicsJointForces( + statusHandle: b3SharedMemoryStatusHandle, + bodyUniqueId: *mut c_int, + dofCount: *mut c_int, + jointForces: *mut f64, + ) -> c_int; + + pub fn b3CalculateMassMatrixCommandInit( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + jointPositionsQ: *const f64, + dofCountQ: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3CalculateMassMatrixSetFlags(commandHandle: b3SharedMemoryCommandHandle, flags: c_int); + + #[doc = "the mass matrix is stored in column-major layout of size dofCount*dofCount"] + pub fn b3GetStatusMassMatrix( + physClient: b3PhysicsClientHandle, + statusHandle: b3SharedMemoryStatusHandle, + dofCount: *mut c_int, + massMatrix: *mut f64, + ) -> c_int; + + pub fn b3CalculateInverseKinematicsCommandInit( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + pub fn b3CalculateInverseKinematicsAddTargetPurePosition( + commandHandle: b3SharedMemoryCommandHandle, + endEffectorLinkIndex: c_int, + targetPosition: *const f64, + ); + pub fn b3CalculateInverseKinematicsAddTargetsPurePosition( + commandHandle: b3SharedMemoryCommandHandle, + numEndEffectorLinkIndices: c_int, + endEffectorIndices: *const c_int, + targetPositions: *const f64, + ); + pub fn b3CalculateInverseKinematicsAddTargetPositionWithOrientation( + commandHandle: b3SharedMemoryCommandHandle, + endEffectorLinkIndex: c_int, + targetPosition: *const f64, + targetOrientation: *const f64, + ); + pub fn b3CalculateInverseKinematicsPosWithNullSpaceVel( + commandHandle: b3SharedMemoryCommandHandle, + numDof: c_int, + endEffectorLinkIndex: c_int, + targetPosition: *const f64, + lowerLimit: *const f64, + upperLimit: *const f64, + jointRange: *const f64, + restPose: *const f64, + ); + pub fn b3CalculateInverseKinematicsPosOrnWithNullSpaceVel( + commandHandle: b3SharedMemoryCommandHandle, + numDof: c_int, + endEffectorLinkIndex: c_int, + targetPosition: *const f64, + targetOrientation: *const f64, + lowerLimit: *const f64, + upperLimit: *const f64, + jointRange: *const f64, + restPose: *const f64, + ); + pub fn b3CalculateInverseKinematicsSetJointDamping( + commandHandle: b3SharedMemoryCommandHandle, + numDof: c_int, + jointDampingCoeff: *const f64, + ); + pub fn b3CalculateInverseKinematicsSelectSolver( + commandHandle: b3SharedMemoryCommandHandle, + solver: c_int, + ); + pub fn b3GetStatusInverseKinematicsJointPositions( + commandHandle: b3SharedMemoryStatusHandle, + bodyUniqueId: *mut c_int, + dofCount: *mut c_int, + jointPositions: *mut f64, + ) -> c_int; + + pub fn b3CalculateInverseKinematicsSetCurrentPositions( + commandHandle: b3SharedMemoryCommandHandle, + numDof: c_int, + currentJointPositions: *const f64, + ); + pub fn b3CalculateInverseKinematicsSetMaxNumIterations( + commandHandle: b3SharedMemoryCommandHandle, + maxNumIterations: c_int, + ); + pub fn b3CalculateInverseKinematicsSetResidualThreshold( + commandHandle: b3SharedMemoryCommandHandle, + residualThreshold: f64, + ); + + pub fn b3CalculateJacobianCommandInit( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + linkIndex: c_int, + localPosition: *const f64, + jointPositionsQ: *const f64, + jointVelocitiesQdot: *const f64, + jointAccelerations: *const f64, + ) -> b3SharedMemoryCommandHandle; + pub fn b3GetStatusJacobian( + statusHandle: b3SharedMemoryStatusHandle, + dofCount: *mut c_int, + linearJacobian: *mut f64, + angularJacobian: *mut f64, + ) -> c_int; + pub fn b3JointControlCommandInit2( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + controlMode: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3JointControlSetDesiredPosition( + commandHandle: b3SharedMemoryCommandHandle, + qIndex: c_int, + value: f64, + ) -> c_int; + pub fn b3JointControlSetKp( + commandHandle: b3SharedMemoryCommandHandle, + dofIndex: c_int, + value: f64, + ) -> c_int; + pub fn b3JointControlSetKd( + commandHandle: b3SharedMemoryCommandHandle, + dofIndex: c_int, + value: f64, + ) -> c_int; + pub fn b3JointControlSetMaximumVelocity( + commandHandle: b3SharedMemoryCommandHandle, + dofIndex: c_int, + maximumVelocity: f64, + ) -> c_int; + + pub fn b3JointControlSetDesiredVelocity( + commandHandle: b3SharedMemoryCommandHandle, + dofIndex: c_int, + value: f64, + ) -> c_int; + pub fn b3JointControlSetMaximumForce( + commandHandle: b3SharedMemoryCommandHandle, + dofIndex: c_int, + value: f64, + ) -> c_int; + pub fn b3JointControlSetDesiredForceTorque( + commandHandle: b3SharedMemoryCommandHandle, + dofIndex: c_int, + value: f64, + ) -> c_int; + pub fn b3InitRequestCameraImage( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + pub fn b3RequestCameraImageSetPixelResolution( + commandHandle: b3SharedMemoryCommandHandle, + width: c_int, + height: c_int, + ); + pub fn b3RequestCameraImageSetCameraMatrices( + commandHandle: b3SharedMemoryCommandHandle, + viewMatrix: *const f32, + projectionMatrix: *const f32, + ); + + pub fn b3GetCameraImageData( + physClient: b3PhysicsClientHandle, + imageData: *mut b3CameraImageData, + ); + + pub fn b3ComputeViewMatrixFromPositions( + cameraPosition: *const f32, + cameraTargetPosition: *const f32, + cameraUp: *const f32, + viewMatrix: *mut f32, + ); + pub fn b3ComputeViewMatrixFromYawPitchRoll( + cameraTargetPosition: *const f32, + distance: f32, + yaw: f32, + pitch: f32, + roll: f32, + upAxis: c_int, + viewMatrix: *mut f32, + ); + pub fn b3ComputePositionFromViewMatrix( + viewMatrix: *const f32, + cameraPosition: *mut f32, + cameraTargetPosition: *mut f32, + cameraUp: *mut f32, + ); + + pub fn b3ComputeProjectionMatrix( + left: f32, + right: f32, + bottom: f32, + top: f32, + nearVal: f32, + farVal: f32, + projectionMatrix: *mut f32, + ); + pub fn b3ComputeProjectionMatrixFOV( + fov: f32, + aspect: f32, + nearVal: f32, + farVal: f32, + projectionMatrix: *mut f32, + ); + + pub fn b3InitUserDebugAddParameter( + physClient: b3PhysicsClientHandle, + txt: *const c_char, + rangeMin: f64, + rangeMax: f64, + startValue: f64, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3GetDebugItemUniqueId(statusHandle: b3SharedMemoryStatusHandle) -> c_int; + + pub fn b3InitUserDebugReadParameter( + physClient: b3PhysicsClientHandle, + debugItemUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3GetStatusDebugParameterValue( + statusHandle: b3SharedMemoryStatusHandle, + paramValue: *mut f64, + ) -> c_int; + #[doc = " Add/remove user-specific debug lines and debug text messages"] + pub fn b3InitUserDebugDrawAddLine3D( + physClient: b3PhysicsClientHandle, + fromXYZ: *const f64, + toXYZ: *const f64, + colorRGB: *const f64, + lineWidth: f64, + lifeTime: f64, + ) -> b3SharedMemoryCommandHandle; + pub fn b3InitUserDebugDrawAddText3D( + physClient: b3PhysicsClientHandle, + txt: *const c_char, + positionXYZ: *const f64, + colorRGB: *const f64, + textSize: f64, + lifeTime: f64, + ) -> b3SharedMemoryCommandHandle; + pub fn b3UserDebugTextSetOptionFlags( + commandHandle: b3SharedMemoryCommandHandle, + optionFlags: c_int, + ); + pub fn b3UserDebugTextSetOrientation( + commandHandle: b3SharedMemoryCommandHandle, + orientation: *const f64, + ); + pub fn b3UserDebugItemSetReplaceItemUniqueId( + commandHandle: b3SharedMemoryCommandHandle, + replaceItem: c_int, + ); + + pub fn b3UserDebugItemSetParentObject( + commandHandle: b3SharedMemoryCommandHandle, + objectUniqueId: c_int, + linkIndex: c_int, + ); + pub fn b3InitUserDebugDrawRemove( + physClient: b3PhysicsClientHandle, + debugItemUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitUserDebugDrawRemoveAll( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + #[doc = " Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates."] + pub fn b3ApplyExternalForceCommandInit( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3ApplyExternalForce( + commandHandle: b3SharedMemoryCommandHandle, + bodyUniqueId: c_int, + linkId: c_int, + force: *const f64, + position: *const f64, + flag: c_int, + ); + pub fn b3ApplyExternalTorque( + commandHandle: b3SharedMemoryCommandHandle, + bodyUniqueId: c_int, + linkId: c_int, + torque: *const f64, + flag: c_int, + ); + + pub fn b3RequestKeyboardEventsCommandInit( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3GetKeyboardEventsData( + physClient: b3PhysicsClientHandle, + keyboardEventsData: *mut b3KeyboardEventsData, + ); + + pub fn b3RequestMouseEventsCommandInit( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3GetMouseEventsData( + physClient: b3PhysicsClientHandle, + mouseEventsData: *mut b3MouseEventsData, + ); + #[doc = "We are currently not reading the sensor information from the URDF file, and programmatically assign sensors."] + #[doc = "This is rather inconsistent, to mix programmatical creation with loading from file."] + pub fn b3CreateSensorCommandInit( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3CreateSensorEnable6DofJointForceTorqueSensor( + commandHandle: b3SharedMemoryCommandHandle, + jointIndex: c_int, + enable: c_int, + ) -> c_int; + pub fn b3InitDebugDrawingCommand( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3SetDebugObjectColor( + commandHandle: b3SharedMemoryCommandHandle, + objectUniqueId: c_int, + linkIndex: c_int, + objectColorRGB: *const f64, + ); + pub fn b3RemoveDebugObjectColor( + commandHandle: b3SharedMemoryCommandHandle, + objectUniqueId: c_int, + linkIndex: c_int, + ); + + #[doc = "the creation of collision shapes and rigid bodies etc is likely going to change,"] + #[doc = "but good to have a b3CreateBoxShapeCommandInit for now"] + pub fn b3CreateCollisionShapeCommandInit( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3CreateCollisionShapeAddSphere( + commandHandle: b3SharedMemoryCommandHandle, + radius: f64, + ) -> c_int; + pub fn b3CreateCollisionShapeAddBox( + commandHandle: b3SharedMemoryCommandHandle, + halfExtents: *const f64, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddCapsule( + commandHandle: b3SharedMemoryCommandHandle, + radius: f64, + height: f64, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddCylinder( + commandHandle: b3SharedMemoryCommandHandle, + radius: f64, + height: f64, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddHeightfield( + commandHandle: b3SharedMemoryCommandHandle, + fileName: *const c_char, + meshScale: *const f64, + textureScaling: f64, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddHeightfield2( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + meshScale: *const f64, + textureScaling: f64, + heightfieldData: *mut f32, + numHeightfieldRows: c_int, + numHeightfieldColumns: c_int, + replaceHeightfieldIndex: c_int, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddPlane( + commandHandle: b3SharedMemoryCommandHandle, + planeNormal: *const f64, + planeConstant: f64, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddMesh( + commandHandle: b3SharedMemoryCommandHandle, + fileName: *const c_char, + meshScale: *const f64, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddConvexMesh( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + meshScale: *const f64, + vertices: *const f64, + numVertices: c_int, + ) -> c_int; + + pub fn b3CreateCollisionShapeAddConcaveMesh( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + meshScale: *const f64, + vertices: *const f64, + numVertices: c_int, + indices: *const c_int, + numIndices: c_int, + ) -> c_int; + + pub fn b3CreateCollisionSetFlag( + commandHandle: b3SharedMemoryCommandHandle, + shapeIndex: c_int, + flags: c_int, + ); + + pub fn b3CreateCollisionShapeSetChildTransform( + commandHandle: b3SharedMemoryCommandHandle, + shapeIndex: c_int, + childPosition: *const f64, + childOrientation: *const f64, + ); + pub fn b3GetStatusCollisionShapeUniqueId(statusHandle: b3SharedMemoryStatusHandle) -> c_int; + + pub fn b3InitRemoveCollisionShapeCommand( + physClient: b3PhysicsClientHandle, + collisionShapeId: c_int, + ) -> b3SharedMemoryCommandHandle; + pub fn b3CreateVisualShapeCommandInit( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3CreateVisualShapeAddSphere( + commandHandle: b3SharedMemoryCommandHandle, + radius: f64, + ) -> c_int; + pub fn b3CreateVisualShapeAddBox( + commandHandle: b3SharedMemoryCommandHandle, + halfExtents: *const f64, + ) -> c_int; + + pub fn b3CreateVisualShapeAddCapsule( + commandHandle: b3SharedMemoryCommandHandle, + radius: f64, + height: f64, + ) -> c_int; + + pub fn b3CreateVisualShapeAddCylinder( + commandHandle: b3SharedMemoryCommandHandle, + radius: f64, + height: f64, + ) -> c_int; + + pub fn b3CreateVisualShapeAddPlane( + commandHandle: b3SharedMemoryCommandHandle, + planeNormal: *const f64, + planeConstant: f64, + ) -> c_int; + + pub fn b3CreateVisualShapeAddMesh( + commandHandle: b3SharedMemoryCommandHandle, + fileName: *const c_char, + meshScale: *const f64, + ) -> c_int; + + pub fn b3CreateVisualShapeAddMesh2( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + meshScale: *const f64, + vertices: *const f64, + numVertices: c_int, + indices: *const c_int, + numIndices: c_int, + normals: *const f64, + numNormals: c_int, + uvs: *const f64, + numUVs: c_int, + ) -> c_int; + + pub fn b3CreateVisualSetFlag( + commandHandle: b3SharedMemoryCommandHandle, + shapeIndex: c_int, + flags: c_int, + ); + + pub fn b3CreateVisualShapeSetChildTransform( + commandHandle: b3SharedMemoryCommandHandle, + shapeIndex: c_int, + childPosition: *const f64, + childOrientation: *const f64, + ); + + pub fn b3CreateVisualShapeSetSpecularColor( + commandHandle: b3SharedMemoryCommandHandle, + shapeIndex: c_int, + specularColor: *const f64, + ); + + pub fn b3CreateVisualShapeSetRGBAColor( + commandHandle: b3SharedMemoryCommandHandle, + shapeIndex: c_int, + rgbaColor: *const f64, + ); + + pub fn b3GetStatusVisualShapeUniqueId(statusHandle: b3SharedMemoryStatusHandle) -> c_int; + + pub fn b3CreateMultiBodyCommandInit( + physClient: b3PhysicsClientHandle, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3CreateMultiBodyBase( + commandHandle: b3SharedMemoryCommandHandle, + mass: f64, + collisionShapeUnique: c_int, + visualShapeUniqueId: c_int, + basePosition: *const f64, + baseOrientation: *const f64, + baseInertialFramePosition: *const f64, + baseInertialFrameOrientation: *const f64, + ) -> c_int; + + pub fn b3CreateMultiBodyLink( + commandHandle: b3SharedMemoryCommandHandle, + linkMass: f64, + linkCollisionShapeIndex: f64, + linkVisualShapeIndex: f64, + linkPosition: *const f64, + linkOrientation: *const f64, + linkInertialFramePosition: *const f64, + linkInertialFrameOrientation: *const f64, + linkParentIndex: c_int, + linkJointType: c_int, + linkJointAxis: *const f64, + ) -> c_int; + + pub fn b3CreateMultiBodySetBatchPositions( + physClient: b3PhysicsClientHandle, + commandHandle: b3SharedMemoryCommandHandle, + batchPositions: *mut f64, + numBatchObjects: c_int, + ) -> c_int; + + pub fn b3CreateMultiBodyUseMaximalCoordinates(commandHandle: b3SharedMemoryCommandHandle); + + pub fn b3CreateMultiBodySetFlags(commandHandle: b3SharedMemoryCommandHandle, flags: c_int); + + pub fn b3InitRequestVisualShapeInformation( + physClient: b3PhysicsClientHandle, + bodyUniqueIdA: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3GetVisualShapeInformation( + physClient: b3PhysicsClientHandle, + visualShapeInfo: *mut b3VisualShapeInformation, + ); + + pub fn b3InitRequestCollisionShapeInformation( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + linkIndex: c_int, + ) -> b3SharedMemoryCommandHandle; + + // pub fn b3GetCollisionShapeInformation( + // physClient: b3PhysicsClientHandle, + // collisionShapeInfo: *mut b3CollisionShapeInformation, + // ); + + pub fn b3InitLoadTexture( + physClient: b3PhysicsClientHandle, + filename: *const c_char, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3GetStatusTextureUniqueId(statusHandle: b3SharedMemoryStatusHandle) -> c_int; + + pub fn b3CreateChangeTextureCommandInit( + physClient: b3PhysicsClientHandle, + textureUniqueId: c_int, + width: c_int, + height: c_int, + rgbPixels: *const c_char, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitUpdateVisualShape( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + jointIndex: c_int, + shapeIndex: c_int, + textureUniqueId: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3InitUpdateVisualShape2( + physClient: b3PhysicsClientHandle, + bodyUniqueId: c_int, + jointIndex: c_int, + shapeIndex: c_int, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3UpdateVisualShapeTexture( + commandHandle: b3SharedMemoryCommandHandle, + textureUniqueId: c_int, + ); + + pub fn b3UpdateVisualShapeRGBAColor( + commandHandle: b3SharedMemoryCommandHandle, + rgbaColor: *const f64, + ); + + pub fn b3UpdateVisualShapeFlags(commandHandle: b3SharedMemoryCommandHandle, flags: c_int); + + pub fn b3UpdateVisualShapeSpecularColor( + commandHandle: b3SharedMemoryCommandHandle, + specularColor: *const f64, + ); + + pub fn b3LoadSdfCommandInit( + physClient: b3PhysicsClientHandle, + sdfFileName: *const c_char, + ) -> b3SharedMemoryCommandHandle; + + pub fn b3LoadSdfCommandSetUseMultiBody( + commandHandle: b3SharedMemoryCommandHandle, + useMultiBody: c_int, + ) -> c_int; + + pub fn b3LoadSdfCommandSetUseGlobalScaling( + commandHandle: b3SharedMemoryCommandHandle, + globalScaling: f64, + ) -> c_int; +} + +#[repr(C)] +#[derive(Clone, Copy)] +pub enum eURDF_Flags { + URDF_USE_INERTIA_FROM_FILE = 2, + URDF_USE_SELF_COLLISION = 8, + URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16, + URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32, + URDF_RESERVED = 64, + URDF_USE_IMPLICIT_CYLINDER = 128, + URDF_GLOBAL_VELOCITIES_MB = 256, + MJCF_COLORS_FROM_FILE = 512, + URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, + URDF_ENABLE_SLEEPING = 2048, + URDF_INITIALIZE_SAT_FEATURES = 4096, + URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, + URDF_PARSE_SENSORS = 16384, + URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768, + URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536, + URDF_MAINTAIN_LINK_ORDER = 131072, + URDF_ENABLE_WAKEUP = 262144, +} + +#[repr(C)] +pub enum EnumSharedMemoryServerStatus { + CMD_SHARED_MEMORY_NOT_INITIALIZED = 0, + CMD_WAITING_FOR_CLIENT_COMMAND, + CMD_CLIENT_COMMAND_COMPLETED, + CMD_UNKNOWN_COMMAND_FLUSHED, + CMD_SDF_LOADING_COMPLETED, + CMD_SDF_LOADING_FAILED, + CMD_URDF_LOADING_COMPLETED, + CMD_URDF_LOADING_FAILED, + CMD_BULLET_LOADING_COMPLETED, + CMD_BULLET_LOADING_FAILED, + CMD_BULLET_SAVING_COMPLETED, + CMD_BULLET_SAVING_FAILED, + CMD_MJCF_LOADING_COMPLETED, + CMD_MJCF_LOADING_FAILED, + CMD_REQUEST_INTERNAL_DATA_COMPLETED, + CMD_REQUEST_INTERNAL_DATA_FAILED, + CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, + CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, + CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, + CMD_RIGID_BODY_CREATION_COMPLETED, + CMD_SET_JOINT_FEEDBACK_COMPLETED, + CMD_ACTUAL_STATE_UPDATE_COMPLETED, + CMD_ACTUAL_STATE_UPDATE_FAILED, + CMD_DEBUG_LINES_COMPLETED, + CMD_DEBUG_LINES_OVERFLOW_FAILED, + CMD_DESIRED_STATE_RECEIVED_COMPLETED, + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + CMD_RESET_SIMULATION_COMPLETED, + CMD_CAMERA_IMAGE_COMPLETED, + CMD_CAMERA_IMAGE_FAILED, + CMD_BODY_INFO_COMPLETED, + CMD_BODY_INFO_FAILED, + CMD_INVALID_STATUS, + CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, + CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, + CMD_CALCULATED_JACOBIAN_COMPLETED, + CMD_CALCULATED_JACOBIAN_FAILED, + CMD_CALCULATED_MASS_MATRIX_COMPLETED, + CMD_CALCULATED_MASS_MATRIX_FAILED, + CMD_CONTACT_POINT_INFORMATION_COMPLETED, + CMD_CONTACT_POINT_INFORMATION_FAILED, + CMD_REQUEST_AABB_OVERLAP_COMPLETED, + CMD_REQUEST_AABB_OVERLAP_FAILED, + CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED, + CMD_CALCULATE_INVERSE_KINEMATICS_FAILED, + CMD_SAVE_WORLD_COMPLETED, + CMD_SAVE_WORLD_FAILED, + CMD_VISUAL_SHAPE_INFO_COMPLETED, + CMD_VISUAL_SHAPE_INFO_FAILED, + CMD_VISUAL_SHAPE_UPDATE_COMPLETED, + CMD_VISUAL_SHAPE_UPDATE_FAILED, + CMD_LOAD_TEXTURE_COMPLETED, + CMD_LOAD_TEXTURE_FAILED, + CMD_USER_DEBUG_DRAW_COMPLETED, + CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, + CMD_USER_DEBUG_DRAW_FAILED, + CMD_USER_CONSTRAINT_COMPLETED, + CMD_USER_CONSTRAINT_INFO_COMPLETED, + CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED, + CMD_REMOVE_USER_CONSTRAINT_COMPLETED, + CMD_CHANGE_USER_CONSTRAINT_COMPLETED, + CMD_REMOVE_USER_CONSTRAINT_FAILED, + CMD_CHANGE_USER_CONSTRAINT_FAILED, + CMD_USER_CONSTRAINT_FAILED, + CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, + CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, + CMD_SYNC_BODY_INFO_COMPLETED, + CMD_SYNC_BODY_INFO_FAILED, + CMD_STATE_LOGGING_COMPLETED, + CMD_STATE_LOGGING_START_COMPLETED, + CMD_STATE_LOGGING_FAILED, + CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED, + CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED, + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED, + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, + CMD_REMOVE_BODY_COMPLETED, + CMD_REMOVE_BODY_FAILED, + CMD_GET_DYNAMICS_INFO_COMPLETED, + CMD_GET_DYNAMICS_INFO_FAILED, + CMD_CREATE_COLLISION_SHAPE_FAILED, + CMD_CREATE_COLLISION_SHAPE_COMPLETED, + CMD_CREATE_VISUAL_SHAPE_FAILED, + CMD_CREATE_VISUAL_SHAPE_COMPLETED, + CMD_CREATE_MULTI_BODY_FAILED, + CMD_CREATE_MULTI_BODY_COMPLETED, + CMD_REQUEST_COLLISION_INFO_COMPLETED, + CMD_REQUEST_COLLISION_INFO_FAILED, + CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, + CMD_CHANGE_TEXTURE_COMMAND_FAILED, + CMD_CUSTOM_COMMAND_COMPLETED, + CMD_CUSTOM_COMMAND_FAILED, + CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED, + CMD_SAVE_STATE_FAILED, + CMD_SAVE_STATE_COMPLETED, + CMD_RESTORE_STATE_FAILED, + CMD_RESTORE_STATE_COMPLETED, + CMD_COLLISION_SHAPE_INFO_COMPLETED, + CMD_COLLISION_SHAPE_INFO_FAILED, + CMD_LOAD_SOFT_BODY_FAILED, + CMD_LOAD_SOFT_BODY_COMPLETED, + + CMD_SYNC_USER_DATA_COMPLETED, + CMD_SYNC_USER_DATA_FAILED, + CMD_REQUEST_USER_DATA_COMPLETED, + CMD_REQUEST_USER_DATA_FAILED, + CMD_ADD_USER_DATA_COMPLETED, + CMD_ADD_USER_DATA_FAILED, + CMD_REMOVE_USER_DATA_COMPLETED, + CMD_REMOVE_USER_DATA_FAILED, + CMD_REMOVE_STATE_COMPLETED, + CMD_REMOVE_STATE_FAILED, + + CMD_REQUEST_MESH_DATA_COMPLETED, + CMD_REQUEST_MESH_DATA_FAILED, + + CMD_MAX_SERVER_COMMANDS, +} + +#[repr(C)] +#[derive(Debug)] +pub struct b3JointInfo { + pub m_link_name: [c_char; 1024], + pub m_joint_name: [c_char; 1024], + pub m_joint_type: i32, + pub m_q_index: i32, + pub m_u_index: i32, + pub m_joint_index: i32, + pub m_flags: i32, + pub m_joint_damping: f64, + pub m_joint_friction: f64, + pub m_joint_lower_limit: f64, + pub m_joint_upper_limit: f64, + pub m_joint_max_force: f64, + pub m_joint_max_velocity: f64, + pub m_parent_frame: [f64; 7], + pub m_child_frame: [f64; 7], + pub m_joint_axis: [f64; 3], + pub m_parent_index: i32, + pub m_q_size: i32, + pub m_u_size: i32, +} + +#[repr(C)] +#[derive(Debug, Default, Copy, Clone)] +pub struct b3JointSensorState { + pub m_joint_position: f64, + pub m_joint_velocity: f64, + pub m_joint_force_torque: [f64; 6], + pub m_joint_motor_torque: f64, +} + +#[repr(C)] +#[derive(Debug, Default)] +pub struct b3JointSensorState2 { + pub m_joint_position: [f64; 4], + pub m_joint_velocity: [f64; 3], + pub m_joint_reaction_force_torque: [f64; 6], + pub m_joint_motor_torque_multi_dof: [f64; 3], + pub m_q_dof_size: c_int, + pub m_u_dof_size: c_int, +} + +#[repr(C)] +#[derive(Debug, Default)] +pub struct b3LinkState { + pub m_world_position: [f64; 3], + pub m_world_orientation: [f64; 4], + pub m_local_inertial_position: [f64; 3], + pub m_local_inertial_orientation: [f64; 4], + pub m_world_link_frame_position: [f64; 3], + pub m_world_link_frame_orientation: [f64; 4], + ///only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity) + pub m_world_linear_velocity: [f64; 3], + ///only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity) + pub m_world_angular_velocity: [f64; 3], + + pub m_world_aabb_min: [f64; 3], + pub m_world_aabb_max: [f64; 3], +} + +#[repr(C)] +#[derive(Copy, Clone)] +pub struct b3BodyInfo { + pub m_baseName: [c_char; 1024usize], + pub m_bodyName: [c_char; 1024usize], +} +#[repr(C)] +#[derive(Debug)] +pub struct b3CameraImageData { + pub m_pixel_width: c_int, + pub m_pixel_height: c_int, + pub m_rgb_color_data: *const c_uchar, + pub m_depth_values: *const f32, + pub m_segmentation_mask_values: *const c_int, +} + +impl Default for b3CameraImageData { + fn default() -> Self { + b3CameraImageData { + m_pixel_width: 0, + m_pixel_height: 0, + m_rgb_color_data: &(0_u8), + m_depth_values: &(0_f32), + m_segmentation_mask_values: &0, + } + } +} + +#[repr(C)] +#[derive(Debug, Copy, Clone)] +pub struct b3KeyboardEventsData { + pub m_numKeyboardEvents: c_int, + pub m_keyboardEvents: *mut b3KeyboardEvent, +} + +impl Default for b3KeyboardEventsData { + fn default() -> Self { + b3KeyboardEventsData { + m_numKeyboardEvents: 0, + m_keyboardEvents: [].as_mut_ptr(), + } + } +} + +#[repr(C)] +#[derive(Debug, Copy, Clone, Default)] +pub struct b3KeyboardEvent { + pub m_keyCode: c_int, + pub m_keyState: c_int, +} + +#[repr(C)] +#[derive(Debug, Copy, Clone)] +pub struct b3MouseEventsData { + pub m_numMouseEvents: c_int, + pub m_mouseEvents: *mut b3MouseEvent, +} + +impl Default for b3MouseEventsData { + fn default() -> Self { + b3MouseEventsData { + m_numMouseEvents: 0, + m_mouseEvents: [].as_mut_ptr(), + } + } +} + +#[repr(C)] +#[derive(Debug, Copy, Clone)] +pub struct b3MouseEvent { + pub m_eventType: c_int, + pub m_mousePosX: f32, + pub m_mousePosY: f32, + pub m_buttonIndex: c_int, + pub m_buttonState: c_int, +} + +pub const B3_MAX_NUM_INDICES: usize = if cfg!(target_os = "macos") { + 32768 +} else { + 524288 +}; +pub const B3_MAX_NUM_VERTICES: usize = if cfg!(target_os = "macos") { + 8192 +} else { + 131072 +}; +#[repr(C)] +#[derive(Debug, Copy, Clone)] +pub struct b3VisualShapeInformation { + pub m_numVisualShapes: c_int, + pub m_visualShapeData: *mut b3VisualShapeData, +} +impl Default for b3VisualShapeInformation { + fn default() -> Self { + b3VisualShapeInformation { + m_numVisualShapes: 0, + m_visualShapeData: [].as_mut_ptr(), + } + } +} +impl Default for b3VisualShapeData { + fn default() -> Self { + b3VisualShapeData { + m_objectUniqueId: 0, + m_linkIndex: 0, + m_visualGeometryType: 0, + m_dimensions: [0.; 3], + m_meshAssetFileName: [0; 1024], + m_localVisualFrame: [0.; 7], + m_rgbaColor: [0.; 4], + m_tinyRendererTextureId: 0, + m_textureUniqueId: 0, + m_openglTextureId: 0, + } + } +} +#[repr(C)] +#[derive(Copy, Clone)] +pub struct b3VisualShapeData { + pub m_objectUniqueId: c_int, + pub m_linkIndex: c_int, + pub m_visualGeometryType: c_int, + pub m_dimensions: [f64; 3usize], + pub m_meshAssetFileName: [c_char; 1024usize], + pub m_localVisualFrame: [f64; 7usize], + pub m_rgbaColor: [f64; 4usize], + pub m_tinyRendererTextureId: c_int, + pub m_textureUniqueId: c_int, + pub m_openglTextureId: c_int, +} diff --git a/rubullet/Cargo.toml b/rubullet/Cargo.toml new file mode 100644 index 0000000..ddc5c4c --- /dev/null +++ b/rubullet/Cargo.toml @@ -0,0 +1,55 @@ +[package] +name = "rubullet" +version = "0.1.0-alpha" +authors = ["Nathan Kent ", "Marco Boneberger "] +edition = "2018" +license = "MIT" +repository = "https://github.com/neachdainn/rubullet" +description = "Rust interface to the Bullet Physics SDK simmilar to PyBullet" +categories =["science::robotics","simulation"] +keywords =["pybullet","bullet","bullet3","physics","robotics"] +readme = "../README.md" +documentation = "https://docs.rs/rubullet/" +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[lib] +name = "rubullet" +path = "src/lib.rs" + +[[example]] +name = "panda_demo" +path = "examples/panda_demo.rs" + +[[example]] +name = "panda_camera_demo" +path = "examples/panda_camera_demo.rs" + +[[example]] +name = "heightfield" +path = "examples/heightfield.rs" + +[[example]] +name = "create_visual_shape" +path = "examples/create_visual_shape.rs" + +[[example]] +name = "create_multi_body_batch" +path = "examples/create_multi_body_batch.rs" + +[[example]] +name = "hello_rubullet" +path = "examples/hello_rubullet.rs" + +[[example]] +name = "inverse_dynamics" +path = "examples/inverse_dynamics.rs" + +[dependencies] +nalgebra = "0.23" +image = "0.23.12" +rubullet-sys = { path = "../rubullet-sys" } +bitflags = "1.2.1" + +[dev-dependencies] +anyhow = "1.0" +rand = "0.8.2" diff --git a/rubullet/examples/create_multi_body_batch.rs b/rubullet/examples/create_multi_body_batch.rs new file mode 100644 index 0000000..e52e52f --- /dev/null +++ b/rubullet/examples/create_multi_body_batch.rs @@ -0,0 +1,170 @@ +//! An introduction to the usage of RuBullet. +use std::time::Duration; + +use anyhow::Result; +use nalgebra::{Isometry3, Point3, Vector3}; +use rubullet::DebugVisualizerFlag::{CovEnableGui, CovEnableRendering, CovEnableTinyRenderer}; +use rubullet::*; + +fn main() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + + physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + physics_client.set_time_step(Duration::from_secs_f64(1. / 120.)); + let _plane_id = physics_client.load_urdf("plane100.urdf", None)?; + + physics_client.configure_debug_visualizer(CovEnableRendering, false); + physics_client.configure_debug_visualizer(CovEnableGui, false); + physics_client.configure_debug_visualizer(CovEnableTinyRenderer, false); + + let shift = Isometry3::translation(0.0, -0.02, 0.0); + let mesh_scaling = Vector3::from_element(0.1); + + let vertices = vec![ + [-1., -1., 1.], + [1., -1., 1.], + [1., 1., 1.], + [-1., 1., 1.], + [-1., -1., -1.], + [1., -1., -1.], + [1., 1., -1.], + [-1., 1., -1.], + [-1., -1., -1.], + [-1., 1., -1.], + [-1., 1., 1.], + [-1., -1., 1.], + [1., -1., -1.], + [1., 1., -1.], + [1., 1., 1.], + [1., -1., 1.], + [-1., -1., -1.], + [-1., -1., 1.], + [1., -1., 1.], + [1., -1., -1.], + [-1., 1., -1.], + [-1., 1., 1.], + [1., 1., 1.], + [1., 1., -1.], + ]; + let normals = vec![ + [0., 0., 1.], + [0., 0., 1.], + [0., 0., 1.], + [0., 0., 1.], + [0., 0., -1.], + [0., 0., -1.], + [0., 0., -1.], + [0., 0., -1.], + [-1., 0., 0.], + [-1., 0., 0.], + [-1., 0., 0.], + [-1., 0., 0.], + [1., 0., 0.], + [1., 0., 0.], + [1., 0., 0.], + [1., 0., 0.], + [0., -1., 0.], + [0., -1., 0.], + [0., -1., 0.], + [0., -1., 0.], + [0., 1., 0.], + [0., 1., 0.], + [0., 1., 0.], + [0., 1., 0.], + ]; + + let uvs = vec![ + [0.75, 0.25], + [1., 0.25], + [1., 0.], + [0.75, 0.], + [0.5, 0.25], + [0.25, 0.25], + [0.25, 0.], + [0.5, 0.], + [0.5, 0.], + [0.75, 0.], + [0.75, 0.25], + [0.5, 0.25], + [0.25, 0.5], + [0.25, 0.25], + [0., 0.25], + [0., 0.5], + [0.25, 0.5], + [0.25, 0.25], + [0.5, 0.25], + [0.5, 0.5], + [0., 0.], + [0., 0.25], + [0.25, 0.25], + [0.25, 0.], + ]; + let indices = vec![ + 0, 1, 2, 0, 2, 3, //ground face + 6, 5, 4, 7, 6, 4, //top face + 10, 9, 8, 11, 10, 8, 12, 13, 14, 12, 14, 15, 18, 17, 16, 19, 18, 16, 20, 21, 22, 20, 22, + 23, + ]; + + let visual_shape_id = physics_client.create_visual_shape( + GeometricVisualShape::Mesh { + mesh_scaling: Some(mesh_scaling), + vertices, + indices, + uvs: Some(uvs), + normals: Some(normals), + }, + VisualShapeOptions { + specular_colors: [0.4, 0.4, 0.], + frame_offset: shift, + ..Default::default() + }, + )?; + let collision_shape_id = physics_client.create_collision_shape( + GeometricCollisionShape::Box { + half_extents: mesh_scaling, + }, + Isometry3::identity(), + )?; + + let tex_uid = physics_client.load_texture("tex256.png")?; + let mut batch_positions = Vec::with_capacity(32 * 32 * 10 * 3); + for x in 0..32 { + for y in 0..32 { + for z in 0..10 { + batch_positions.push(Point3::new( + x as f64 * mesh_scaling[0] * 5.5, + y as f64 * mesh_scaling[1] * 5.5, + (0.5 + z as f64) * mesh_scaling[2] * 2.5, + )); + } + } + } + + let body_id = physics_client.create_multi_body( + collision_shape_id, + visual_shape_id, + MultiBodyOptions { + base_pose: Isometry3::translation(0., 0., 2.), + use_maximal_coordinates: true, + + batch_positions: Some(batch_positions), + ..Default::default() + }, + )?; + + physics_client.change_visual_shape( + body_id, + None, + ChangeVisualShapeOptions { + texture_id: Some(tex_uid), + ..Default::default() + }, + )?; + physics_client.configure_debug_visualizer(CovEnableRendering, true); + physics_client.set_gravity(Vector3::new(0.0, 0.0, -10.0))?; + + loop { + physics_client.step_simulation()?; + } +} diff --git a/rubullet/examples/create_visual_shape.rs b/rubullet/examples/create_visual_shape.rs new file mode 100644 index 0000000..8673061 --- /dev/null +++ b/rubullet/examples/create_visual_shape.rs @@ -0,0 +1,64 @@ +//! An introduction to the usage of RuBullet. +use std::{thread, time::Duration}; + +use anyhow::Result; +use nalgebra::{Isometry3, Vector3}; +use rubullet::*; +use std::path::PathBuf; + +fn main() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + + physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + physics_client.set_gravity(Vector3::new(0.0, 0.0, -10.0))?; + physics_client.set_time_step(Duration::from_secs_f64(1. / 120.)); + // physics_client.configure_debug_visualizer(DebugVisualizerFlag::) + let _plane_id = physics_client.load_urdf("plane100.urdf", None)?; + let shift = Isometry3::translation(0.0, -0.02, 0.0); + let visual_shape = physics_client.create_visual_shape( + GeometricVisualShape::MeshFile { + filename: PathBuf::from("duck.obj"), + mesh_scaling: Some(Vector3::from_element(0.1)), + }, + VisualShapeOptions { + specular_colors: [0.4, 0.4, 0.], + frame_offset: shift.clone(), + ..Default::default() + }, + )?; + let collision_shape = physics_client.create_collision_shape( + GeometricCollisionShape::MeshFile { + filename: PathBuf::from("duck_vhacd.obj"), + mesh_scaling: Some(Vector3::from_element(0.1)), + flags: None, + }, + shift, + )?; + let mesh_scaling = Vector3::from_element(0.1); + let rangex = 1; + let rangey = 1; + for i in 0..rangex { + for j in 0..rangey { + let _duck = physics_client.create_multi_body( + collision_shape, + visual_shape, + MultiBodyOptions { + base_pose: Isometry3::translation( + ((-rangex as f64 / 2.) + i as f64) * mesh_scaling[0] * 2., + ((-rangey as f64 / 2.) + j as f64) * mesh_scaling[1] * 2., + 1., + ), + base_mass: 1., + ..Default::default() + }, + )?; + } + } + + physics_client.set_real_time_simulation(true); + for _ in 0..10000 { + thread::sleep(Duration::from_secs_f64(1. / 240.)); + } + + Ok(()) +} diff --git a/rubullet/examples/heightfield.rs b/rubullet/examples/heightfield.rs new file mode 100644 index 0000000..f673868 --- /dev/null +++ b/rubullet/examples/heightfield.rs @@ -0,0 +1,143 @@ +//! An introduction to the usage of RuBullet. +use std::time::Duration; + +use anyhow::Result; +use nalgebra::{Isometry3, Translation3, UnitQuaternion, Vector3}; +use rand::prelude::*; +use rubullet::DebugVisualizerFlag::CovEnableRendering; +use rubullet::*; + +fn main() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + + physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + physics_client.configure_debug_visualizer(CovEnableRendering, false); + let height_pertubation_range = 0.05; + let mut rng = thread_rng(); + let num_heightfield_rows = 256; + let num_heightfield_columns = 256; + let mut heightfield_data = vec![0_f32; num_heightfield_rows * num_heightfield_columns]; + for j in 0..num_heightfield_columns / 2 { + for i in 0..num_heightfield_rows / 2 { + let height: f32 = rng.gen_range((0.)..height_pertubation_range); + heightfield_data[2 * i + 2 * j * num_heightfield_rows] = height; + heightfield_data[2 * i + 1 + 2 * j * num_heightfield_rows] = height; + heightfield_data[2 * i + (2 * j + 1) * num_heightfield_rows] = height; + heightfield_data[2 * i + 1 + (2 * j + 1) * num_heightfield_rows] = height; + } + } + let terrain_shape = physics_client.create_collision_shape( + GeometricCollisionShape::Heightfield { + mesh_scaling: Some(Vector3::new(0.05, 0.05, 1.)), + texture_scaling: (num_heightfield_rows - 1) as f64 / 2., + data: heightfield_data, + num_rows: num_heightfield_rows, + num_columns: num_heightfield_columns, + replace_heightfield: None, + }, + Isometry3::identity(), + )?; + + let terrain = physics_client.create_multi_body( + terrain_shape, + VisualId::NONE, + MultiBodyOptions::default(), + )?; + + physics_client.change_visual_shape( + terrain, + None, + ChangeVisualShapeOptions { + rgba_color: Some([1.; 4]), + ..Default::default() + }, + )?; + + let sphere_radius = 0.05; + + let col_sphere_id = physics_client.create_collision_shape( + GeometricCollisionShape::Sphere { + radius: sphere_radius, + }, + Isometry3::identity(), + )?; + let col_box_id = physics_client.create_collision_shape( + GeometricCollisionShape::Box { + half_extents: Vector3::from_element(sphere_radius), + }, + Isometry3::identity(), + )?; + let mass = 1.; + let visual_shape_id = VisualId::NONE; + + for i in 0..3 { + for j in 0..3 { + for k in 0..3 { + let base_pose = Isometry3::from_parts( + Translation3::new( + i as f64 * 5. * sphere_radius, + j as f64 * 5. * sphere_radius, + 1. + k as f64 * 5. * sphere_radius + 1., + ), + UnitQuaternion::identity(), + ); + let sphere_uid = { + if k & 2 != 0 { + physics_client.create_multi_body( + col_sphere_id, + visual_shape_id, + MultiBodyOptions { + base_mass: mass, + base_pose, + ..Default::default() + }, + )? + } else { + let link_masses = vec![1.]; + let link_collision_shapes = vec![col_box_id]; + let link_visual_shapes = vec![VisualId::NONE]; + let link_poses = vec![Isometry3::translation(0., 0., 0.11)]; + let link_inertial_frame_poses = vec![Isometry3::translation(0., 0., 0.)]; + let indices = vec![0]; + let joint_types = vec![JointType::Revolute]; + let axis = vec![Vector3::new(0., 0., 1.)]; + + physics_client.create_multi_body( + col_box_id, + visual_shape_id, + MultiBodyOptions { + base_mass: mass, + base_pose, + link_masses, + link_collision_shapes, + link_visual_shapes, + link_poses, + link_inertial_frame_poses, + link_parent_indices: indices, + link_joint_types: joint_types, + link_joint_axis: axis, + ..Default::default() + }, + )? + } + }; + + for joint in 0..physics_client.get_num_joints(sphere_uid) { + physics_client.set_joint_motor_control( + sphere_uid, + joint, + ControlMode::Velocity(1.), + Some(10.), + ); + } + } + } + } + physics_client.configure_debug_visualizer(CovEnableRendering, true); + physics_client.set_gravity(Vector3::new(0.0, 0.0, -10.0))?; + physics_client.set_real_time_simulation(true); + + loop { + std::thread::sleep(Duration::from_secs_f64(0.01)); + } +} diff --git a/examples/hello_rubullet.rs b/rubullet/examples/hello_rubullet.rs similarity index 77% rename from examples/hello_rubullet.rs rename to rubullet/examples/hello_rubullet.rs index 708d3f9..10a02cb 100644 --- a/examples/hello_rubullet.rs +++ b/rubullet/examples/hello_rubullet.rs @@ -1,17 +1,17 @@ //! An introduction to the usage of RuBullet. use std::{thread, time::Duration}; -use easy_error::Terminator; +use anyhow::Result; use nalgebra::{Isometry3, Vector3}; use rubullet::*; -fn main() -> Result<(), Terminator> { +fn main() -> Result<()> { let mut physics_client = PhysicsClient::connect(Mode::Gui)?; - physics_client.set_additional_search_path("bullet3/libbullet3/data")?; + physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; physics_client.set_gravity(Vector3::new(0.0, 0.0, -10.0))?; - let plane_id = physics_client.load_urdf("plane.urdf", Default::default())?; + let _plane_id = physics_client.load_urdf("plane.urdf", None)?; let cube_start_position = Isometry3::translation(0.0, 0.0, 1.0); let box_id = physics_client.load_urdf( diff --git a/rubullet/examples/inverse_dynamics.rs b/rubullet/examples/inverse_dynamics.rs new file mode 100644 index 0000000..ebfdb1b --- /dev/null +++ b/rubullet/examples/inverse_dynamics.rs @@ -0,0 +1,86 @@ +use std::f64::consts::PI; +use std::time::Duration; + +use anyhow::Result; + +use rubullet::ControlModeArray::Torques; +use rubullet::*; + +fn main() -> Result<()> { + let delta_t = Duration::from_secs_f64(0.0001); + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + physics_client.set_time_step(delta_t); + let id_revolute_joints = [0, 3]; + let id_robot = physics_client.load_urdf( + "TwoJointRobot_w_fixedJoints.urdf", + UrdfOptions { + use_fixed_base: true, + ..Default::default() + }, + )?; + physics_client.change_dynamics_angular_damping(id_robot, 0.); + physics_client.change_dynamics_linear_damping(id_robot, 0.); + physics_client.set_joint_motor_control_array( + id_robot, + &id_revolute_joints, + ControlModeArray::Velocities(&[0., 0.]), + Some(&[0., 0.]), + )?; + // Target Positions: + let start = 0.; + let end = 1.; + + let steps = ((end - start) / delta_t.as_secs_f64()) as usize; + let mut t = vec![0.; steps]; + + let mut q_pos_desired = vec![vec![0.; steps]; 2]; + let mut q_vel_desired = vec![vec![0.; steps]; 2]; + let mut q_acc_desired = vec![vec![0.; steps]; 2]; + + for s in 0..steps { + t[s] = start + s as f64 * delta_t.as_secs_f64(); + q_pos_desired[0][s] = 1. / (2. * PI) * f64::sin(2. * PI * t[s]) - t[s]; + q_pos_desired[1][s] = -1. / (2. * PI) * f64::sin(2. * PI * t[s]) - 1.; + + q_vel_desired[0][s] = f64::cos(2. * PI * t[s]) - 1.; + q_vel_desired[1][s] = f64::sin(2. * PI * t[s]); + + q_acc_desired[0][s] = -2. * PI * f64::sin(2. * PI * t[s]); + q_acc_desired[1][s] = 2. * PI * f64::cos(2. * PI * t[s]); + } + let mut q_pos = vec![vec![0.; steps]; 2]; + let mut q_vel = vec![vec![0.; steps]; 2]; + let mut q_tor = vec![vec![0.; steps]; 2]; + + for i in 0..t.len() { + let joint_states = physics_client.get_joint_states(id_robot, &[0, 3])?; + q_pos[0][1] = joint_states[0].joint_position; + let a = joint_states[1].joint_position; + q_pos[1][i] = a; + + q_vel[0][i] = joint_states[0].joint_velocity; + q_vel[1][i] = joint_states[1].joint_velocity; + + let obj_pos = [q_pos[0][i], q_pos[1][i]]; + let obj_vel = [q_vel[0][i], q_vel[1][i]]; + let obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]; + + let torque = + physics_client.calculate_inverse_dynamics(id_robot, &obj_pos, &obj_vel, &obj_acc)?; + + q_tor[0][i] = torque[0]; + q_tor[1][i] = torque[1]; + + physics_client.set_joint_motor_control_array( + id_robot, + &id_revolute_joints, + Torques(&torque), + None, + )?; + physics_client.step_simulation()?; + std::thread::sleep(delta_t); + } + + Ok(()) +} diff --git a/rubullet/examples/jacobian.rs b/rubullet/examples/jacobian.rs new file mode 100644 index 0000000..27f5f72 --- /dev/null +++ b/rubullet/examples/jacobian.rs @@ -0,0 +1,172 @@ +use nalgebra::{Isometry3, Matrix3xX, Vector3}; +use rubullet::Mode::Direct; +use rubullet::{BodyId, ControlModeArray, JointInfo, JointState, PhysicsClient, UrdfOptions}; +use std::time::Duration; + +use anyhow::Result; + +pub fn set_joint_positions(client: &mut PhysicsClient, robot: BodyId, position: &[f64]) { + let num_joints = client.get_num_joints(robot); + assert_eq!(num_joints, position.len()); + let indices = (0..num_joints).into_iter().collect::>(); + let zero_vec = vec![0.; num_joints]; + let position_gains = vec![1.; num_joints]; + let velocity_gains = vec![0.3; num_joints]; + client + .set_joint_motor_control_array( + robot, + indices.as_slice(), + ControlModeArray::PositionsWithPd { + target_positions: position, + target_velocities: zero_vec.as_slice(), + position_gains: position_gains.as_slice(), + velocity_gains: velocity_gains.as_slice(), + }, + None, + ) + .unwrap(); +} + +pub fn get_joint_states( + client: &mut PhysicsClient, + robot: BodyId, +) -> (Vec, Vec, Vec) { + let num_joints = client.get_num_joints(robot); + let indices = (0..num_joints).into_iter().collect::>(); + let joint_states = client.get_joint_states(robot, indices.as_slice()).unwrap(); + let pos = joint_states + .iter() + .map(|x| x.joint_position) + .collect::>(); + let vel = joint_states + .iter() + .map(|x| x.joint_velocity) + .collect::>(); + let torque = joint_states + .iter() + .map(|x| x.joint_motor_torque) + .collect::>(); + (pos, vel, torque) +} + +pub fn get_motor_joint_states( + client: &mut PhysicsClient, + robot: BodyId, +) -> (Vec, Vec, Vec) { + let num_joints = client.get_num_joints(robot); + let indices = (0..num_joints).into_iter().collect::>(); + let joint_states = client.get_joint_states(robot, indices.as_slice()).unwrap(); + let joint_infos: Vec = (0..num_joints) + .into_iter() + .map(|y| client.get_joint_info(robot, y)) + .collect::>(); + let joint_states = joint_states + .iter() + .zip(joint_infos.iter()) + .filter(|(_, i)| i.q_index > -1) + .map(|(j, _)| *j) + .collect::>(); + let pos = joint_states + .iter() + .map(|x| x.joint_position) + .collect::>(); + let vel = joint_states + .iter() + .map(|x| x.joint_velocity) + .collect::>(); + let torque = joint_states + .iter() + .map(|x| x.joint_motor_torque) + .collect::>(); + (pos, vel, torque) +} + +pub fn multiply_jacobian( + client: &mut PhysicsClient, + robot: BodyId, + jacobian: &Matrix3xX, + vector: &[f64], +) -> Vector3 { + let mut result = Vector3::new(0., 0., 0.); + let mut i = 0; + for c in 0..vector.len() { + if client.get_joint_info(robot, c).q_index > -1 { + for r in 0..3 { + result[r] += jacobian[(r, i)] * vector[c]; + } + i += 1; + } + } + result +} + +fn main() -> Result<()> { + let delta_t = Duration::from_secs_f64(0.001); + let mut p = PhysicsClient::connect(Direct).unwrap(); + p.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + let gravity_constant = -9.81; + p.set_time_step(delta_t); + p.set_gravity(Vector3::new(0., 0., gravity_constant))?; + p.load_urdf( + "plane.urdf", + UrdfOptions { + base_transform: Isometry3::translation(0., 0., -0.3), + ..Default::default() + }, + )?; + + let kuka_id = p.load_urdf( + "TwoJointRobot_w_fixedJoints.urdf", + UrdfOptions { + use_fixed_base: true, + ..Default::default() + }, + )?; + // let kuka_id = p.load_urdf("kuka_iiwa/model.urdf", UrdfOptions::default())?; + // let kuka_id = p.load_urdf("kuka_lwr/kuka.urdf", UrdfOptions::default())?; + let num_joints = p.get_num_joints(kuka_id); + let kuka_end_effector_index = num_joints - 1; + + set_joint_positions(&mut p, kuka_id, vec![0.1; num_joints].as_slice()); + p.step_simulation()?; + + let (_pos, vel, _torq) = get_joint_states(&mut p, kuka_id); + let (mpos, _mvel, _mtorq) = get_motor_joint_states(&mut p, kuka_id); + let result = p.get_link_state(kuka_id, kuka_end_effector_index, true, true)?; + + let zero_vec = vec![0.; mpos.len()]; + let jacobian = p.calculate_jacobian( + kuka_id, + kuka_end_effector_index, + result.local_inertial_pose.translation, + mpos.as_slice(), + zero_vec.as_slice(), + zero_vec.as_slice(), + )?; + println!("Link linear velocity of CoM from getLinkState:"); + println!("{:?}", result.get_linear_world_velocity()); + println!("Link linear velocity of CoM from linearJacobian * q_dot:"); + println!( + "{:?}", + multiply_jacobian( + &mut p, + kuka_id, + &jacobian.get_linear_jacobian(), + vel.as_slice() + ) + ); + println!("Link angular velocity of CoM from getLinkState:"); + println!("{:?}", result.get_angular_world_velocity()); + println!("Link angular velocity of CoM from angularJacobian * q_dot:"); + println!( + "{:?}", + multiply_jacobian( + &mut p, + kuka_id, + &jacobian.get_angular_jacobian(), + vel.as_slice() + ) + ); + + Ok(()) +} diff --git a/rubullet/examples/panda_camera_demo.rs b/rubullet/examples/panda_camera_demo.rs new file mode 100644 index 0000000..5096951 --- /dev/null +++ b/rubullet/examples/panda_camera_demo.rs @@ -0,0 +1,187 @@ +use std::f64::consts::PI; +use std::time::Duration; + +use anyhow::Result; +use nalgebra::{Isometry3, Matrix4, Quaternion, Rotation3, Translation3, UnitQuaternion, Vector3}; +use rubullet::*; + +fn main() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + physics_client.configure_debug_visualizer(DebugVisualizerFlag::CovEnableYAxisUp, true); + physics_client.set_additional_search_path( + "../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data", + )?; + physics_client.set_time_step(Duration::from_secs_f64(1. / 60.)); + physics_client.set_gravity(Vector3::new(0.0, -9.8, 0.))?; + + let time_step = Duration::from_secs_f64(1. / 60.); + let mut panda = PandaSim::new(&mut physics_client, Vector3::zeros())?; + loop { + let _images = physics_client.get_camera_image( + 128, + 128, + panda.view_matrix, + panda.projection_matrix, + )?; + _images.rgba.save("/tmp/test.png")?; + panda.step(&mut physics_client); + physics_client.step_simulation()?; + + std::thread::sleep(time_step); + } +} + +pub struct PandaSim { + pub offset: Vector3, + pub id: BodyId, + pub t: Duration, + pub view_matrix: Matrix4, + pub projection_matrix: Matrix4, +} + +impl PandaSim { + const INITIAL_JOINT_POSITIONS: [f64; 9] = + [0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02]; + const PANDA_NUM_DOFS: usize = 7; + const PANDA_END_EFFECTOR_INDEX: usize = 11; + const LL: [f64; 9] = [-7.; 9]; + const UL: [f64; 9] = [7.; 9]; + const JR: [f64; 9] = [7.; 9]; + const NULL_SPACE_PARAMETERS: InverseKinematicsNullSpaceParameters<'static> = + InverseKinematicsNullSpaceParameters { + lower_limits: &PandaSim::LL, + upper_limits: &PandaSim::UL, + joint_ranges: &PandaSim::JR, + rest_poses: &PandaSim::INITIAL_JOINT_POSITIONS, + }; + + pub fn new(client: &mut PhysicsClient, offset: Vector3) -> Result { + let transform = Isometry3::new( + Vector3::new(0., 0., -0.6) + offset.clone(), + Rotation3::from(UnitQuaternion::from_quaternion(Quaternion::new( + 0.5, -0.5, -0.5, -0.5, + ))) + .scaled_axis(), + ); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("tray/traybox.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x + 0.1, offset.y + 0.3, offset.z - 0.5); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("lego/lego.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x - 0.1, offset.y + 0.3, offset.z - 0.5); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("lego/lego.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x + 0.1, offset.y + 0.3, offset.z - 0.7); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("lego/lego.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.6); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("sphere_small.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.5); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("sphere_small.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.7); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("sphere_small.urdf", urdf_options)?; + let cube_start_position = Isometry3::new( + offset.clone(), + UnitQuaternion::from_euler_angles(-PI / 2., 0., 0.).scaled_axis(), + ); + let urdf_options = UrdfOptions { + use_fixed_base: true, + base_transform: cube_start_position.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + let panda_id = client.load_urdf("franka_panda/panda.urdf", urdf_options)?; + client.change_dynamics_linear_damping(panda_id, 0.); + client.change_dynamics_angular_damping(panda_id, 0.); + let mut index = 0; + for i in 0..client.get_num_joints(panda_id) { + let info = client.get_joint_info(panda_id, i); + if info.joint_type == JointType::Revolute || info.joint_type == JointType::Prismatic { + client.reset_joint_state( + panda_id, + i, + PandaSim::INITIAL_JOINT_POSITIONS[index], + None, + )?; + index += 1; + } + } + let t = Duration::new(0, 0); + let view_matrix = + PhysicsClient::compute_view_matrix([0.5, 0.5, 0.5], [0., 0.5, 0.], [0., 1., 0.]); + let projection_matrix = PhysicsClient::compute_projection_matrix_fov(60., 1., 0.02, 1.); + + Ok(PandaSim { + offset, + id: panda_id, + t, + view_matrix, + projection_matrix, + }) + } + pub fn step(&mut self, client: &mut PhysicsClient) { + let t = self.t.as_secs_f64(); + self.t += Duration::from_secs_f64(1. / 60.); + let pose = Isometry3::from_parts( + Translation3::new( + 0.2 * f64::sin(1.5 * t), + 0.044, + -0.6 + 0.1 * f64::cos(1.5 * t), + ), + UnitQuaternion::::from_euler_angles(PI / 2., 0., 0.), + ); + let inverse_kinematics_parameters = + InverseKinematicsParametersBuilder::new(PandaSim::PANDA_END_EFFECTOR_INDEX, &pose) + .set_max_num_iterations(5) + .use_null_space(PandaSim::NULL_SPACE_PARAMETERS) + .build(); + let joint_poses = client + .calculate_inverse_kinematics(self.id, inverse_kinematics_parameters) + .unwrap(); + for i in 0..PandaSim::PANDA_NUM_DOFS { + client.set_joint_motor_control( + self.id, + i, + ControlMode::Position(joint_poses[i]), + Some(240. * 5.), + ); + } + } +} diff --git a/rubullet/examples/panda_demo.rs b/rubullet/examples/panda_demo.rs new file mode 100644 index 0000000..d6d5df0 --- /dev/null +++ b/rubullet/examples/panda_demo.rs @@ -0,0 +1,172 @@ +use std::f64::consts::PI; +use std::time::Duration; + +use anyhow::Result; +use nalgebra::{Isometry3, Quaternion, Rotation3, Translation3, UnitQuaternion, Vector3}; + +use rubullet::*; + +fn main() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + physics_client.set_additional_search_path( + "../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data", + )?; + physics_client.configure_debug_visualizer(DebugVisualizerFlag::CovEnableYAxisUp, true); + physics_client.set_time_step(Duration::from_secs_f64(1. / 60.)); + physics_client.set_gravity(Vector3::new(0.0, -9.8, 0.))?; + + let time_step = Duration::from_secs_f64(1. / 60.); + let mut panda = PandaSim::new(&mut physics_client, Vector3::zeros())?; + loop { + panda.step(&mut physics_client); + physics_client.step_simulation()?; + std::thread::sleep(time_step); + } +} + +pub struct PandaSim { + pub offset: Vector3, + pub id: BodyId, + pub t: Duration, +} + +impl PandaSim { + const INITIAL_JOINT_POSITIONS: [f64; 9] = + [0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02]; + const PANDA_NUM_DOFS: usize = 7; + const PANDA_END_EFFECTOR_INDEX: usize = 11; + const LL: [f64; 9] = [-7.; 9]; + const UL: [f64; 9] = [7.; 9]; + const JR: [f64; 9] = [7.; 9]; + const NULL_SPACE_PARAMETERS: InverseKinematicsNullSpaceParameters<'static> = + InverseKinematicsNullSpaceParameters { + lower_limits: &PandaSim::LL, + upper_limits: &PandaSim::UL, + joint_ranges: &PandaSim::JR, + rest_poses: &PandaSim::INITIAL_JOINT_POSITIONS, + }; + pub fn new(client: &mut PhysicsClient, offset: Vector3) -> Result { + let transform = Isometry3::new( + Vector3::new(0., 0., -0.6) + offset.clone(), + Rotation3::from(UnitQuaternion::from_quaternion(Quaternion::new( + 0.5, -0.5, -0.5, -0.5, + ))) + .scaled_axis(), + ); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("tray/traybox.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x + 0.1, offset.y + 0.3, offset.z - 0.5); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("lego/lego.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x - 0.1, offset.y + 0.3, offset.z - 0.5); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("lego/lego.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x + 0.1, offset.y + 0.3, offset.z - 0.7); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("lego/lego.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.6); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("sphere_small.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.5); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("sphere_small.urdf", urdf_options)?; + + let transform = Isometry3::translation(offset.x, offset.y + 0.3, offset.z - 0.7); + let urdf_options = UrdfOptions { + base_transform: transform.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + client.load_urdf("sphere_small.urdf", urdf_options)?; + let cube_start_position = Isometry3::new( + offset.clone(), + UnitQuaternion::from_euler_angles(-PI / 2., 0., 0.).scaled_axis(), + ); + let urdf_options = UrdfOptions { + use_fixed_base: true, + base_transform: cube_start_position.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + let panda_id = client.load_urdf("franka_panda/panda.urdf", urdf_options)?; + client.change_dynamics_linear_damping(panda_id, 0.); + client.change_dynamics_angular_damping(panda_id, 0.); + let mut index = 0; + for i in 0..client.get_num_joints(panda_id) { + let info = client.get_joint_info(panda_id, i); + if info.joint_type == JointType::Revolute || info.joint_type == JointType::Prismatic { + client.reset_joint_state( + panda_id, + i, + PandaSim::INITIAL_JOINT_POSITIONS[index], + None, + )?; + index += 1; + } + } + let t = Duration::new(0, 0); + Ok(PandaSim { + offset, + id: panda_id, + t, + }) + } + pub fn step(&mut self, client: &mut PhysicsClient) { + let t = self.t.as_secs_f64(); + self.t += Duration::from_secs_f64(1. / 60.); + + let pose = Isometry3::from_parts( + Translation3::new( + 0.2 * f64::sin(1.5 * t), + 0.044, + -0.6 + 0.1 * f64::cos(1.5 * t), + ), + UnitQuaternion::::from_euler_angles(PI / 2., 0., 0.), + ); + let inverse_kinematics_parameters = + InverseKinematicsParametersBuilder::new(PandaSim::PANDA_END_EFFECTOR_INDEX, &pose) + .set_max_num_iterations(5) + .use_null_space(PandaSim::NULL_SPACE_PARAMETERS) + .build(); + let joint_poses = client + .calculate_inverse_kinematics(self.id, inverse_kinematics_parameters) + .unwrap(); + for i in 0..PandaSim::PANDA_NUM_DOFS { + client.set_joint_motor_control( + self.id, + i, + ControlMode::Position(joint_poses[i]), + Some(240. * 5.), + ); + } + } +} diff --git a/rubullet/src/client.rs b/rubullet/src/client.rs new file mode 100644 index 0000000..df65928 --- /dev/null +++ b/rubullet/src/client.rs @@ -0,0 +1,3227 @@ +//! The main physics client. +//! +//! This is largely modeled after the PyBullet API but utilizes Rust's more expressive type system +//! where available. +use std::convert::TryFrom; +use std::os::unix::ffi::OsStrExt; +use std::{ffi::CString, os::raw::c_int, path::Path, ptr}; + +use nalgebra::{ + DMatrix, Isometry3, Matrix4, Matrix6xX, Point3, Quaternion, Translation3, UnitQuaternion, + Vector3, +}; + +use self::gui_marker::GuiMarker; +use crate::types::{ + AddDebugLineOptions, AddDebugTextOptions, BodyId, ChangeVisualShapeOptions, CollisionId, + ControlModeArray, ExternalForceFrame, GeometricCollisionShape, GeometricVisualShape, Images, + InverseKinematicsParameters, ItemId, Jacobian, JointInfo, JointState, JointType, KeyboardEvent, + LinkState, LoadModelFlags, MouseButtonState, MouseEvent, MultiBodyOptions, SdfOptions, + TextureId, Velocity, VisualId, VisualShapeOptions, +}; +use crate::{ + BodyInfo, ControlMode, DebugVisualizerFlag, Error, Mode, UrdfOptions, VisualShapeData, +}; +use image::{ImageBuffer, Luma, RgbaImage}; +use rubullet_sys as ffi; +use rubullet_sys::EnumSharedMemoryServerStatus::{ + CMD_ACTUAL_STATE_UPDATE_COMPLETED, CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, + CMD_CALCULATED_JACOBIAN_COMPLETED, CMD_CALCULATED_MASS_MATRIX_COMPLETED, + CMD_CAMERA_IMAGE_COMPLETED, CMD_CLIENT_COMMAND_COMPLETED, CMD_CREATE_COLLISION_SHAPE_COMPLETED, + CMD_CREATE_MULTI_BODY_COMPLETED, CMD_CREATE_VISUAL_SHAPE_COMPLETED, CMD_LOAD_TEXTURE_COMPLETED, + CMD_USER_DEBUG_DRAW_COMPLETED, CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, + CMD_VISUAL_SHAPE_INFO_COMPLETED, CMD_VISUAL_SHAPE_UPDATE_COMPLETED, +}; +use rubullet_sys::{ + b3CameraImageData, b3JointInfo, b3JointSensorState, b3KeyboardEventsData, b3LinkState, + b3MouseEventsData, b3SubmitClientCommandAndWaitStatus, B3_MAX_NUM_INDICES, B3_MAX_NUM_VERTICES, + MAX_SDF_BODIES, +}; +use std::time::Duration; + +/// The "handle" to the physics client. +/// +/// For whatever reason, the Bullet C API obfuscates the handle type by defining the handle type as +/// a pointer to an (essentially) anonymous struct. That's ugly to use here, and we know it isn't +/// going to be null, so we'll just do this alias. +type Handle = ffi::b3PhysicsClientHandle; + +/// Connection to a physics server. +/// +/// This serves as an abstraction over the possible physics servers, providing a unified interface. +pub struct PhysicsClient { + /// The underlying `b3PhysicsClientHandle` that is guaranteed to not be null. + handle: Handle, + + /// A marker indicating whether or not a GUI is in use by this client. + _gui_marker: Option, +} + +impl PhysicsClient { + /// Creates a PhysicsClient by connecting to a physics simulation. + /// + /// There are different Modes for creating a PhysicsClient: + /// + /// * [`Gui mode`](`crate::mode::Mode::Gui`) creates an OpenGL window where + /// the simulation will be visualized. There can only be one Gui instance at a time. + /// * [`Direct mode`](`crate::mode::Mode::Direct`) will run without any visualization. + /// In this mode you cannot access OpenGL features like debugging lines, text and Parameters. + /// You also cannot get mouse or keyboard events. + /// The can be many instances of PhysicsClients running in Direct mode + /// + /// Other modes are currently not implemented. + pub fn connect(mode: Mode) -> Result { + let (raw_handle, _gui_marker) = match mode { + Mode::Direct => unsafe { (ffi::b3ConnectPhysicsDirect(), None) }, + Mode::Gui => { + // Only one GUI is allowed per process. Try to get the marker and fail if there is + // another. + let gui_marker = GuiMarker::acquire()?; + + // Looking at the source code for both of these functions, they do not assume + // anything about the size of `argv` beyond what is supplied by `argc`. So, and + // `argc` of zero should keep this safe. + let raw_handle = if cfg!(target_os = "macos") { + unsafe { + ffi::b3CreateInProcessPhysicsServerAndConnectMainThread(0, ptr::null_mut()) + } + } else { + unsafe { ffi::b3CreateInProcessPhysicsServerAndConnect(0, ptr::null_mut()) } + }; + + (raw_handle, Some(gui_marker)) + } + }; + let handle = raw_handle.expect("Bullet returned a null pointer"); + + // At this point, we need to disconnect the physics client at any error. So we create the + // Rust struct and allow the `Drop` implementation to take care of that. + let mut client = PhysicsClient { + handle, + _gui_marker, + }; + + // Make sure it is up and running. + if !client.can_submit_command() { + return Err(Error::new("Physics server is not running")); + } + + // Now perform a series of commands to finish starting up the server. I don't know what they + // do but it's what PyBullet does. Note that PyBullet does not check these for `null` so I + // am assuming that they either can't be null or the consumer does the check. + unsafe { + let command = ffi::b3InitSyncBodyInfoCommand(client.handle); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(client.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + + if status_type != ffi::EnumSharedMemoryServerStatus::CMD_SYNC_BODY_INFO_COMPLETED as _ { + return Err(Error::new("Connection terminated, couldn't get body info")); + } + + let command = ffi::b3InitSyncUserDataCommand(client.handle); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(client.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + + if status_type != ffi::EnumSharedMemoryServerStatus::CMD_SYNC_USER_DATA_COMPLETED as _ { + return Err(Error::new("Connection terminated, couldn't get user data")); + } + } + + // The client is up and running + Ok(client) + } + /// reset_simulation will remove all objects from the world and reset the world to initial conditions. + pub fn reset_simulation(&mut self) { + unsafe { + let command_handle = ffi::b3InitResetSimulationCommand(self.handle); + ffi::b3InitResetSimulationSetFlags(command_handle, 0); + let _status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + } + /// Warning: in many cases it is best to leave the timeStep to default, which is 240Hz. + /// Several parameters are tuned with this value in mind. For example the number of solver + /// iterations and the error reduction parameters (erp) for contact, friction and non-contact + /// joints are related to the time step. If you change the time step, you may need to re-tune + /// those values accordingly, especially the erp values. + /// + /// You can set the physics engine timestep that is used when calling + /// [`step_simulation`](`Self::step_simulation()`). + /// It is best to only call this method at the start of a simulation. + /// Don't change this time step regularly. + pub fn set_time_step(&mut self, time_step: Duration) { + unsafe { + let command = ffi::b3InitPhysicsParamCommand(self.handle); + let _ret = ffi::b3PhysicsParamSetTimeStep(command, time_step.as_secs_f64()); + let _status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + } + /// By default, the physics server will not step the simulation, unless you explicitly send a + /// [`step_simulation`](`Self::step_simulation()`) command. + /// This way you can maintain control determinism of the simulation + /// It is possible to run the simulation in real-time by letting the physics server + /// automatically step the simulation according to its real-time-clock (RTC) using the + /// set_real_time_simulation command. If you enable the real-time simulation, + /// you don't need to call [`step_simulation`](`Self::step_simulation()`). + /// + /// Note that set_real_time_simulation has no effect in + /// [`Direct mode`](`crate::mode::Mode::Direct`) : + /// in [`Direct mode`](`crate::mode::Mode::Direct`) mode the physics + /// server and client happen in the same thread and you trigger every command. + /// In [`Gui mode`](`crate::mode::Mode::Gui`) and in Virtual Reality mode, and TCP/UDP mode, + /// the physics server runs in a separate thread from the client (RuBullet), + /// and set_real_time_simulation allows the physics server thread + /// to add additional calls to [`step_simulation`](`Self::step_simulation()`). + /// + /// # Arguments + /// * `enable_real_time_simulation` - activates or deactivates real-time simulation + pub fn set_real_time_simulation(&mut self, enable_real_time_simulation: bool) { + unsafe { + let command = ffi::b3InitPhysicsParamCommand(self.handle); + let _ret = ffi::b3PhysicsParamSetRealTimeSimulation( + command, + enable_real_time_simulation as i32, + ); + let _status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + } + /// Sets an additional search path for loading assets. + pub fn set_additional_search_path>(&mut self, path: P) -> Result<(), Error> { + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + let path = CString::new(path.as_ref().as_os_str().as_bytes()) + .map_err(|_| Error::new("Invalid path"))?; + + unsafe { + // Based on PyBullet, it appears that this path is copied and it does not need to live + // after calling the function. + let command_handle = ffi::b3SetAdditionalSearchPath(self.handle, path.as_ptr()); + let _status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + + Ok(()) + } + + /// Sets the default gravity force for all objects. + /// + /// By default, there is no gravitational force enabled. + /// + /// # Arguments + /// * `gravity` - a gravity vector. Can be a Vector3, a \[f64;3\]-array or anything else that can be + /// converted into a Vector3. + pub fn set_gravity>>( + &mut self, + gravity: GravityVector, + ) -> Result<(), Error> { + let gravity = gravity.into(); + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + unsafe { + // PyBullet error checks none of these. Looking through the code, it looks like there is + // no possible way to return an error on them. + let command = ffi::b3InitPhysicsParamCommand(self.handle); + let _ret = ffi::b3PhysicsParamSetGravity(command, gravity.x, gravity.y, gravity.z); + let _status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + + Ok(()) + } + + /// Sends a command to the physics server to load a physics model from a Unified Robot + /// Description Format (URDF) model. + /// + /// # Arguments + /// * `file` - a relative or absolute path to the URDF file on the file system of the physics server + /// * `options` - use additional options like `global_scaling` and `use_maximal_coordinates` for + /// loading the URDF-file. See [`UrdfOptions`](`crate::types::UrdfOptions`). + /// # Example + /// ```rust + /// use anyhow::Result; + /// use rubullet::*; + /// fn main() -> Result<()> { + /// let mut physics_client = PhysicsClient::connect(Mode::Direct)?; + /// physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + /// let cube = physics_client.load_urdf("cube.urdf", None)?; + /// assert_eq!("baseLink", physics_client.get_body_info(cube)?.base_name); + /// for i in 0..10 { + /// let _cube = physics_client.load_urdf( + /// "cube.urdf", + /// UrdfOptions { + /// flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + /// ..Default::default() + /// }, + /// )?; + /// } + /// assert_eq!(11, physics_client.get_num_bodies()); + /// Ok(()) + /// } + /// ``` + pub fn load_urdf, Options: Into>>( + &mut self, + file: P, + options: Options, + ) -> Result { + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + let file = CString::new(file.as_ref().as_os_str().as_bytes()) + .map_err(|_| Error::new("Invalid path"))?; + + let options = options.into().unwrap_or_default(); + unsafe { + // As always, PyBullet does not document and does not check return codes. + let command = ffi::b3LoadUrdfCommandInit(self.handle, file.as_ptr()); + let _ret = ffi::b3LoadUrdfCommandSetFlags(command, options.flags.bits()); + let _ret = ffi::b3LoadUrdfCommandSetStartPosition( + command, + options.base_transform.translation.x, + options.base_transform.translation.y, + options.base_transform.translation.z, + ); + let _ret = ffi::b3LoadUrdfCommandSetStartOrientation( + command, + options.base_transform.rotation.coords.x, + options.base_transform.rotation.coords.y, + options.base_transform.rotation.coords.z, + options.base_transform.rotation.coords.w, + ); + if let Some(use_maximal_coordinates) = options.use_maximal_coordinates { + if use_maximal_coordinates { + ffi::b3LoadUrdfCommandSetUseMultiBody(command, 0); + } else { + ffi::b3LoadUrdfCommandSetUseMultiBody(command, 1); + } + } + if options.use_fixed_base { + let _ret = ffi::b3LoadUrdfCommandSetUseFixedBase(command, 1); + } + + if options.global_scaling > 0.0 { + let _ret = + ffi::b3LoadUrdfCommandSetGlobalScaling(command, options.global_scaling as f64); + } + + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != ffi::EnumSharedMemoryServerStatus::CMD_URDF_LOADING_COMPLETED as c_int + { + return Err(Error::new("Cannot load URDF file")); + } + + Ok(BodyId(ffi::b3GetStatusBodyIndex(status_handle))) + } + } + + /// Sends a command to the physics server to load a physics model from + /// a Simulation Description Format (SDF) model. + /// # Arguments + /// * `file` - a relative or absolute path to the SDF file on the file system of the physics server. + /// * `options` - use additional options like `global_scaling` and `use_maximal_coordinates` for + /// loading the SDF-file. See [`SdfOptions`](`crate::types::SdfOptions`). + /// + /// # Return + /// Returns a list of unique body id's + /// + /// # Example + /// ```rust + /// use anyhow::Result; + /// use rubullet::*; + /// fn main() -> Result<()> { + /// let mut physics_client = PhysicsClient::connect(Mode::Direct)?; + /// physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + /// let cubes = physics_client.load_sdf("two_cubes.sdf", None)?; + /// assert_eq!(3, cubes.len()); // 2 cubes + 1 plane + /// Ok(()) + /// } + /// ``` + pub fn load_sdf, Options: Into>>( + &mut self, + file: P, + options: Options, + ) -> Result, Error> { + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + let file = CString::new(file.as_ref().as_os_str().as_bytes()) + .map_err(|_| Error::new("Invalid path"))?; + + unsafe { + let command = ffi::b3LoadSdfCommandInit(self.handle, file.as_ptr()); + if let Some(options) = options.into() { + if options.use_maximal_coordinates { + ffi::b3LoadSdfCommandSetUseMultiBody(command, 0); + } + if options.global_scaling > 0. { + ffi::b3LoadSdfCommandSetUseGlobalScaling(command, options.global_scaling); + } + } + + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != ffi::EnumSharedMemoryServerStatus::CMD_SDF_LOADING_COMPLETED as c_int + { + return Err(Error::new("Cannot load SDF file")); + } + let mut body_indices_out = [0; MAX_SDF_BODIES as usize]; + let num_bodies = ffi::b3GetStatusBodyIndices( + status_handle, + body_indices_out.as_mut_ptr(), + MAX_SDF_BODIES as i32, + ); + if num_bodies as u32 > MAX_SDF_BODIES { + return Err(Error::with(format!( + "SDF exceeds body capacity of {}", + MAX_SDF_BODIES + ))); + } + let mut bodies = Vec::::with_capacity(num_bodies as usize); + if num_bodies > 0 && num_bodies <= MAX_SDF_BODIES as i32 { + for i in 0..num_bodies { + bodies.push(BodyId(body_indices_out[i as usize])); + } + } + Ok(bodies) + } + } + /// Sends a command to the physics server to load a physics model from + /// a MuJoCo model. + /// # Arguments + /// * `file` - a relative or absolute path to the MuJoCo file on the file system of the physics server. + /// * `flags` - Flags for loading the model. Set to None if you do not whish to provide any. + /// + /// # Return + /// Returns a list of unique body id's + /// + /// # Example + /// ```rust + /// use anyhow::Result; + /// use rubullet::*; + /// fn main() -> Result<()> { + /// let mut physics_client = PhysicsClient::connect(Mode::Direct)?; + /// physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + /// let stadium = physics_client.load_mjcf("mjcf/hello_mjcf.xml", None)?; + /// assert_eq!(2, stadium.len()); // 1 cube + 1 plane + /// + /// let plane = physics_client.load_mjcf("mjcf/ground_plane.xml", LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES)?; + /// assert_eq!(1, plane.len()); + /// Ok(()) + /// } + /// ``` + pub fn load_mjcf, Flags: Into>>( + &mut self, + file: P, + flags: Flags, + ) -> Result, Error> { + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + let file = CString::new(file.as_ref().as_os_str().as_bytes()) + .map_err(|_| Error::new("Invalid path"))?; + + unsafe { + let command = ffi::b3LoadMJCFCommandInit(self.handle, file.as_ptr()); + if let Some(flags) = flags.into() { + ffi::b3LoadMJCFCommandSetFlags(command, flags.bits()); + } + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != ffi::EnumSharedMemoryServerStatus::CMD_MJCF_LOADING_COMPLETED as c_int + { + return Err(Error::new("Cannot load .mjcf file")); + } + let mut body_indices_out = [0; MAX_SDF_BODIES as usize]; + let num_bodies = ffi::b3GetStatusBodyIndices( + status_handle, + body_indices_out.as_mut_ptr(), + MAX_SDF_BODIES as i32, + ); + if num_bodies as u32 > MAX_SDF_BODIES { + return Err(Error::with(format!( + "MuJoCo exceeds body capacity of {}", + MAX_SDF_BODIES + ))); + } + let mut bodies = Vec::::with_capacity(num_bodies as usize); + if num_bodies > 0 && num_bodies <= MAX_SDF_BODIES as i32 { + for i in 0..num_bodies { + bodies.push(BodyId(body_indices_out[i as usize])); + } + } + Ok(bodies) + } + } + + /// Performs all the actions in a single forward dynamics simulation step such as collision + /// detection, constraint solving, and integration. + // TODO: Mention changing step size and automatic steps. + // TODO: Return analytics data? + pub fn step_simulation(&mut self) -> Result<(), Error> { + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + unsafe { + let command = ffi::b3InitStepSimulationCommand(self.handle); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type + != ffi::EnumSharedMemoryServerStatus::CMD_STEP_FORWARD_SIMULATION_COMPLETED as i32 + { + return Err(Error::new("Failed to perform forward step")); + } + } + + Ok(()) + } + + /// Reports the current transform of the base. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + pub fn get_base_transform(&mut self, body: BodyId) -> Result, Error> { + if !self.can_submit_command() { + return Err(Error::new("Not connected to physics server")); + } + + unsafe { + let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle, body.0); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, cmd_handle); + let status_type = ffi::b3GetStatusType(status_handle); + + if status_type + != ffi::EnumSharedMemoryServerStatus::CMD_ACTUAL_STATE_UPDATE_COMPLETED as c_int + { + return Err(Error::new("Failed to get base transform")); + } + + // To be totally honest, I'm not sure this part is correct. + let mut actual_state_q: *const f64 = ptr::null(); + ffi::b3GetStatusActualState( + status_handle, + ptr::null_mut(), + ptr::null_mut(), + ptr::null_mut(), + ptr::null_mut(), + &mut actual_state_q, + ptr::null_mut(), + ptr::null_mut(), + ); + + assert!(!actual_state_q.is_null()); + + let tx = *actual_state_q; + let ty = *(actual_state_q.offset(1)); + let tz = *(actual_state_q.offset(2)); + + let rx = *(actual_state_q.offset(3)); + let ry = *(actual_state_q.offset(4)); + let rz = *(actual_state_q.offset(5)); + let rw = *(actual_state_q.offset(6)); + + let tra = Translation3::new(tx, ty, tz); + let rot = Quaternion::new(rw, rx, ry, rz); + + Ok(Isometry3::from_parts( + tra, + UnitQuaternion::from_quaternion(rot), + )) + } + } + /// You can reset the position and orientation of the base (root) of each object. + /// It is best only to do this at the start, and not during a running simulation, + /// since the command will override the effect of all physics simulation. + /// The linear and angular velocity is set to zero. + /// You can use [reset_base_velocity](`Self::reset_base_velocity()`) + /// to reset to a non-zero linear and/or angular velocity. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `pose` - reset the base of the object at the specified position in world space coordinates + pub fn reset_base_transform(&mut self, body: BodyId, pose: &Isometry3) { + unsafe { + let command_handle = ffi::b3CreatePoseCommandInit(self.handle, body.0); + ffi::b3CreatePoseCommandSetBasePosition( + command_handle, + pose.translation.x, + pose.translation.y, + pose.translation.z, + ); + ffi::b3CreatePoseCommandSetBaseOrientation( + command_handle, + pose.rotation.i, + pose.rotation.j, + pose.rotation.k, + pose.rotation.w, + ); + let _status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + } + /// You get access to the linear and angular velocity of the base of a body. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// # Return + /// Returns a result which is either a Velocity (linear velocity, angular velocity) + /// or an Error + /// # See also + /// [reset_base_velocity](`Self::reset_base_velocity()`) to reset a base velocity and for examples + pub fn get_base_velocity(&mut self, body: BodyId) -> Result { + let mut base_velocity = [0.; 6]; + unsafe { + let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle, body.0); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, cmd_handle); + let status_type = ffi::b3GetStatusType(status_handle); + let mut actual_state_qdot: *const f64 = ptr::null(); + if status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED as i32 { + return Err(Error::new("get_base_velocity_failed.")); + } + ffi::b3GetStatusActualState( + status_handle, + ptr::null_mut(), + ptr::null_mut(), + ptr::null_mut(), + ptr::null_mut(), + ptr::null_mut(), + &mut actual_state_qdot, + ptr::null_mut(), + ); + let base_velocity_slice = std::slice::from_raw_parts(actual_state_qdot, 6); + base_velocity[..6].clone_from_slice(&base_velocity_slice[..6]); + } + Ok(base_velocity.into()) + } + /// Reset the linear and/or angular velocity of the base of a body + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `linear_velocity` - either a \[f64;3\] or a Vector3 which contains the desired linear velocity (x,y,z) + /// in Cartesian world coordinates or `None` to not change the linear velocity + /// * `angular_velocity` - either a \[f64;3\] or a Vector3 which contains the desired angular velocity + /// (wx,wy,wz) in Cartesian world coordinates or `None` to not change the angular velocity + /// + /// + /// # Example + /// ```rust + ///use anyhow::Result; + ///use nalgebra::{Isometry3, Vector3}; + ///use rubullet::*; + /// + ///fn main() -> Result<()> { + /// let mut physics_client = PhysicsClient::connect(Mode::Direct)?; + /// physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; + /// let _plane_id = physics_client.load_urdf("plane.urdf", None)?; + /// let cube_start_position = Isometry3::translation(0.0, 0.0, 1.0); + /// let box_id = physics_client.load_urdf( + /// "r2d2.urdf", + /// UrdfOptions { + /// base_transform: cube_start_position, + /// ..Default::default() + /// }, + /// )?; + /// + /// physics_client + /// .reset_base_velocity(box_id, [1., 2., 3.], [4., 5., 6.]); + /// let velocity = physics_client.get_base_velocity(box_id)?; + /// assert_eq!(velocity.to_vector().as_slice(), &[1., 2., 3., 4., 5., 6.]); + /// + /// physics_client + /// .reset_base_velocity(box_id, Vector3::zeros(), None); + /// let velocity = physics_client.get_base_velocity(box_id)?; + /// assert_eq!(velocity.to_vector().as_slice(), &[0., 0., 0., 4., 5., 6.]); + /// + /// physics_client + /// .reset_base_velocity(box_id, None, [0., 0., 0.]); + /// let velocity = physics_client.get_base_velocity(box_id)?; + /// assert_eq!(velocity.to_vector().as_slice(), & [0.; 6]); + /// + /// Ok(()) + ///} + /// ``` + pub fn reset_base_velocity< + IntoVector3: Into>, + Linear: Into>, + Angular: Into>, + >( + &mut self, + body: BodyId, + linear_velocity: Linear, + angular_velocity: Angular, + ) { + let maybe_lin = linear_velocity.into(); + let maybe_angular = angular_velocity.into(); + unsafe { + let command_handle = ffi::b3CreatePoseCommandInit(self.handle, body.0); + match (maybe_lin, maybe_angular) { + (None, None) => {} + (Some(linear), Some(angular)) => { + let linear: [f64; 3] = linear.into().into(); + let angular: [f64; 3] = angular.into().into(); + ffi::b3CreatePoseCommandSetBaseLinearVelocity(command_handle, linear.as_ptr()); + ffi::b3CreatePoseCommandSetBaseAngularVelocity( + command_handle, + angular.as_ptr(), + ); + } + (Some(linear), None) => { + let linear: [f64; 3] = linear.into().into(); + ffi::b3CreatePoseCommandSetBaseLinearVelocity(command_handle, linear.as_ptr()); + } + (None, Some(angular)) => { + let angular: [f64; 3] = angular.into().into(); + ffi::b3CreatePoseCommandSetBaseAngularVelocity( + command_handle, + angular.as_ptr(), + ); + } + } + let _status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + } + /// Queries the Cartesian world pose for the center of mass for a link. + /// It will also report the local inertial frame of the center of mass to the URDF link frame, + /// to make it easier to compute the graphics/visualization frame. + /// + /// # Warning + /// * the returned link velocity will only be available if `compute_link_velocity` is set to true. + /// Otherwise, it will be None. + /// + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_index` - link index + /// * `compute_link_velocity` - will compute the link velocity and put it into the [`LinkState`](`crate::types::LinkState`) if set to true. Otherwise the velocity within the LinkState will be invalid. + /// * `compute_forward_kinematics` - if true the Cartesian world pose will be recomputed using forward kinematics. + /// + /// # See also + /// * [`LinkState`](`crate::types::LinkState`) for more information about different types of link frames + /// * [`get_link_states()`](`Self::get_link_states()`) to get multiple link states + pub fn get_link_state( + &mut self, + body: BodyId, + link_index: usize, + compute_link_velocity: bool, + compute_forward_kinematics: bool, + ) -> Result { + unsafe { + assert!(body.0 >= 0, "get_link_state failed; invalid BodyId"); + + let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle, body.0); + if compute_link_velocity { + ffi::b3RequestActualStateCommandComputeLinkVelocity(cmd_handle, 1); + } + if compute_forward_kinematics { + ffi::b3RequestActualStateCommandComputeForwardKinematics(cmd_handle, 1); + } + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, cmd_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED as i32 { + return Err(Error::new("getLinkState failed.")); + } + let mut link_state = b3LinkState::default(); + if ffi::b3GetLinkState( + self.handle, + status_handle, + link_index as i32, + &mut link_state, + ) != 0 + { + return Ok((link_state, compute_link_velocity).into()); + } + } + Err(Error::new("getLinkState failed.")) + } + /// getLinkStates will return the information for multiple links. + /// Instead of link_index it will accept link_indices as an array of i32. + /// This can improve performance by reducing calling overhead of multiple calls to + /// [`get_link_state()`](`Self::get_link_state()`) . + /// + /// # Warning + /// * the returned link velocity will only be valid if `compute_link_velocity` is set to true + /// + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_indices` - link indices + /// * `compute_link_velocity` - will compute the link velocity and put it into the [`LinkState`](`crate::types::LinkState`) if set to true. Otherwise the velocity within the LinkState will be invalid. + /// * `compute_forward_kinematics` - if true the Cartesian world pose will be recomputed using forward kinematics. + /// + /// # See also + /// * [`LinkState`](`crate::types::LinkState`) for more information about different types of link frames + /// * [`get_link_state()`](`Self::get_link_states()`) to get only a single link state + pub fn get_link_states( + &mut self, + body: BodyId, + link_indices: &[usize], + compute_link_velocity: bool, + compute_forward_kinematics: bool, + ) -> Result, Error> { + unsafe { + assert!(body.0 >= 0, "get_link_states failed; invalid BodyId"); + + let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle, body.0); + if compute_link_velocity { + ffi::b3RequestActualStateCommandComputeLinkVelocity(cmd_handle, 1); + } + if compute_forward_kinematics { + ffi::b3RequestActualStateCommandComputeForwardKinematics(cmd_handle, 1); + } + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, cmd_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED as i32 { + return Err(Error::new("getLinkState failed.")); + } + let mut link_states = Vec::::with_capacity(link_indices.len()); + for &link_index in link_indices.iter() { + let mut link_state = b3LinkState::default(); + if ffi::b3GetLinkState( + self.handle, + status_handle, + link_index as i32, + &mut link_state, + ) != 0 + { + link_states.push((link_state, compute_link_velocity).into()); + } else { + return Err(Error::new("getLinkStates failed.")); + } + } + Ok(link_states) + } + } + + /// Returns whether or not this client can submit a command. + fn can_submit_command(&mut self) -> bool { + unsafe { ffi::b3CanSubmitCommand(self.handle) != 0 } + } + + pub fn change_dynamics_linear_damping(&mut self, body: BodyId, linear_damping: f64) { + unsafe { + let command = ffi::b3InitChangeDynamicsInfo(self.handle); + assert!(linear_damping >= 0.); + ffi::b3ChangeDynamicsInfoSetLinearDamping(command, body.0, linear_damping); + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + } + + pub fn change_dynamics_angular_damping(&mut self, body: BodyId, angular_damping: f64) { + unsafe { + let command = ffi::b3InitChangeDynamicsInfo(self.handle); + assert!(angular_damping >= 0.); + ffi::b3ChangeDynamicsInfoSetAngularDamping(command, body.0, angular_damping); + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + } + /// returns the number of joints of a body + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + pub fn get_num_joints(&mut self, body: BodyId) -> usize { + unsafe { ffi::b3GetNumJoints(self.handle, body.0) as usize } + } + /// Query info about a joint like its name and type + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `joint_index` - an index in the range \[0..[`get_num_joints(body)`](`Self::get_num_joints()`)\] + /// + /// See [JointInfo](`crate::types::JointInfo`) for an example use + pub fn get_joint_info(&mut self, body: BodyId, joint_index: usize) -> JointInfo { + self.get_joint_info_intern(body, joint_index).into() + } + + fn get_joint_info_intern(&mut self, body: BodyId, joint_index: usize) -> b3JointInfo { + unsafe { + let mut joint_info = b3JointInfo { + m_link_name: [2; 1024], + m_joint_name: [2; 1024], + m_joint_type: 0, + m_q_index: 0, + m_u_index: 0, + m_joint_index: 0, + m_flags: 0, + m_joint_damping: 0.0, + m_joint_friction: 0.0, + m_joint_lower_limit: 0.0, + m_joint_upper_limit: 0.0, + m_joint_max_force: 0.0, + m_joint_max_velocity: 0.0, + m_parent_frame: [0.; 7], + m_child_frame: [0.; 7], + m_joint_axis: [0.; 3], + m_parent_index: 0, + m_q_size: 0, + m_u_size: 0, + }; + ffi::b3GetJointInfo(self.handle, body.0, joint_index as i32, &mut joint_info); + joint_info + } + } + /// You can reset the state of the joint. It is best only to do this at the start, + /// while not running the simulation: resetJointState overrides all physics simulation. + /// Note that we only support 1-DOF motorized joints at the moment, + /// sliding joint or revolute joints. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `joint_index` - a joint index in the range \[0..[`get_num_joints(body)`](`Self::get_num_joints()`)\] + /// * `value` - the joint position (angle in radians or position) + /// * `velocity`- optional joint velocity (angular or linear velocity) + pub fn reset_joint_state>>( + &mut self, + body: BodyId, + joint_index: usize, + value: f64, + velocity: Velocity, + ) -> Result<(), Error> { + unsafe { + let joint_index = joint_index as i32; + let num_joints = ffi::b3GetNumJoints(self.handle, body.0); + assert!(joint_index < num_joints, "Joint index out-of-range."); + let command_handle = ffi::b3CreatePoseCommandInit(self.handle, body.0); + + ffi::b3CreatePoseCommandSetJointPosition( + self.handle, + command_handle, + joint_index, + value, + ); + ffi::b3CreatePoseCommandSetJointVelocity( + self.handle, + command_handle, + joint_index, + velocity.into().unwrap_or(0.), + ); + let _handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + Ok(()) + } + } + /// get information about the [joint state](`crate::types::JointState`) + /// such as the joint position, velocity, joint reaction forces and joint motor torque. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `joint_index` - a joint index in the range \[0..[`get_num_joints(body)`](`Self::get_num_joints()`)\] + pub fn get_joint_state( + &mut self, + body: BodyId, + joint_index: usize, + ) -> Result { + unsafe { + assert!(body.0 >= 0, "get_joint_state failed; invalid BodyId"); + let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle, body.0); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, cmd_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED as i32 { + return Err(Error::new("getJointState failed.")); + } + let mut sensor_state = b3JointSensorState::default(); + if 0 != ffi::b3GetJointState( + self.handle, + status_handle, + joint_index as i32, + &mut sensor_state, + ) { + return Ok(sensor_state.into()); + } + } + Err(Error::new("getJointState failed (2).")) + } + /// get_joint_states is the array version of [get_joint_state](`Self::get_joint_state()`). + /// Instead of passing in a single joint_index, you pass in a list of joint_indices. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `joint_indices` - a list of joint indices which each index in the range \[0..[`get_num_joints(body)`](`Self::get_num_joints()`)\] + pub fn get_joint_states( + &mut self, + body: BodyId, + joint_indices: &[usize], + ) -> Result, Error> { + unsafe { + assert!(body.0 >= 0, "get_joint_states failed; invalid BodyId"); + let num_joints = self.get_num_joints(body); + if joint_indices.is_empty() { + return Err(Error::new("expected a sequence of joint indices")); + } + let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle, body.0); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, cmd_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED as i32 { + return Err(Error::new("getJointState failed.")); + } + let mut result_list_joint_states = Vec::::with_capacity(num_joints); + for &joint_index in joint_indices.iter() { + assert!( + joint_index < num_joints, + "get_joint_states failed; invalid joint_index ({}). The robot only has {} joints", + joint_index, + num_joints, + ); + let mut sensor_state = b3JointSensorState::default(); + if 0 != ffi::b3GetJointState( + self.handle, + status_handle, + joint_index as i32, + &mut sensor_state, + ) { + result_list_joint_states.push(sensor_state.into()); + } else { + return Err(Error::new("getJointState failed (2).")); + } + } + Ok(result_list_joint_states) + } + } + /// calculate_mass_matrix will compute the system inertia for an articulated body given + /// its joint positions. + /// The composite rigid body algorithm (CBRA) is used to compute the mass matrix. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `object_positions` - joint positions for each link + /// # Return + /// The result is the square mass matrix with dimensions dofCount * dofCount, stored as a + /// list of dofCount rows, each row is a list of dofCount mass matrix elements. + /// Note that when multidof (spherical) joints are involved, calculate_mass_matrix will use a + /// different code path, that is a bit slower. + pub fn calculate_mass_matrix( + &mut self, + body: BodyId, + object_positions: &[f64], + ) -> Result, Error> { + if !object_positions.is_empty() { + let joint_positions = object_positions; + let flags = 0; // TODO add flags + unsafe { + let command_handle = ffi::b3CalculateMassMatrixCommandInit( + self.handle, + body.0, + joint_positions.as_ptr(), + joint_positions.len() as i32, + ); + ffi::b3CalculateMassMatrixSetFlags(command_handle, flags); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED as i32 { + let mut dof_count = 0; + ffi::b3GetStatusMassMatrix( + self.handle, + status_handle, + &mut dof_count, + std::ptr::null_mut(), + ); + if dof_count != 0 { + let mut mass_vec = vec![0.; (dof_count * dof_count) as usize]; + ffi::b3GetStatusMassMatrix( + self.handle, + status_handle, + &mut dof_count, + mass_vec.as_mut_slice().as_mut_ptr(), + ); + let mass_mat = + DMatrix::from_vec(dof_count as usize, dof_count as usize, mass_vec); + return Ok(mass_mat); + } + } else { + return Err(Error::new("Internal error in calculateMassMatrix")); + } + } + } + Err(Error::new("error in calculate_mass_matrix")) + } + /// You can compute the joint angles that makes the end-effector reach a given target position + /// in Cartesian world space. Internally, Bullet uses an improved version of + /// Samuel Buss Inverse Kinematics library. At the moment only the Damped Least Squares method + /// with or without Null Space control is exposed, with a single end-effector target. + /// Optionally you can also specify the target orientation of the end effector. + /// In addition, there is an option to use the null-space to specify joint limits and + /// rest poses. + /// + /// See [`InverseKinematicsParametersBuilder`](`crate::types::InverseKinematicsParametersBuilder`) and + /// [`InverseKinematicsParameters`](`crate::types::InverseKinematicsParameters`) for more details. + /// # Arguments + /// * `body` - The [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`crate::PhysicsClient::load_urdf()`) etc. + /// # Note + /// If you set the [`NullSpaceParameters`](`crate::types::InverseKinematicsNullSpaceParameters`) + /// wrong this function will return an error, while the PyBullet just uses the normal Ik instead. + pub fn calculate_inverse_kinematics( + &mut self, + body: BodyId, + params: InverseKinematicsParameters, + ) -> Result, Error> { + let solver = params.solver.into(); + let end_effector_link_index = params.end_effector_link_index; + + let pos = params.target_position; + let ori = params.target_orientation; + let mut sz_lower_limits = 0; + let mut sz_upper_limits = 0; + let mut sz_joint_ranges = 0; + let mut sz_rest_poses = 0; + + if let Some(limits) = ¶ms.limits { + sz_lower_limits = limits.lower_limits.len(); + sz_upper_limits = limits.upper_limits.len(); + sz_joint_ranges = limits.joint_ranges.len(); + sz_rest_poses = limits.rest_poses.len(); + } + + let dof_count = unsafe { ffi::b3ComputeDofCount(self.handle, body.0) } as usize; + + let mut has_null_space = false; + let mut has_joint_damping = false; + let mut has_current_positions = false; + + let current_positions = params.current_position; + let joint_damping = params.joint_damping; + + if dof_count != 0 + && (sz_lower_limits == dof_count) + && (sz_upper_limits == dof_count) + && (sz_joint_ranges == dof_count) + && (sz_rest_poses == dof_count) + { + has_null_space = true; + } else if params.limits.is_some() { + panic!( + "Null space parameter lengths do not match the number DoF! Robot has {} DoF", + dof_count + ); + } + if let Some(positions) = current_positions { + assert_ne!( + positions.len(), + dof_count, + "number of current_positions ({}) is not equal to the number of DoF's ({})", + positions.len(), + dof_count + ); + has_current_positions = true; + } + if let Some(damping) = joint_damping { + assert_eq!(damping.len(), + dof_count, + "calculateInverseKinematics: the size of input joint damping values ({}) should be equal to the number of degrees of freedom ({})", + damping.len(), + dof_count, + ); + + has_joint_damping = true; + } + let mut num_pos = 0; + unsafe { + let command = ffi::b3CalculateInverseKinematicsCommandInit(self.handle, body.0); + ffi::b3CalculateInverseKinematicsSelectSolver(command, solver); + if has_current_positions { + ffi::b3CalculateInverseKinematicsSetCurrentPositions( + command, + dof_count as i32, + current_positions.unwrap().as_ptr(), + ) + } + if let Some(max_num_iterations) = params.max_num_iterations { + ffi::b3CalculateInverseKinematicsSetMaxNumIterations( + command, + max_num_iterations as i32, + ); + } + if let Some(residual_threshold) = params.residual_threshold { + ffi::b3CalculateInverseKinematicsSetResidualThreshold(command, residual_threshold); + } + if has_null_space { + let limits = params.limits.unwrap(); + let lower_limits = limits.lower_limits; + let upper_limits = limits.upper_limits; + let joint_ranges = limits.joint_ranges; + let rest_poses = limits.rest_poses; + if let Some(orientation) = ori { + ffi::b3CalculateInverseKinematicsPosOrnWithNullSpaceVel( + command, + dof_count as i32, + end_effector_link_index as i32, + pos.coords.as_ptr(), + orientation.coords.as_ptr(), + lower_limits.as_ptr(), + upper_limits.as_ptr(), + joint_ranges.as_ptr(), + rest_poses.as_ptr(), + ); + } else { + ffi::b3CalculateInverseKinematicsPosWithNullSpaceVel( + command, + dof_count as i32, + end_effector_link_index as i32, + pos.coords.as_ptr(), + lower_limits.as_ptr(), + upper_limits.as_ptr(), + joint_ranges.as_ptr(), + rest_poses.as_ptr(), + ); + } + } else if let Some(orientation) = ori { + ffi::b3CalculateInverseKinematicsAddTargetPositionWithOrientation( + command, + end_effector_link_index as i32, + pos.coords.as_ptr(), + orientation.coords.as_ptr(), + ); + } else { + ffi::b3CalculateInverseKinematicsAddTargetPurePosition( + command, + end_effector_link_index as i32, + pos.coords.as_ptr(), + ); + } + + if has_joint_damping { + ffi::b3CalculateInverseKinematicsSetJointDamping( + command, + dof_count as i32, + joint_damping.unwrap().as_ptr(), + ) + } + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + let mut result_body_index: c_int = 0; + let result = ffi::b3GetStatusInverseKinematicsJointPositions( + status_handle, + &mut result_body_index, + &mut num_pos, + std::ptr::null_mut(), + ); + if result != 0 && num_pos != 0 { + let mut ik_output_joint_pos = vec![0.; num_pos as usize]; + ffi::b3GetStatusInverseKinematicsJointPositions( + status_handle, + &mut result_body_index, + &mut num_pos, + ik_output_joint_pos.as_mut_slice().as_mut_ptr(), + ); + return Ok(ik_output_joint_pos); + } + } + Err(Error::new("Error in calculateInverseKinematics")) + } + /// calculate_inverse_dynamics will compute the forces needed to reach the given + /// joint accelerations, starting from specified joint positions and velocities. + /// The inverse dynamics is computed using the recursive Newton Euler algorithm (RNEA). + /// + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `object_positions` - joint positions for each degree of freedom (DoF). + /// Note that fixed joints have 0 degrees of freedom. The base is skipped/ignored in all cases (floating base and fixed base). + /// * `object_velocities` - joint velocities for each degree of freedom (DoF) + /// * `object_acceleration` - desired joint accelerations for each degree of freedom (DoF) + /// + /// # Note + /// When multidof (spherical) joints are involved, the calculate_inverse_dynamics uses a + /// different code path and is a bit slower. Also note that calculate_inverse_dynamics ignores + /// the joint/link damping, while forward dynamics (in stepSimulation) includes those damping + /// terms. So if you want to compare the inverse dynamics and forward dynamics, + /// make sure to set those damping terms to zero using + /// [change_dynamics_linear_damping](`Self::change_dynamics_linear_damping`) and + /// [change_dynamics_angular_damping](`Self::change_dynamics_angular_damping`). + pub fn calculate_inverse_dynamics( + &mut self, + body: BodyId, + object_positions: &[f64], + object_velocities: &[f64], + object_accelerations: &[f64], + ) -> Result, Error> { + let flags = 0; // TODO find out what those flags are and let the user set them + assert_eq!(object_velocities.len(), + object_accelerations.len(), + "number of object velocities ({}) should be equal to the number of object accelerations ({})", + object_velocities.len(), + object_accelerations.len(), + ); + unsafe { + let command_handle = ffi::b3CalculateInverseDynamicsCommandInit2( + self.handle, + body.0, + object_positions.as_ptr(), + object_positions.len() as i32, + object_velocities.as_ptr(), + object_accelerations.as_ptr(), + object_velocities.len() as i32, + ); + ffi::b3CalculateInverseDynamicsSetFlags(command_handle, flags); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED as i32 { + let mut body_unique_id = 0; + let mut dof_count = 0; + ffi::b3GetStatusInverseDynamicsJointForces( + status_handle, + &mut body_unique_id, + &mut dof_count, + std::ptr::null_mut(), + ); + if dof_count != 0 { + let mut joint_forces_output = vec![0.; dof_count as usize]; + ffi::b3GetStatusInverseDynamicsJointForces( + status_handle, + &mut 0, + &mut 0, + joint_forces_output.as_mut_slice().as_mut_ptr(), + ); + return Ok(joint_forces_output); + } + } + } + Err(Error::new( + "Error in calculateInverseDynamics, please check arguments.", + )) + } + /// calculate_jacobian will compute the translational and rotational jacobians for a point on a + /// link, e.g. x_dot = J * q_dot. The returned jacobians are slightly different depending on + /// whether the root link is fixed or floating. If floating, the jacobians will include columns + /// corresponding to the root link degrees of freedom; if fixed, the jacobians will only have + /// columns associated with the joints. The function call takes the full description of the + /// kinematic state, this is because calculateInverseDynamics is actually called first and the + /// desired jacobians are extracted from this; therefore, it is reasonable to pass zero vectors + /// for joint velocities and accelerations if desired. + /// + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_index` - link index for the jacobian. + /// * `local_position` - the point on the specified link to compute the jacobian for, in link local coordinates around its center of mass. It needs to be something + /// that can be converted to a Translation3 (Vector3, Point3, \[f64;3\], ..) + /// * `object_positions` - joint positions (angles) + /// * `object_velocities` - joint velocities + /// * `object_accelerations` - desired joint accelerations + /// + /// See jacobian.rs for an example + pub fn calculate_jacobian>>( + &mut self, + body: BodyId, + link_index: usize, + local_position: LocalPosition, + object_positions: &[f64], + object_velocities: &[f64], + object_accelerations: &[f64], + ) -> Result { + assert_eq!( + object_velocities.len(), + object_positions.len(), + "object_velocities (size: {}) has not the same size as object_positions (size: {})", + object_velocities.len(), + object_positions.len(), + ); + assert_eq!( + object_accelerations.len(), + object_positions.len(), + "object_accelerations (size: {}) has not the same size as object_positions (size: {})", + object_accelerations.len(), + object_positions.len(), + ); + + let num_joints = self.get_num_joints(body); + let mut dof_count_org = 0; + for j in 0..num_joints { + let joint_type = + JointType::try_from(self.get_joint_info_intern(body, j).m_joint_type).unwrap(); + match joint_type { + JointType::Revolute | JointType::Prismatic => { + dof_count_org += 1; + } + JointType::Spherical => { + return Err(Error::new( + "Spherical joints are not supported in the rubullet binding", + )) + } + JointType::Planar => { + return Err(Error::new( + "Planar joints are not supported in the rubullet binding", + )) + } + _ => {} + } + } + if dof_count_org != 0 && dof_count_org == object_positions.len() { + let joint_positions = object_positions; + let joint_velocities = object_velocities; + let joint_accelerations = object_accelerations; + + unsafe { + let command_handle = ffi::b3CalculateJacobianCommandInit( + self.handle, + body.0, + link_index as i32, + local_position.into().vector.as_ptr(), + joint_positions.as_ptr(), + joint_velocities.as_ptr(), + joint_accelerations.as_ptr(), + ); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CALCULATED_JACOBIAN_COMPLETED as i32 { + let mut dof_count = 0; + ffi::b3GetStatusJacobian( + status_handle, + &mut dof_count, + std::ptr::null_mut(), + std::ptr::null_mut(), + ); + if dof_count != 0 { + let mut linear_jacobian = vec![0.; 3 * dof_count as usize]; + let mut angular_jacobian = vec![0.; 3 * dof_count as usize]; + ffi::b3GetStatusJacobian( + status_handle, + &mut dof_count, + linear_jacobian.as_mut_slice().as_mut_ptr(), + angular_jacobian.as_mut_slice().as_mut_ptr(), + ); + let mut jacobian = linear_jacobian; + jacobian.append(&mut angular_jacobian); + let jacobian_mat = Matrix6xX::from_row_slice(&jacobian); + return Ok(Jacobian { + jacobian: jacobian_mat, + }); + } + } + } + } + Err(Error::new("Error in calculateJacobian")) + } + + /// sets joint motor commands. This function is the rust version of `setJointMotorControl2` from PyBullet. + /// This function differs a bit from the corresponding PyBullet function. + /// Instead of providing optional arguments that depend on the Control Mode, the necessary parameters + /// are directly encoded in the ControlMode Enum. + /// + /// We can control a robot by setting a desired control mode for one or more joint motors. + /// During the stepSimulation the physics engine will simulate the motors to reach the given + /// target value that can be reached within the maximum motor forces and other constraints. + /// + /// # Important Note: + /// by default, each revolute joint and prismatic joint is motorized using a velocity motor. + /// You can disable those default motor by using a maximum force of 0. + /// This will let you perform torque control. + /// For Example: + /// ```rust + ///# use rubullet::{ControlMode, PhysicsClient, Mode}; + ///# use anyhow::Result; + ///# pub fn main() -> Result<()> { + ///# let mut client = PhysicsClient::connect(Mode::Direct)?; + ///# client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data")?; + ///# let panda_id = client.load_urdf("franka_panda/panda.urdf", None)?; + ///# let joint_index = 1; + /// client.set_joint_motor_control(panda_id, joint_index, ControlMode::Velocity(0.), Some(0.)); + ///# Ok(()) + ///# } + /// ``` + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `joint_index` - link index in range [0..get_num_joints(bodyUniqueId)] (note that link index == joint index) + /// * `control_mode` - Specifies how to control the robot (Position, Torque, etc.) + /// * `maximum_force` - this is the maximum motor force used to reach the target value. It has no effect in Torque mode. + /// # Example + /// ```rust + /// use rubullet::{ControlMode, PhysicsClient, Mode}; + /// use anyhow::Result; + /// pub fn main() -> Result<()> { + /// let mut client = PhysicsClient::connect(Mode::Direct)?; + /// client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data")?; + /// let panda_id = client.load_urdf("franka_panda/panda.urdf", None)?; + /// let joint_index = 1; + /// client.set_joint_motor_control(panda_id, joint_index, ControlMode::Torque(100.), None); + /// client.set_joint_motor_control(panda_id, joint_index, ControlMode::Position(0.4), Some(1000.)); + /// Ok(()) + /// } + /// ``` + #[doc(alias = "set_joint_motor_control_2")] + #[doc(alias = "setJointMotorControl2")] + pub fn set_joint_motor_control( + &mut self, + body: BodyId, + joint_index: usize, + control_mode: ControlMode, + maximum_force: Option, + ) { + let force = maximum_force.unwrap_or(100000.); + let kp = 0.1; + let kd = 1.0; + let target_velocity = 0.; + unsafe { + let command_handle = + ffi::b3JointControlCommandInit2(self.handle, body.0, control_mode.get_int()); + let info = self.get_joint_info_intern(body, joint_index); + + match control_mode { + ControlMode::Position(target_position) => { + ffi::b3JointControlSetDesiredPosition( + command_handle, + info.m_q_index, + target_position, + ); + + ffi::b3JointControlSetKp(command_handle, info.m_u_index, kp); + ffi::b3JointControlSetDesiredVelocity( + command_handle, + info.m_u_index, + target_velocity, + ); + + ffi::b3JointControlSetKd(command_handle, info.m_u_index, kd); + ffi::b3JointControlSetMaximumForce(command_handle, info.m_u_index, force); + } + ControlMode::Pd { + target_position: pos, + target_velocity: vel, + position_gain: kp, + velocity_gain: kd, + maximum_velocity: max_vel, + } + | ControlMode::PositionWithPd { + target_position: pos, + target_velocity: vel, + position_gain: kp, + velocity_gain: kd, + maximum_velocity: max_vel, + } => { + if let Some(max_vel) = max_vel { + ffi::b3JointControlSetMaximumVelocity( + command_handle, + info.m_u_index, + max_vel, + ); + } + ffi::b3JointControlSetDesiredPosition(command_handle, info.m_q_index, pos); + + ffi::b3JointControlSetKp(command_handle, info.m_u_index, kp); + ffi::b3JointControlSetDesiredVelocity(command_handle, info.m_u_index, vel); + + ffi::b3JointControlSetKd(command_handle, info.m_u_index, kd); + ffi::b3JointControlSetMaximumForce(command_handle, info.m_u_index, force); + } + ControlMode::Velocity(vel) => { + ffi::b3JointControlSetDesiredVelocity(command_handle, info.m_u_index, vel); + ffi::b3JointControlSetKd(command_handle, info.m_u_index, kd); + ffi::b3JointControlSetMaximumForce(command_handle, info.m_u_index, force); + } + ControlMode::Torque(f) => { + ffi::b3JointControlSetDesiredForceTorque(command_handle, info.m_u_index, f); + } + } + let _status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + } + /// The array version of [`set_joint_motor_control()`](`crate::client::PhysicsClient::set_joint_motor_control()`). + /// This reduces the calling overhead and should therefore be faster. See [`set_joint_motor_control()`](`crate::client::PhysicsClient::set_joint_motor_control()`) + /// for more details. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `joint_indices` - list of link indices in range [0..get_num_joints(bodyUniqueId)] (note that link index == joint index) + /// * `control_mode` - Specifies how to control the robot (Position, Torque, etc.) + /// * `maximum_force` - this is the maximum motor force used to reach the target value for each joint. It has no effect in Torque mode. + pub fn set_joint_motor_control_array( + &mut self, + body: BodyId, + joint_indices: &[usize], + control_mode: ControlModeArray, + maximum_force: Option<&[f64]>, + ) -> Result<(), Error> { + let alloc_vec; + let forces; + match maximum_force { + None => { + alloc_vec = vec![100000.; joint_indices.len()]; + forces = alloc_vec.as_slice(); + } + Some(max_forces) => { + forces = max_forces; + } + } + assert_eq!(forces.len(), + joint_indices.len(), + "number of maximum forces (size: {}) should match the number of joint indices (size: {})", + forces.len(), + joint_indices.len(), + ); + let kp = 0.1; + let kd = 1.0; + let num_joints = self.get_num_joints(body); + unsafe { + let command_handle = + ffi::b3JointControlCommandInit2(self.handle, body.0, control_mode.get_int()); + + for &joint_index in joint_indices.iter() { + assert!( + joint_index < num_joints, + "Joint index ({}) out-of-range. Robot has a total number of {} joints", + joint_index, + num_joints, + ); + } + match control_mode { + ControlModeArray::Positions(target_positions) => { + assert_eq!(target_positions.len(), + joint_indices.len(), + "number of target positions ({}) should match the number of joint indices ({})", + target_positions.len(), + joint_indices.len(), + ); + for i in 0..target_positions.len() { + let info = self.get_joint_info_intern(body, joint_indices[i]); + ffi::b3JointControlSetDesiredPosition( + command_handle, + info.m_q_index, + target_positions[i], + ); + ffi::b3JointControlSetKp(command_handle, info.m_u_index, kp); + ffi::b3JointControlSetDesiredVelocity(command_handle, info.m_u_index, 0.); + + ffi::b3JointControlSetKd(command_handle, info.m_u_index, kd); + ffi::b3JointControlSetMaximumForce( + command_handle, + info.m_u_index, + forces[i], + ); + } + } + ControlModeArray::Pd { + target_positions: pos, + target_velocities: vel, + position_gains: pg, + velocity_gains: vg, + } + | ControlModeArray::PositionsWithPd { + target_positions: pos, + target_velocities: vel, + position_gains: pg, + velocity_gains: vg, + } => { + assert_eq!(pos.len(), + joint_indices.len(), + "number of target positions ({}) should match the number of joint indices ({})", + pos.len(), + joint_indices.len(), + ); + assert_eq!(vel.len(), + joint_indices.len(), + "number of target velocities ({}) should match the number of joint indices ({})", + vel.len(), + joint_indices.len(), + ); + assert_eq!(pg.len(), + joint_indices.len(), + "number of position gains ({}) should match the number of joint indices ({})", + pg.len(), + joint_indices.len(), + ); + assert_eq!(vg.len(), + joint_indices.len(), + "number of velocity gains ({}) should match the number of joint indices ({})", + vg.len(), + joint_indices.len(), + ); + + for i in 0..pos.len() { + let info = self.get_joint_info_intern(body, joint_indices[i]); + ffi::b3JointControlSetDesiredPosition( + command_handle, + info.m_q_index, + pos[i], + ); + + ffi::b3JointControlSetKp(command_handle, info.m_u_index, pg[i]); + ffi::b3JointControlSetDesiredVelocity( + command_handle, + info.m_u_index, + vel[i], + ); + + ffi::b3JointControlSetKd(command_handle, info.m_u_index, vg[i]); + ffi::b3JointControlSetMaximumForce( + command_handle, + info.m_u_index, + forces[i], + ); + } + } + ControlModeArray::Velocities(vel) => { + assert_eq!(vel.len(), + joint_indices.len(), + "number of target velocities ({}) should match the number of joint indices ({})", + vel.len(), + joint_indices.len(), + ); + for i in 0..vel.len() { + let info = self.get_joint_info_intern(body, joint_indices[i]); + ffi::b3JointControlSetDesiredVelocity( + command_handle, + info.m_u_index, + vel[i], + ); + ffi::b3JointControlSetKd(command_handle, info.m_u_index, kd); + ffi::b3JointControlSetMaximumForce( + command_handle, + info.m_u_index, + forces[i], + ); + } + } + ControlModeArray::Torques(f) => { + assert_eq!(f.len(), + joint_indices.len(), + "number of target torques ({}) should match the number of joint indices ({})", + f.len(), + joint_indices.len(), + ); + for i in 0..f.len() { + let info = self.get_joint_info_intern(body, joint_indices[i]); + ffi::b3JointControlSetDesiredForceTorque( + command_handle, + info.m_u_index, + f[i], + ); + } + } + } + let _status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + Ok(()) + } + /// computes the view matrix which can be used together with the projection matrix to generate + /// camera images within the simulation + /// + /// # Arguments + /// * `camera_eye_position` - eye position in Cartesian world coordinates + /// * `camera_target_position` - position of the target (focus) point, in Cartesian world coordinates + /// * `camera_up_vector` - up vector of the camera, in Cartesian world coordinates + /// + /// # Example + /// ```rust + /// use rubullet::PhysicsClient; + /// use nalgebra::{Point3, Vector3}; + /// + /// // variant 1: using arrays + /// let eye_position = [1.; 3]; + /// let target_position = [1., 0., 0.]; + /// let up_vector = [0., 1., 0.]; + /// let view_matrix_from_arrays = PhysicsClient::compute_view_matrix(eye_position, target_position, up_vector); + /// + /// // variant 2: using vectors and points + /// let eye_position = Point3::new(1.,1.,1.); + /// let target_position = Point3::new(1., 0., 0.); + /// let up_vector = Vector3::new(0., 1., 0.); + /// let view_matrix_from_points = PhysicsClient::compute_view_matrix(eye_position, target_position, up_vector); + /// assert_eq!(view_matrix_from_arrays.as_slice(),view_matrix_from_points.as_slice()); + /// ``` + /// # See also + /// * [compute_view_matrix_from_yaw_pitch_roll](`Self::compute_view_matrix_from_yaw_pitch_roll`) + /// * [compute_projection_matrix](`Self::compute_projection_matrix`) + /// * [compute_projection_matrix_fov](`Self::compute_projection_matrix_fov`) + /// * [get_camera_image](`Self::get_camera_image`) + pub fn compute_view_matrix>, Vector: Into>>( + camera_eye_position: Point, + camera_target_position: Point, + camera_up_vector: Vector, + ) -> Matrix4 { + let mut view_matrix: Matrix4 = Matrix4::zeros(); + unsafe { + ffi::b3ComputeViewMatrixFromPositions( + camera_eye_position.into().coords.as_ptr(), + camera_target_position.into().coords.as_ptr(), + camera_up_vector.into().as_slice().as_ptr(), + view_matrix.as_mut_ptr(), + ); + view_matrix + } + } + /// computes the view matrix which can be used together with the projection matrix to generate + /// camera images within the simulation + /// + /// # Arguments + /// * `camera_target_position` - position of the target (focus) point, in Cartesian world coordinates + /// * `distance` - distance from eye to focus point + /// * `yaw` - yaw angle in degrees left/right around up-axis. + /// * `pitch` - pitch in degrees up/down. + /// * `roll` - roll in degrees around forward vector + /// * `z_axis_is_up` - if true the Z axis is the up axis of the camera. Otherwise the Y axis will be the up axis. + /// + /// # Example + /// ```rust + /// use rubullet::PhysicsClient; + /// use nalgebra::Point3; + /// // variant 1: using array + /// let target_position = [1., 0., 0.]; + /// let view_matrix_from_array = PhysicsClient::compute_view_matrix_from_yaw_pitch_roll( + /// target_position, + /// 0.6, + /// 0.2, + /// 0.3, + /// 0.5, + /// false, + /// ); + /// // variant 1: using Point3 + /// let target_position = Point3::new(1., 0., 0.); + /// let view_matrix_from_point = PhysicsClient::compute_view_matrix_from_yaw_pitch_roll( + /// target_position, + /// 0.6, + /// 0.2, + /// 0.3, + /// 0.5, + /// false, + /// ); + /// assert_eq!(view_matrix_from_array.as_slice(),view_matrix_from_point.as_slice()); + /// ``` + /// # See also + /// * [compute_view_matrix](`Self::compute_view_matrix`) + /// * [compute_projection_matrix](`Self::compute_projection_matrix`) + /// * [compute_projection_matrix_fov](`Self::compute_projection_matrix_fov`) + /// * [get_camera_image](`Self::get_camera_image`) + pub fn compute_view_matrix_from_yaw_pitch_roll>>( + camera_target_position: Point, + distance: f32, + yaw: f32, + pitch: f32, + roll: f32, + z_axis_is_up: bool, + ) -> Matrix4 { + let mut view_matrix: Matrix4 = Matrix4::zeros(); + let up_axis_index = match z_axis_is_up { + true => 2, + false => 1, + }; + unsafe { + ffi::b3ComputeViewMatrixFromYawPitchRoll( + camera_target_position.into().coords.as_ptr(), + distance, + yaw, + pitch, + roll, + up_axis_index, + view_matrix.as_mut_ptr(), + ); + view_matrix + } + } + /// computes the projection matrix which can be used together with the view matrix to generate + /// camera images within the simulation + /// + /// # Arguments + /// * `left` - left screen (canvas) coordinate + /// * `right` - right screen (canvas) coordinate + /// * `bottom` - bottom screen (canvas) coordinate + /// * `top` - top screen (canvas) coordinate + /// * `near` - near plane distance + /// * `far` - far plane distance + /// + /// # See also + /// * [compute_view_matrix](`Self::compute_view_matrix`) + /// * [compute_view_matrix_from_yaw_pitch_roll](`Self::compute_view_matrix_from_yaw_pitch_roll`) + /// * [compute_projection_matrix_fov](`Self::compute_projection_matrix_fov`) + /// * [get_camera_image](`Self::get_camera_image`) + pub fn compute_projection_matrix( + left: f32, + right: f32, + bottom: f32, + top: f32, + near_val: f32, + far_val: f32, + ) -> Matrix4 { + let mut projection_matrix = Matrix4::zeros(); + unsafe { + ffi::b3ComputeProjectionMatrix( + left, + right, + bottom, + top, + near_val, + far_val, + projection_matrix.as_mut_ptr(), + ); + projection_matrix + } + } + /// computes the projection matrix which can be used together with the view matrix to generate + /// camera images within the simulation + /// + /// # Arguments + /// * `fov` - left screen (canvas) coordinate + /// * `aspect` - right screen (canvas) coordinate + /// * `near_val` - near plane distance + /// * `far_val` - far plane distance + /// + /// # See also + /// * [compute_view_matrix](`Self::compute_view_matrix`) + /// * [compute_view_matrix_from_yaw_pitch_roll](`Self::compute_view_matrix_from_yaw_pitch_roll`) + /// * [compute_projection_matrix](`Self::compute_projection_matrix`) + /// * [get_camera_image](`Self::get_camera_image`) + pub fn compute_projection_matrix_fov( + fov: f32, + aspect: f32, + near_val: f32, + far_val: f32, + ) -> Matrix4 { + let mut projection_matrix = Matrix4::zeros(); + unsafe { + ffi::b3ComputeProjectionMatrixFOV( + fov, + aspect, + near_val, + far_val, + projection_matrix.as_mut_ptr(), + ); + projection_matrix + } + } + /// returns an RGBA, depth and segmentation images. + /// + /// # Note + /// Depth and segmentation images are currently not really images as the image crate does not + /// properly support these types yet. + /// + /// # Arguments + /// * `width` - eye position in Cartesian world coordinates + /// * `height` - position of the target (focus) point, in Cartesian world coordinates + /// * `view_matrix` - view matrix, see [compute_view_matrix](`Self::compute_view_matrix`) + /// * `projection_matrix` - projection matrix, see [compute_projection_matrix](`Self::compute_projection_matrix`) + /// # See also + /// * [compute_view_matrix](`Self::compute_view_matrix`) + /// * [compute_view_matrix_from_yaw_pitch_roll](`Self::compute_view_matrix_from_yaw_pitch_roll`) + /// * [compute_projection_matrix](`Self::compute_projection_matrix`) + /// * [compute_projection_matrix_fov](`Self::compute_projection_matrix_fov`) + /// * panda_camera_demo.rs for an example + pub fn get_camera_image( + &mut self, + width: i32, + height: i32, + view_matrix: Matrix4, + projection_matrix: Matrix4, + ) -> Result { + unsafe { + let command = ffi::b3InitRequestCameraImage(self.handle); + ffi::b3RequestCameraImageSetPixelResolution(command, width, height); + ffi::b3RequestCameraImageSetCameraMatrices( + command, + view_matrix.as_ptr(), + projection_matrix.as_ptr(), + ); + if self.can_submit_command() { + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CAMERA_IMAGE_COMPLETED as i32 { + let mut image_data = b3CameraImageData::default(); + ffi::b3GetCameraImageData(self.handle, &mut image_data); + let buffer = std::slice::from_raw_parts( + image_data.m_rgb_color_data, + (width * height * 4) as usize, + ); + let depth_buffer = std::slice::from_raw_parts( + image_data.m_depth_values, + (width * height) as usize, + ); + let segmentation_buffer = std::slice::from_raw_parts( + image_data.m_segmentation_mask_values, + (width * height) as usize, + ); + let rgba: RgbaImage = + ImageBuffer::from_vec(width as u32, height as u32, buffer.into()).unwrap(); + + let depth = ImageBuffer::, Vec>::from_vec( + width as u32, + height as u32, + depth_buffer.into(), + ) + .unwrap(); + let segmentation = ImageBuffer::, Vec>::from_vec( + width as u32, + height as u32, + segmentation_buffer.into(), + ) + .unwrap(); + return Ok(Images { + rgba, + depth, + segmentation, + }); + } + } + Err(Error::new("getCameraImage failed")) + } + } + /// This method can configure some settings of the built-in OpenGL visualizer, + /// such as enabling or disabling wireframe, shadows and GUI rendering. + /// This is useful since some laptops or Desktop GUIs have + /// performance issues with our OpenGL 3 visualizer. + /// # Arguments + /// * `flag` - Feature to enable or disable + /// * `enable` - enables or disables the feature + // TODO implement the other options + pub fn configure_debug_visualizer(&mut self, flag: DebugVisualizerFlag, enable: bool) { + unsafe { + let command_handle = ffi::b3InitConfigureOpenGLVisualizer(self.handle); + ffi::b3ConfigureOpenGLVisualizerSetVisualizationFlags( + command_handle, + flag as i32, + enable as i32, + ); + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + } + + /// You can add a 3d line specified by a 3d starting point (from) and end point (to), + /// a color \[red,green,blue\], a line width and a duration in seconds. + /// + /// # Arguments + /// * `line_from_xyz` - starting point of the line in Cartesian world coordinates. Can be + /// a Point3, a Vector3, an array or anything else than can be converted into a Point3. + /// * `line_to_xyz` - end point of the line in Cartesian world coordinates. Can be + /// a Point3, a Vector3, an array or anything else than can be converted into a Point3. + /// * `options` - advanced options for the line. Use None for default settings. + /// + /// # Return + /// A unique item id of the line. + /// You can use [`remove_user_debug_item()`](`Self::remove_user_debug_item()`) to delete it. + /// + /// # Example + /// ```no_run + ///# use anyhow::Result; + ///# use rubullet::Mode::Gui; + ///# use rubullet::AddDebugLineOptions; + ///# use rubullet::PhysicsClient; + ///# use std::time::Duration; + ///# + ///# pub fn main() -> Result<()> { + ///# use nalgebra::Point3; + /// let mut client = PhysicsClient::connect(Gui)?; + /// let red_line = client.add_user_debug_line( + /// [0.; 3], + /// Point3::new(1.,1.,1.), + /// AddDebugLineOptions { + /// line_color_rgb: [1., 0., 0.], + /// ..Default::default() + /// }, + /// )?; + ///# std::thread::sleep(Duration::from_secs(10)); + ///# Ok(()) + ///# } + /// ``` + pub fn add_user_debug_line< + Options: Into>, + Start: Into>, + End: Into>, + >( + &mut self, + line_from_xyz: Start, + line_to_xyz: End, + options: Options, + ) -> Result { + unsafe { + let options = options.into().unwrap_or_default(); + let command_handle = ffi::b3InitUserDebugDrawAddLine3D( + self.handle, + line_from_xyz.into().coords.as_ptr(), + line_to_xyz.into().coords.as_ptr(), + options.line_color_rgb.as_ptr(), + options.line_width, + options.life_time, + ); + if let Some(parent) = options.parent_object_id { + let parent_link_index = match options.parent_link_index { + None => -1, + Some(index) => index as i32, + }; + ffi::b3UserDebugItemSetParentObject(command_handle, parent.0, parent_link_index); + } + if let Some(replacement) = options.replace_item_id { + ffi::b3UserDebugItemSetReplaceItemUniqueId(command_handle, replacement.0); + } + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_USER_DEBUG_DRAW_COMPLETED as i32 { + let debug_item = ItemId(ffi::b3GetDebugItemUniqueId(status_handle)); + return Ok(debug_item); + } + Err(Error::new("Error in addUserDebugLine.")) + } + } + /// Lets you add custom sliders and buttons to tune parameters. + /// + /// # Arguments + /// * `param_name` - name of the parameter. Needs to be something that can be converted to a &str. + /// * `range_min` - minimum value of the slider. If minimum value > maximum value a button instead + /// of a slider will appear. + /// * `range_max` - maximum value of the slider + /// * `start_value` - starting value of the slider + /// + /// # Return + /// A unique item id of the button/slider, which can be used by [`read_user_debug_parameter()`](`Self::read_user_debug_parameter()`) + /// # Example + /// ```no_run + /// use rubullet::PhysicsClient; + /// use rubullet::Mode::Gui; + /// use anyhow::Result; + /// pub fn main() -> Result<()> { + /// let mut client = PhysicsClient::connect(Gui)?; + /// let slider = client.add_user_debug_parameter("my_slider",0.,1.,0.5)?; + /// let button = client.add_user_debug_parameter("my_button",1.,0.,1.)?; + /// let value_slider = client.read_user_debug_parameter(slider)?; + /// let value_button = client.read_user_debug_parameter(button)?; // value increases by one for every press + /// Ok(()) + /// } + /// ``` + pub fn add_user_debug_parameter<'a, Text: Into<&'a str>>( + &mut self, + param_name: Text, + range_min: f64, + range_max: f64, + start_value: f64, + ) -> Result { + unsafe { + let param_name = CString::new(param_name.into().as_bytes()).unwrap(); + let command_handle = ffi::b3InitUserDebugAddParameter( + self.handle, + param_name.as_ptr(), + range_min, + range_max, + start_value, + ); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_USER_DEBUG_DRAW_COMPLETED as i32 { + let debug_item_unique_id = ffi::b3GetDebugItemUniqueId(status_handle); + return Ok(ItemId(debug_item_unique_id)); + } + Err(Error::new("Error in addUserDebugParameter.")) + } + } + /// Reads the current value of a debug parameter. For a button the value will increase by 1 every + /// time the button is clicked. + /// + /// # Arguments + /// `item` - the unique item generated by [`add_user_debug_parameter()`)[`Self::add_user_debug_parameter()`] + /// See [`add_user_debug_parameter()`)[`Self::add_user_debug_parameter()`] for an example. + pub fn read_user_debug_parameter(&mut self, item: ItemId) -> Result { + unsafe { + let command_handle = ffi::b3InitUserDebugReadParameter(self.handle, item.0); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED as i32 { + let mut param_value = 0.; + let ok = ffi::b3GetStatusDebugParameterValue(status_handle, &mut param_value); + if ok != 0 { + return Ok(param_value); + } + } + Err(Error::new("Failed to read parameter.")) + } + } + /// You can add some 3d text at a specific location using a color and size. + /// # Arguments + /// * `text` - text represented by something which can be converted to a &str + /// * `text_position` - 3d position of the text in Cartesian world coordinates \[x,y,z\]. Can be + /// a Point3, a Vector3, an array or anything else than can be converted into a Point3. + /// * `options` - advanced options for the text. Use None for default settings. + /// + /// # Return + /// A unique item id of the text. + /// You can use [`remove_user_debug_item()`](`Self::remove_user_debug_item()`) to delete it. + /// + /// # Example + /// ```no_run + ///# use anyhow::Result; + ///# use rubullet::Mode::Gui; + ///# use rubullet::AddDebugTextOptions; + ///# use rubullet::PhysicsClient; + ///# use std::time::Duration; + ///# use nalgebra::Point3; + ///# pub fn main() -> Result<()> { + ///# use nalgebra::UnitQuaternion; + ///# use std::f64::consts::PI; + /// let mut client = PhysicsClient::connect(Gui)?; + /// let text = client.add_user_debug_text("My text", Point3::new(0., 0., 1.), None)?; + /// let text_red_on_floor = client.add_user_debug_text( + /// "My red text on the floor", + /// [0.;3], + /// AddDebugTextOptions { + /// text_color_rgb: [1., 0., 0.], + /// text_orientation: Some(UnitQuaternion::from_euler_angles(0.,0.,0.)), + /// ..Default::default() + /// }, + /// )?; + ///# std::thread::sleep(Duration::from_secs(10)); + ///# Ok(()) + ///# } + /// ``` + pub fn add_user_debug_text< + 'a, + Text: Into<&'a str>, + Position: Into>, + Options: Into>, + >( + &mut self, + text: Text, + text_position: Position, + options: Options, + ) -> Result { + unsafe { + let options = options.into().unwrap_or_default(); + let text = CString::new(text.into().as_bytes()).unwrap(); + let command_handle = ffi::b3InitUserDebugDrawAddText3D( + self.handle, + text.as_ptr(), + text_position.into().coords.as_ptr(), + options.text_color_rgb.as_ptr(), + options.text_size, + options.life_time, + ); + if let Some(parent_object) = options.parent_object_id { + let parent_link_index = match options.parent_link_index { + None => -1, + Some(index) => index as i32, + }; + ffi::b3UserDebugItemSetParentObject( + command_handle, + parent_object.0, + parent_link_index, + ); + } + if let Some(text_orientation) = options.text_orientation { + ffi::b3UserDebugTextSetOrientation( + command_handle, + text_orientation.coords.as_ptr(), + ); + } + if let Some(replacement_id) = options.replace_item_id { + ffi::b3UserDebugItemSetReplaceItemUniqueId(command_handle, replacement_id.0); + } + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_USER_DEBUG_DRAW_COMPLETED as i32 { + let debug_item_id = ItemId(ffi::b3GetDebugItemUniqueId(status_handle)); + return Ok(debug_item_id); + } + } + Err(Error::new("Error in add_user_debug_text")) + } + /// Removes debug items which were created with [`add_user_debug_line`](`crate::PhysicsClient::add_user_debug_line()`), + /// [`add_user_debug_parameter`](`crate::PhysicsClient::add_user_debug_parameter()`) or + /// [`add_user_debug_text`](`crate::PhysicsClient::add_user_debug_text()`). + /// + /// # Arguments + /// * `item` - unique id of the debug item to be removed (line, text etc) + /// + /// # Example + /// ```no_run + ///# use anyhow::Result; + ///# use rubullet::Mode::Gui; + ///# use rubullet::PhysicsClient; + ///# use std::time::Duration; + ///# + ///# pub fn main() -> Result<()> { + ///# let mut client = PhysicsClient::connect(Gui)?; + /// let text = client.add_user_debug_text("My text", [0., 0., 1.], None)?; + /// client.remove_user_debug_item(text); + ///# Ok(()) + ///# } + /// ``` + pub fn remove_user_debug_item(&mut self, item: ItemId) { + unsafe { + let command_handle = ffi::b3InitUserDebugDrawRemove(self.handle, item.0); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let _status_type = ffi::b3GetStatusType(status_handle); + } + } + /// will remove all debug items (text, lines etc). + /// # Example + /// ```no_run + ///# use anyhow::Result; + ///# use rubullet::Mode::Gui; + ///# use rubullet::PhysicsClient; + ///# use std::time::Duration; + ///# + ///# pub fn main() -> Result<()> { + ///# use nalgebra::Point3; + /// let mut client = PhysicsClient::connect(Gui)?; + /// let text = client.add_user_debug_text("My text", Point3::new(0., 0., 1.), None)?; + /// let text_2 = client.add_user_debug_text("My text2", [0., 0., 2.], None)?; + /// client.remove_all_user_debug_items(); + ///# std::thread::sleep(Duration::from_secs(10)); + ///# Ok(()) + ///# } + /// ``` + pub fn remove_all_user_debug_items(&mut self) { + unsafe { + let command_handle = ffi::b3InitUserDebugDrawRemoveAll(self.handle); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let _status_type = ffi::b3GetStatusType(status_handle); + } + } + /// The built-in OpenGL visualizers have a wireframe debug rendering feature: press 'w' to toggle. + /// The wireframe has some default colors. + /// You can override the color of a specific object and link using this method. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_index` - link index. Use None for the base. + /// * `object_debug_color` - debug color in \[Red,Green,Blue\]. If not provided, the custom color will be removed. + pub fn set_debug_object_color>, Color: Into>>( + &mut self, + body: BodyId, + link_index: Link, + object_debug_color: Color, + ) { + unsafe { + let link_index = match link_index.into() { + None => -1, + Some(index) => index as i32, + }; + let command_handle = ffi::b3InitDebugDrawingCommand(self.handle); + + if let Some(color) = object_debug_color.into() { + ffi::b3SetDebugObjectColor(command_handle, body.0, link_index, color.as_ptr()); + } else { + ffi::b3RemoveDebugObjectColor(command_handle, body.0, link_index); + } + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + } + } + /// You can receive all keyboard events that happened since the last time you called + /// [`get_keyboard_events()`](`Self::get_keyboard_events()`) + /// This method will return a List of all KeyboardEvents that happened since then. + /// + /// # Example + /// ```no_run + /// use std::time::Duration; + /// use anyhow::Result; + /// use rubullet::*; + /// + /// fn main() -> Result<()> { + /// let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + /// loop { + /// let events = physics_client.get_keyboard_events(); + /// for event in events.iter() { + /// if event.key == 'i' && event.was_triggered() { + /// println!("i-key was pressed"); + /// } + /// } + /// std::thread::sleep(Duration::from_secs_f64(0.01)); + /// } + /// Ok(()) + /// } + /// ``` + pub fn get_keyboard_events(&mut self) -> Vec { + unsafe { + let mut keyboard_events = b3KeyboardEventsData::default(); + let command_handle = ffi::b3RequestKeyboardEventsCommandInit(self.handle); + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + ffi::b3GetKeyboardEventsData(self.handle, &mut keyboard_events); + let mut events = + Vec::::with_capacity(keyboard_events.m_numKeyboardEvents as usize); + let data = std::slice::from_raw_parts_mut( + keyboard_events.m_keyboardEvents, + keyboard_events.m_numKeyboardEvents as usize, + ); + for &event in data.iter() { + events.push(KeyboardEvent { + key: std::char::from_u32(event.m_keyCode as u32).expect("Got invalid key code"), + key_state: event.m_keyState, + }); + } + events + } + } + + /// Similar to [`get_keyboard_events()`](`Self::get_keyboard_events()`) + /// you can get the mouse events that happened since the last call to [`get_mouse_events()`](`Self::get_mouse_events()`). + /// All the mouse move events are merged into a single mouse move event with the most up-to-date position. + /// The mouse move event is only returned when the mouse has been moved. The mouse button event + /// always includes the current mouse position. + /// # Example + /// ```no_run + /// use anyhow::Result; + /// use rubullet::*; + /// use std::time::Duration; + /// + /// fn main() -> Result<()> { + /// let mut physics_client = PhysicsClient::connect(Mode::Gui)?; + /// loop { + /// let events = physics_client.get_mouse_events(); + /// for event in events.iter() { + /// match event { + /// MouseEvent::Move { + /// mouse_pos_x, + /// mouse_pos_y, + /// } => { + /// println!( + /// "The mouse has moved to x: {}, y: {}", + /// mouse_pos_x, mouse_pos_y + /// ); + /// } + /// MouseEvent::Button { + /// mouse_pos_x, + /// mouse_pos_y, + /// button_index, + /// button_state, + /// } => { + /// println!( + /// "The mouse position is x: {}, y: {}", + /// mouse_pos_x, mouse_pos_y + /// ); + /// if button_state.was_triggered() { + /// println!("Mouse Button {} has been triggered", button_index); + /// } + /// } + /// } + /// } + /// std::thread::sleep(Duration::from_secs_f64(0.01)); + /// } + /// Ok(()) + /// } + /// ``` + pub fn get_mouse_events(&mut self) -> Vec { + unsafe { + let mut mouse_events = b3MouseEventsData::default(); + let command_handle = ffi::b3RequestMouseEventsCommandInit(self.handle); + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + ffi::b3GetMouseEventsData(self.handle, &mut mouse_events); + let mut events = + Vec::::with_capacity(mouse_events.m_numMouseEvents as usize); + let data = std::slice::from_raw_parts_mut( + mouse_events.m_mouseEvents, + mouse_events.m_numMouseEvents as usize, + ); + for &event in data.iter() { + if event.m_eventType == 1 { + events.push(MouseEvent::Move { + mouse_pos_x: event.m_mousePosX, + mouse_pos_y: event.m_mousePosY, + }); + } else if event.m_eventType == 2 { + events.push(MouseEvent::Button { + mouse_pos_x: event.m_mousePosX, + mouse_pos_y: event.m_mousePosY, + button_index: event.m_buttonIndex, + button_state: MouseButtonState { + flag: event.m_buttonState, + }, + }); + } + } + events + } + } + /// Applies a force to a body. + /// + /// Note that this method will only work when explicitly stepping the simulation using + /// [`step_simulation()`](`Self::step_simulation()`), in other words: + /// [`set_real_time_simulation(false)`](`Self::set_real_time_simulation()`) + /// After each simulation step, the external forces are cleared to zero. + /// If you are using [`set_real_time_simulation(true)`](`Self::set_real_time_simulation()`), + /// This method will have undefined behavior (either 0, 1 or multiple force applications). + /// + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_index` - link index or None for the base. + /// * `force_object` - force vector to be applied \[x,y,z\] either as an array, Point3 or Vector3. + /// See flags for coordinate system + /// * `position_object` - position on the link where the force is applied. + /// * `flags` - Specify the coordinate system of force/position: + /// either WORLD_FRAME for Cartesian world coordinates or LINK_FRAME for local link coordinates. + pub fn apply_external_force< + Force: Into>, + Position: Into>, + Link: Into>, + >( + &mut self, + body: BodyId, + link_index: Link, + force_object: Force, + position_object: Position, + flags: ExternalForceFrame, + ) { + let link_index = match link_index.into() { + None => -1, + Some(index) => index as i32, + }; + unsafe { + let command = ffi::b3ApplyExternalForceCommandInit(self.handle); + ffi::b3ApplyExternalForce( + command, + body.0, + link_index, + force_object.into().as_ptr(), + position_object.into().coords.as_ptr(), + flags as i32, + ); + let _status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + } + /// Applies a torque to a body. + /// + /// Note that this method will only work when explicitly stepping the simulation using + /// [`step_simulation()`](`Self::step_simulation()`), in other words: [`set_real_time_simulation(false)`](`Self::set_real_time_simulation()`) + /// After each simulation step, the external torques are cleared to zero. + /// If you are using [`set_real_time_simulation(true)`](`Self::set_real_time_simulation()`), + /// This method will have undefined behavior (either 0, 1 or multiple torque applications). + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_index` - link index or None for the base. + /// * `torque_object` - torque vector to be applied \[x,y,z\] either as an array or a Vector3. + /// See flags for coordinate system + /// * `flags` - Specify the coordinate system of torque: + /// either WORLD_FRAME for Cartesian world coordinates or LINK_FRAME for local link coordinates. + pub fn apply_external_torque>, Link: Into>>( + &mut self, + body: BodyId, + link_index: Link, + torque_object: Torque, + flags: ExternalForceFrame, + ) { + let link_index = match link_index.into() { + None => -1, + Some(index) => index as i32, + }; + unsafe { + let command = ffi::b3ApplyExternalForceCommandInit(self.handle); + ffi::b3ApplyExternalTorque( + command, + body.0, + link_index, + torque_object.into().as_ptr(), + flags as i32, + ); + let _status_handle = ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command); + } + } + /// You can enable or disable a joint force/torque sensor in each joint. + /// Once enabled, if you perform a [`step_simulation()`](`Self::step_simulation()`), + /// the [`get_joint_state()`](`Self::get_joint_state()`) will report + /// the joint reaction forces in the fixed degrees of freedom: + /// a fixed joint will measure all 6DOF joint forces/torques. + /// A revolute/hinge joint force/torque sensor will measure 5DOF reaction forces + /// along all axis except the hinge axis. The applied force by a joint motor is available in the + /// applied_joint_motor_torque field in [`JointState`](`crate::types::JointState`) + /// if you call [`get_joint_state()`](`Self::get_joint_state()`). + pub fn enable_joint_torque_sensor( + &mut self, + body: BodyId, + joint_index: usize, + enable_sensor: bool, + ) -> Result<(), Error> { + unsafe { + let command_handle = ffi::b3CreateSensorCommandInit(self.handle, body.0); + ffi::b3CreateSensorEnable6DofJointForceTorqueSensor( + command_handle, + joint_index as i32, + enable_sensor as i32, + ); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CLIENT_COMMAND_COMPLETED as i32 { + return Ok(()); + } + Err(Error::new("Error creating sensor.")) + } + } + /// You can create a collision shape in a similar way to creating a visual shape. If you have + /// both you can use them to create objects in RuBullet. + /// # Arguments + /// * `shape` - A geometric body from which to create the shape + /// * `frame_offset` - offset of the shape with respect to the link frame. + /// + /// # Return + /// Returns a unique [CollisionId](crate::CollisionId) which can then be used to create a body. + /// # See also + /// * [create_visual_shape](`Self::create_visual_shape`) + /// * [create_multi_body](`Self::create_multi_body`) + pub fn create_collision_shape( + &mut self, + shape: GeometricCollisionShape, + frame_offset: Isometry3, + ) -> Result { + unsafe { + let mut shape_index = -1; + let command_handle = ffi::b3CreateCollisionShapeCommandInit(self.handle); + + match shape { + GeometricCollisionShape::Sphere { radius } if radius > 0. => { + shape_index = ffi::b3CreateCollisionShapeAddSphere(command_handle, radius); + } + GeometricCollisionShape::Box { half_extents } => { + shape_index = + ffi::b3CreateCollisionShapeAddBox(command_handle, half_extents.as_ptr()); + } + GeometricCollisionShape::Capsule { radius, height } + if radius > 0. && height >= 0. => + { + shape_index = + ffi::b3CreateCollisionShapeAddCapsule(command_handle, radius, height); + } + GeometricCollisionShape::Cylinder { radius, height } + if radius > 0. && height >= 0. => + { + shape_index = + ffi::b3CreateCollisionShapeAddCylinder(command_handle, radius, height); + } + GeometricCollisionShape::HeightfieldFile { + filename, + mesh_scaling, + texture_scaling, + } => { + let file = CString::new(filename.into_os_string().as_bytes()).unwrap(); + shape_index = ffi::b3CreateCollisionShapeAddHeightfield( + command_handle, + file.as_ptr(), + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + texture_scaling, + ); + } + GeometricCollisionShape::Heightfield { + mesh_scaling, + texture_scaling: heightfield_texture_scaling, + data: mut heightfield_data, + num_rows: num_heightfield_rows, + num_columns: num_heightfield_columns, + replace_heightfield, + } => { + if num_heightfield_columns > 0 && num_heightfield_rows > 0 { + let num_height_field_points = heightfield_data.len(); + assert_eq!( num_heightfield_rows * num_heightfield_columns, + num_height_field_points, + "Size of heightfield_data ({}) doesn't match num_heightfield_columns * num_heightfield_rows = {}", + num_height_field_points, + num_heightfield_rows * num_heightfield_columns, + ); + shape_index = ffi::b3CreateCollisionShapeAddHeightfield2( + self.handle, + command_handle, + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + heightfield_texture_scaling, + heightfield_data.as_mut_slice().as_mut_ptr(), + num_heightfield_rows as i32, + num_heightfield_columns as i32, + replace_heightfield.unwrap_or_else(|| CollisionId(-1)).0, + ); + } + } + GeometricCollisionShape::MeshFile { + filename, + mesh_scaling, + flags, + } => { + let file = CString::new(filename.into_os_string().as_bytes()).unwrap(); + shape_index = ffi::b3CreateCollisionShapeAddMesh( + command_handle, + file.as_ptr(), + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + ); + if shape_index >= 0 { + if let Some(flags) = flags { + ffi::b3CreateCollisionSetFlag(command_handle, shape_index, flags); + } + } + } + GeometricCollisionShape::Mesh { + vertices, + indices, + mesh_scaling, + } => { + if vertices.len() > B3_MAX_NUM_VERTICES { + return Err(Error::new("Number of vertices exceeds the maximum.")); + } + + let mut new_vertices = Vec::::with_capacity(vertices.len() * 3); + for vertex in vertices.iter() { + new_vertices.extend_from_slice(vertex); + } + if let Some(indices) = indices { + if indices.len() > B3_MAX_NUM_INDICES { + return Err(Error::new("Number of indices exceeds the maximum.")); + } + shape_index = ffi::b3CreateCollisionShapeAddConcaveMesh( + self.handle, + command_handle, + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + new_vertices.as_slice().as_ptr(), + vertices.len() as i32, + indices.as_slice().as_ptr(), + indices.len() as i32, + ); + } else { + shape_index = ffi::b3CreateCollisionShapeAddConvexMesh( + self.handle, + command_handle, + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + new_vertices.as_slice().as_ptr(), + vertices.len() as i32, + ); + } + } + GeometricCollisionShape::Plane { plane_normal } => { + let plane_constant = 0.; + shape_index = ffi::b3CreateCollisionShapeAddPlane( + command_handle, + plane_normal.as_ptr(), + plane_constant, + ); + } + _ => {} + } + if shape_index >= 0 { + let position_vector = &frame_offset.translation.vector; + let rotation = &frame_offset.rotation; + let position_array = [position_vector.x, position_vector.y, position_vector.z]; + let rotation_array = [rotation.i, rotation.j, rotation.k, rotation.w]; + ffi::b3CreateCollisionShapeSetChildTransform( + command_handle, + shape_index, + position_array.as_ptr(), + rotation_array.as_ptr(), + ); + } + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED as i32 { + let uid = ffi::b3GetStatusCollisionShapeUniqueId(status_handle); + return Ok(CollisionId(uid)); + } + Err(Error::new("create_collision_shape failed.")) + } + } + /// You can create a visual shape in a similar way to creating a collision shape, with some + /// additional arguments to control the visual appearance, such as diffuse and specular color. + /// When you use the [GeometricVisualShape::MeshFile](`crate::GeometricVisualShape::MeshFile`) + /// type, you can point to a Wavefront OBJ file, and the + /// visual shape will parse some parameters from the material file (.mtl) and load a texture. + /// Note that large textures (above 1024x1024 pixels) + /// can slow down the loading and run-time performance. + /// + /// # Arguments + /// * `shape` - A geometric body from which to create the shape + /// * `options` - additional option to specify, like colors. + /// + /// # Return + /// Returns a unique [VisualId](crate::VisualId) which can then be used to create a body. + /// # See also + /// * [create_collision_shape](`Self::create_collision_shape`) + /// * [create_multi_body](`Self::create_multi_body`) + pub fn create_visual_shape( + &mut self, + shape: GeometricVisualShape, + options: VisualShapeOptions, + ) -> Result { + unsafe { + let mut shape_index = -1; + let command_handle = ffi::b3CreateVisualShapeCommandInit(self.handle); + + match shape { + GeometricVisualShape::Sphere { radius } if radius > 0. => { + shape_index = ffi::b3CreateVisualShapeAddSphere(command_handle, radius); + } + GeometricVisualShape::Box { half_extents } => { + shape_index = + ffi::b3CreateVisualShapeAddBox(command_handle, half_extents.as_ptr()); + } + GeometricVisualShape::Capsule { radius, length } if radius > 0. && length > 0. => { + shape_index = + ffi::b3CreateVisualShapeAddCapsule(command_handle, radius, length); + } + GeometricVisualShape::Cylinder { radius, length } => { + shape_index = + ffi::b3CreateVisualShapeAddCylinder(command_handle, radius, length); + } + + GeometricVisualShape::MeshFile { + filename, + mesh_scaling, + } => { + let file = CString::new(filename.into_os_string().as_bytes()).unwrap(); + shape_index = ffi::b3CreateVisualShapeAddMesh( + command_handle, + file.as_ptr(), + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + ); + } + GeometricVisualShape::Mesh { + mesh_scaling, + vertices, + indices, + uvs, + normals, + } => { + let mut new_vertices = Vec::::with_capacity(vertices.len() * 3); + let mut new_normals = Vec::::with_capacity(vertices.len() * 3); + let mut new_uvs = Vec::::with_capacity(vertices.len() * 2); + + if vertices.len() > B3_MAX_NUM_VERTICES { + return Err(Error::new("Number of vertices exceeds the maximum.")); + } + for vertex in vertices.iter() { + new_vertices.extend_from_slice(vertex); + } + + if indices.len() > B3_MAX_NUM_INDICES { + return Err(Error::new("Number of indices exceeds the maximum.")); + } + let new_indices = indices; + + if let Some(uvs) = uvs { + if uvs.len() > B3_MAX_NUM_VERTICES { + return Err(Error::new("Number of uvs exceeds the maximum.")); + } + for uv in uvs.iter() { + new_uvs.extend_from_slice(uv); + } + } + if let Some(normals) = normals { + if normals.len() > B3_MAX_NUM_VERTICES { + return Err(Error::new("Number of normals exceeds the maximum.")); + } + for normal in normals.iter() { + new_normals.extend_from_slice(normal); + } + } + shape_index = ffi::b3CreateVisualShapeAddMesh2( + self.handle, + command_handle, + mesh_scaling + .unwrap_or_else(|| Vector3::from_element(1.)) + .as_ptr(), + new_vertices.as_slice().as_ptr(), + new_vertices.len() as i32 / 3, + new_indices.as_slice().as_ptr(), + new_indices.len() as i32, + new_normals.as_slice().as_ptr(), + new_normals.len() as i32 / 3, + new_uvs.as_slice().as_ptr(), + new_uvs.len() as i32 / 2, + ); + } + GeometricVisualShape::Plane { plane_normal } => { + let plane_constant = 0.; + shape_index = ffi::b3CreateVisualShapeAddPlane( + command_handle, + plane_normal.as_ptr(), + plane_constant, + ); + } + _ => {} + } + + if shape_index >= 0 { + if let Some(flags) = options.flags { + ffi::b3CreateVisualSetFlag(command_handle, shape_index, flags); + } + ffi::b3CreateVisualShapeSetRGBAColor( + command_handle, + shape_index, + options.rgba_colors.as_ptr(), + ); + ffi::b3CreateVisualShapeSetSpecularColor( + command_handle, + shape_index, + options.specular_colors.as_ptr(), + ); + let position_vector = &options.frame_offset.translation.vector; + let rotation = &options.frame_offset.rotation; + let position_array = [position_vector.x, position_vector.y, position_vector.z]; + let rotation_array = [rotation.i, rotation.j, rotation.k, rotation.w]; + ffi::b3CreateVisualShapeSetChildTransform( + command_handle, + shape_index, + position_array.as_ptr(), + rotation_array.as_ptr(), + ); + } + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED as i32 { + let uid = ffi::b3GetStatusVisualShapeUniqueId(status_handle); + if uid == -1 { + return Err(Error::new("create visual Shape failed.")); + } + return Ok(VisualId(uid)); + } + Err(Error::new("create visual Shape failed.")) + } + } + /// You can create a multi body with only a single base without joints/child links or + /// you can create a multi body with joints/child links. If you provide links, make sure + /// the length of every vector is the same . + /// # Arguments + /// * `base_collision_shape` - unique id from [create_collision_shape](`Self::create_collision_shape`) + /// or use [`CollisionId::NONE`](`crate::types::CollisionId::NONE`) if you do not want to have a collision shape. + /// You can re-use the collision shape for multiple multibodies (instancing) + /// * `base_visual_shape` - unique id from [create_visual_shape](`Self::create_visual_shape`) + /// or use [`VisualId::NONE`](`crate::types::VisualId::NONE`) if you do not want to set a visual shape. + /// You can re-use the visual shape (instancing) + /// * `options` - additional options for creating a multi_body. See [MultiBodyOptions](`crate::MultiBodyOptions`) + /// for details + /// + /// # Return + /// returns the [BodyId](`crate::BodyId`) of the newly created body. + /// + /// Note: Currently, if you use `batch_positions` you will not get back a list of Id's, like in PyBullet. + /// Instead you will get the Id of the last body in the batch. + /// + /// # Example + /// ```rust + ///# use anyhow::Result; + ///# use nalgebra::Isometry3; + ///# use nalgebra::Vector3; + ///# use rubullet::Mode::Direct; + ///# use rubullet::*; + ///# use std::time::Duration; + ///# fn main() -> Result<()> { + ///# + ///# let mut physics_client = PhysicsClient::connect(Direct)?; + /// let sphere_shape = GeometricCollisionShape::Sphere { radius: 0.4 }; + /// let box_collision = physics_client.create_collision_shape(sphere_shape, Isometry3::identity())?; + /// let box_shape = GeometricVisualShape::Box { + /// half_extents: Vector3::from_element(0.5), + /// }; + /// let box_visual = physics_client.create_visual_shape( + /// box_shape, + /// VisualShapeOptions { + /// rgba_colors: [0., 1., 0., 1.], + /// ..Default::default() + /// }, + /// )?; + /// let box_id = + /// physics_client.create_multi_body(box_collision, box_visual, MultiBodyOptions::default())?; + ///# Ok(()) + ///# } + /// ``` + // TODO: think about splitting this function in two function. A normal one. And one for batches which returns a list instead + pub fn create_multi_body( + &mut self, + base_collision_shape: CollisionId, + base_visual_shape: VisualId, + options: MultiBodyOptions, + ) -> Result { + unsafe { + assert!( + options.link_masses.len() == options.link_collision_shapes.len() + && options.link_masses.len() == options.link_visual_shapes.len() + && options.link_masses.len() == options.link_poses.len() + && options.link_masses.len() == options.link_joint_types.len() + && options.link_masses.len() == options.link_joint_axis.len() + && options.link_masses.len() == options.link_inertial_frame_poses.len() + && options.link_masses.len() == options.link_collision_shapes.len(), + "All link arrays need to be same size." + ); + + let command_handle = ffi::b3CreateMultiBodyCommandInit(self.handle); + let position_vector = &options.base_pose.translation.vector; + let rotation = &options.base_pose.rotation; + let base_position_array = [position_vector.x, position_vector.y, position_vector.z]; + let base_rotation_array = [rotation.i, rotation.j, rotation.k, rotation.w]; + + let position_vector = &options.base_inertial_frame_pose.translation.vector; + let rotation = &options.base_inertial_frame_pose.rotation; + let base_inertial_position_array = + [position_vector.x, position_vector.y, position_vector.z]; + let base_inertial_rotation_array = [rotation.i, rotation.j, rotation.k, rotation.w]; + let _base_index = ffi::b3CreateMultiBodyBase( + command_handle, + options.base_mass, + base_collision_shape.0, + base_visual_shape.0, + base_position_array.as_ptr(), + base_rotation_array.as_ptr(), + base_inertial_position_array.as_ptr(), + base_inertial_rotation_array.as_ptr(), + ); + if let Some(batch_positions) = options.batch_positions { + let mut new_batch_positions = Vec::::with_capacity(batch_positions.len() * 3); + for pos in batch_positions.iter() { + new_batch_positions.extend_from_slice(pos.coords.as_slice()); + } + ffi::b3CreateMultiBodySetBatchPositions( + self.handle, + command_handle, + new_batch_positions.as_mut_slice().as_mut_ptr(), + batch_positions.len() as i32, + ); + } + for i in 0..options.link_masses.len() { + let link_mass = options.link_masses[i]; + let link_collision_shape_index = options.link_collision_shapes[i].0; + let link_visual_shape_index = options.link_visual_shapes[i].0; + let position_vector = &options.link_poses[i].translation.vector; + let rotation = &options.link_poses[i].rotation; + let link_position = [position_vector.x, position_vector.y, position_vector.z]; + let link_orientation = [rotation.i, rotation.j, rotation.k, rotation.w]; + + let link_joint_axis: [f64; 3] = options.link_joint_axis[i].into(); + + let position_vector = &options.link_inertial_frame_poses[i].translation.vector; + let rotation = &options.link_inertial_frame_poses[i].rotation; + let link_inertial_frame_position = + [position_vector.x, position_vector.y, position_vector.z]; + let link_inertial_frame_orientation = + [rotation.i, rotation.j, rotation.k, rotation.w]; + + let link_parent_index = options.link_parent_indices[i]; + let link_joint_type = options.link_joint_types[i] as i32; + + ffi::b3CreateMultiBodyLink( + command_handle, + link_mass, + link_collision_shape_index as f64, + link_visual_shape_index as f64, + link_position.as_ptr(), + link_orientation.as_ptr(), + link_inertial_frame_position.as_ptr(), + link_inertial_frame_orientation.as_ptr(), + link_parent_index, + link_joint_type, + link_joint_axis.as_ptr(), + ); + } + if options.use_maximal_coordinates { + ffi::b3CreateMultiBodyUseMaximalCoordinates(command_handle); + } + if let Some(flags) = options.flags { + ffi::b3CreateMultiBodySetFlags(command_handle, flags.bits()); + } + let status_handle = b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_CREATE_MULTI_BODY_COMPLETED as i32 { + let uid = ffi::b3GetStatusBodyIndex(status_handle); + return Ok(BodyId(uid)); + } + } + Err(Error::new("create_multi_body failed.")) + } + /// Use this function to change the texture of a shape, + /// the RGBA color and other properties. + /// # Arguments + /// * `body` - the [`BodyId`](`crate::types::BodyId`), as returned by [`load_urdf`](`Self::load_urdf()`) etc. + /// * `link_index` - link index or None for the base. + /// * `options` - optional parameters to change the visual shape. + /// See [ChangeVisualShapeOptions](`crate::types::ChangeVisualShapeOptions`) + /// # Example + /// In this example we change the color of a shape + /// ```rust + ///# use anyhow::Result; + ///# use nalgebra::Isometry3; + ///# use nalgebra::Vector3; + ///# use rubullet::Mode::Direct; + ///# use rubullet::*; + ///# use std::time::Duration; + ///# fn main() -> Result<()> { + ///# + ///# let mut physics_client = PhysicsClient::connect(Direct)?; + /// let sphere_shape = GeometricCollisionShape::Sphere { radius: 0.4 }; + /// let box_collision = physics_client.create_collision_shape(sphere_shape, Isometry3::identity())?; + /// let box_shape = GeometricVisualShape::Box { + /// half_extents: Vector3::from_element(0.5), + /// }; + /// let box_visual = physics_client.create_visual_shape( + /// box_shape, + /// VisualShapeOptions { + /// rgba_colors: [0., 1., 0., 1.], + /// ..Default::default() + /// }, + /// )?; + /// let box_id = + /// physics_client.create_multi_body(box_collision, box_visual, MultiBodyOptions::default())?; + /// + /// let color = physics_client.get_visual_shape_data(box_id,false)?[0].rgba_color; + /// assert_eq!(color, [0.,1.,0.,1.]); + /// + /// physics_client.change_visual_shape( + /// box_id, + /// None, + /// ChangeVisualShapeOptions { + /// rgba_color: Some([1., 0., 0., 1.]), + /// ..Default::default() + /// }, + /// )?; + /// + /// let color = physics_client.get_visual_shape_data(box_id,false)?[0].rgba_color; + /// assert_eq!(color, [1.,0.,0.,1.]); + ///# Ok(()) + ///# } + /// ``` + pub fn change_visual_shape>>( + &mut self, + body: BodyId, + link_index: Link, + options: ChangeVisualShapeOptions, + ) -> Result<(), Error> { + let link_index = match link_index.into() { + None => -1, + Some(index) => index as i32, + }; + unsafe { + let command_handle = + ffi::b3InitUpdateVisualShape2(self.handle, body.0, link_index, options.shape.0); + if let Some(texture_id) = options.texture_id { + ffi::b3UpdateVisualShapeTexture(command_handle, texture_id.0); + } + if let Some(specular) = options.specular_color { + ffi::b3UpdateVisualShapeSpecularColor(command_handle, specular.as_ptr()); + } + if let Some(rgba) = options.rgba_color { + ffi::b3UpdateVisualShapeRGBAColor(command_handle, rgba.as_ptr()); + } + if let Some(flags) = options.flags { + ffi::b3UpdateVisualShapeFlags(command_handle, flags); + } + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + + if status_type != CMD_VISUAL_SHAPE_UPDATE_COMPLETED as i32 { + return Err(Error::new("Error resetting visual shape info")); + } + } + Ok(()) + } + /// Returns a list of visual shape data of a body. + /// See [`change_visual_shape()`](`Self::change_visual_shape`) for an example. + pub fn get_visual_shape_data( + &mut self, + body: BodyId, + request_texture_id: bool, + ) -> Result, Error> { + unsafe { + let command_handle = ffi::b3InitRequestVisualShapeInformation(self.handle, body.0); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_VISUAL_SHAPE_INFO_COMPLETED as i32 { + let mut visual_shape_info = ffi::b3VisualShapeInformation::default(); + ffi::b3GetVisualShapeInformation(self.handle, &mut visual_shape_info); + let mut visual_shapes: Vec = + Vec::with_capacity(visual_shape_info.m_numVisualShapes as usize); + let data = std::slice::from_raw_parts_mut( + visual_shape_info.m_visualShapeData, + visual_shape_info.m_numVisualShapes as usize, + ); + for i in 0..visual_shape_info.m_numVisualShapes { + let mut shape_data: VisualShapeData = data[i as usize].into(); + if request_texture_id { + shape_data.texture_id = Some(TextureId(data[i as usize].m_textureUniqueId)); + } + visual_shapes.push(shape_data); + } + return Ok(visual_shapes); + } + } + Err(Error::new("Error receiving visual shape info")) + } + /// Load a texture from file and return a non-negative texture unique id if the loading succeeds. + /// This unique id can be used with [change_visual_shape](`Self::change_visual_shape`). + /// + /// + /// See create_multi_body_batch.rs for an example + pub fn load_texture>(&mut self, file: File) -> Result { + unsafe { + let cfilename = CString::new(file.as_ref().as_os_str().as_bytes()).unwrap(); + let command_handle = ffi::b3InitLoadTexture(self.handle, cfilename.as_ptr()); + let status_handle = + ffi::b3SubmitClientCommandAndWaitStatus(self.handle, command_handle); + let status_type = ffi::b3GetStatusType(status_handle); + if status_type == CMD_LOAD_TEXTURE_COMPLETED as i32 { + let texture_id = TextureId(ffi::b3GetStatusTextureUniqueId(status_handle)); + return Ok(texture_id); + } + } + Err(Error::new("Error loading texture")) + } + /// will remove a body by its body unique id + pub fn remove_body(&mut self, body: BodyId) { + unsafe { + assert!(body.0 >= 0, "Invalid BodyId"); + assert!( + self.can_submit_command(), + "Internal Error: Can not submit command!", + ); + let status_handle = ffi::b3SubmitClientCommandAndWaitStatus( + self.handle, + ffi::b3InitRemoveBodyCommand(self.handle, body.0), + ); + let _status_type = ffi::b3GetStatusType(status_handle); + } + } + /// gets the BodyInfo (base name and body name) of a body + pub fn get_body_info(&mut self, body: BodyId) -> Result { + let mut body_info_c = ffi::b3BodyInfo { + m_baseName: [0; 1024], + m_bodyName: [0; 1024], + }; + unsafe { + if ffi::b3GetBodyInfo(self.handle, body.0, &mut body_info_c) != 0 { + return Ok(body_info_c.into()); + } + } + Err(Error::new("Couldn't get body info")) + } + /// returns the total number of bodies in the physics server + pub fn get_num_bodies(&mut self) -> usize { + unsafe { ffi::b3GetNumBodies(self.handle) as usize } + } +} + +impl Drop for PhysicsClient { + fn drop(&mut self) { + unsafe { ffi::b3DisconnectSharedMemory(self.handle) } + } +} + +/// Module used to enforce the existence of only a single GUI +mod gui_marker { + use std::sync::atomic::{AtomicBool, Ordering}; + + /// A marker for whether or not a GUI has been started. + /// + /// PyBullet only allows a single GUI per process. I assume that this is a limitation of the + /// underlying C API, so it will also be enforced here. + static GUI_EXISTS: AtomicBool = AtomicBool::new(false); + + /// A marker type for keeping track of the existence of a GUI. + pub struct GuiMarker { + _unused: (), + } + + impl GuiMarker { + /// Attempts to acquire the GUI marker. + pub fn acquire() -> Result { + // We can probably use a weaker ordering but this will be called so little that we + // may as well be sure about it. + match GUI_EXISTS.compare_exchange(false, true, Ordering::SeqCst, Ordering::SeqCst) { + Ok(false) => Ok(GuiMarker { _unused: () }), + _ => Err(crate::Error::new( + "Only one in-process GUI connection allowed", + )), + } + } + } + + impl Drop for GuiMarker { + fn drop(&mut self) { + // We are the only marker so no need to CAS + GUI_EXISTS.store(false, Ordering::SeqCst) + } + } +} diff --git a/src/error.rs b/rubullet/src/error.rs similarity index 100% rename from src/error.rs rename to rubullet/src/error.rs diff --git a/rubullet/src/lib.rs b/rubullet/src/lib.rs new file mode 100644 index 0000000..22c8009 --- /dev/null +++ b/rubullet/src/lib.rs @@ -0,0 +1,59 @@ +//! A Rust interface for Bullet physics inspired by PyBullet. +//! +//! # Example +//! ```no_run +//! use std::{thread, time::Duration}; +//! +//! use anyhow::Result; +//! use nalgebra::{Isometry3, Vector3}; +//! use rubullet::*; +//! +//! fn main() -> Result<()> { +//! let mut physics_client = PhysicsClient::connect(Mode::Gui)?; +//! +//! physics_client.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data")?; +//! physics_client.set_gravity(Vector3::new(0.0, 0.0, -10.0))?; +//! +//! let _plane_id = physics_client.load_urdf("plane.urdf", None)?; +//! +//! let cube_start_position = Isometry3::translation(0.0, 0.0, 1.0); +//! let box_id = physics_client.load_urdf( +//! "r2d2.urdf", +//! UrdfOptions { +//! base_transform: cube_start_position, +//! ..Default::default() +//! }, +//! )?; +//! +//! for _ in 0..10000 { +//! physics_client.step_simulation()?; +//! thread::sleep(Duration::from_micros(4167)); +//! } +//! +//! let cube_transform = physics_client.get_base_transform(box_id)?; +//! println!("{}", cube_transform); +//! +//! Ok(()) +//! } +//! ``` +pub use crate::{ + client::PhysicsClient, + error::Error, + mode::Mode, + types::{ + AddDebugLineOptions, AddDebugTextOptions, BodyId, BodyInfo, ChangeVisualShapeOptions, + CollisionId, ControlMode, ControlModeArray, DebugVisualizerFlag, ExternalForceFrame, + GeometricCollisionShape, GeometricVisualShape, IkSolver, Images, + InverseKinematicsNullSpaceParameters, InverseKinematicsParameters, + InverseKinematicsParametersBuilder, ItemId, Jacobian, JointInfo, JointInfoFlags, + JointState, JointType, KeyboardEvent, LinkState, LoadModelFlags, MouseButtonState, + MouseEvent, MultiBodyOptions, SdfOptions, TextureId, UrdfOptions, Velocity, VisualId, + VisualShapeData, VisualShapeOptions, + }, +}; +pub use image; +pub use nalgebra; +mod client; +mod error; +mod mode; +mod types; diff --git a/src/mode.rs b/rubullet/src/mode.rs similarity index 100% rename from src/mode.rs rename to rubullet/src/mode.rs diff --git a/rubullet/src/types.rs b/rubullet/src/types.rs new file mode 100644 index 0000000..02ad8a2 --- /dev/null +++ b/rubullet/src/types.rs @@ -0,0 +1,1326 @@ +//! Custom data types for RuBullet +use crate::Error; +use image::{ImageBuffer, Luma, RgbaImage}; +use nalgebra::{ + DVector, Isometry3, Matrix3xX, Matrix6xX, Point3, Quaternion, Translation3, UnitQuaternion, + Vector3, Vector6, U3, +}; +use rubullet_sys::{b3BodyInfo, b3JointInfo, b3JointSensorState, b3LinkState, b3VisualShapeData}; +use std::convert::TryFrom; +use std::ffi::CStr; + +use std::os::raw::c_int; +use std::path::PathBuf; + +/// The unique ID for a body within a physics server. +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub struct BodyId(pub(crate) c_int); + +/// The unique ID for a Visual Shape +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub struct VisualId(pub(crate) c_int); +impl VisualId { + /// Use it to create an object which does not have a visual appearance. It will be just be + /// the CollisionShape colored in red. + pub const NONE: VisualId = VisualId(-1); +} +/// The unique ID for a Collision Shape. +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub struct CollisionId(pub(crate) c_int); + +impl CollisionId { + /// Use it to create an object which does not collide with anything. + pub const NONE: CollisionId = CollisionId(-1); +} + +/// The unique ID for a Texture +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub struct TextureId(pub(crate) c_int); + +/// The unique ID for a User Debug Parameter Item +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub struct ItemId(pub(crate) c_int); + +/// An enum to represent different types of joints +#[derive(Debug, PartialEq, Copy, Clone)] +pub enum JointType { + Revolute = 0, + Prismatic = 1, + Spherical = 2, + Planar = 3, + Fixed = 4, + Point2Point = 5, + Gear = 6, +} + +impl TryFrom for JointType { + type Error = Error; + + fn try_from(value: i32) -> Result { + match value { + 0 => Ok(JointType::Revolute), + 1 => Ok(JointType::Prismatic), + 2 => Ok(JointType::Spherical), + 3 => Ok(JointType::Planar), + 4 => Ok(JointType::Fixed), + 5 => Ok(JointType::Point2Point), + 6 => Ok(JointType::Gear), + _ => Err(Error::new("could not convert into a valid joint type")), + } + } +} + +/// Contains basic information about a joint like its type and name. It can be obtained via +/// [`get_joint_info()`](`crate::PhysicsClient::get_joint_info()`) +/// # Example +/// ```rust +/// use rubullet::{PhysicsClient, UrdfOptions}; +/// use nalgebra::Isometry3; +/// use rubullet::Mode::Direct; +/// use anyhow::Result; +/// fn main() -> Result<()> { +/// +/// let mut client = PhysicsClient::connect(Direct)?; +/// client.set_additional_search_path( +/// "../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data", +/// )?; +/// let panda_id = client.load_urdf("franka_panda/panda.urdf", UrdfOptions::default())?; +/// let joint_info = client.get_joint_info(panda_id,4); +/// assert_eq!("panda_joint5",joint_info.joint_name); +/// Ok(()) +/// } +/// ``` +/// # See also +/// * [`JointState`](`crate::types::JointState`) - For information about the current state of the joint. +#[derive(Debug)] +pub struct JointInfo { + /// the same joint index as the input parameter + pub joint_index: usize, + /// the name of the joint, as specified in the URDF (or SDF etc) file + pub joint_name: String, + /// type of the joint, this also implies the number of position and velocity variables. + pub joint_type: JointType, + /// the first position index in the positional state variables for this body + pub q_index: i32, + /// the first velocity index in the velocity state variables for this body + pub u_index: i32, + /// reserved + #[doc(hidden)] + pub flags: JointInfoFlags, + /// the joint damping value, as specified in the URDF file + pub joint_damping: f64, + /// the joint friction value, as specified in the URDF file + pub joint_friction: f64, + /// Positional lower limit for slider and revolute (hinge) joints. + pub joint_lower_limit: f64, + /// Positional upper limit for slider and revolute joints. Values ignored in case upper limit , + /// joint pose in parent frame + pub parent_frame_pose: Isometry3, + /// parent link index. None means that the base is the parent link + pub parent_index: Option, +} +impl From for JointInfo { + fn from(b3: b3JointInfo) -> Self { + unsafe { + let b3JointInfo { + m_link_name, + m_joint_name, + m_joint_type, + m_q_index, + m_u_index, + m_joint_index, + m_flags, + m_joint_damping, + m_joint_friction, + m_joint_upper_limit, + m_joint_lower_limit, + m_joint_max_force, + m_joint_max_velocity, + m_parent_frame, + m_child_frame: _, + m_joint_axis, + m_parent_index, + m_q_size: _, + m_u_size: _, + } = b3; + let parent_index = match m_parent_index { + -1 => None, + index => Some(index as usize), + }; + + JointInfo { + link_name: CStr::from_ptr(m_link_name.as_ptr()) + .to_string_lossy() + .into_owned(), + joint_name: CStr::from_ptr(m_joint_name.as_ptr()) + .to_string_lossy() + .into_owned(), + joint_type: JointType::try_from(m_joint_type).unwrap(), + q_index: m_q_index, + u_index: m_u_index, + joint_index: m_joint_index as usize, + flags: JointInfoFlags::from_bits(m_flags).expect("Could not parse JointInfoFlags"), + joint_damping: m_joint_damping, + joint_friction: m_joint_friction, + joint_upper_limit: m_joint_upper_limit, + joint_lower_limit: m_joint_lower_limit, + joint_max_force: m_joint_max_force, + joint_max_velocity: m_joint_max_velocity, + parent_frame_pose: Isometry3::::from_parts( + Translation3::from(Vector3::from_column_slice(&m_parent_frame[0..4])), + UnitQuaternion::from_quaternion(Quaternion::from_parts( + m_parent_frame[6], + Vector3::from_column_slice(&m_parent_frame[3..6]), + )), + ), + joint_axis: m_joint_axis.into(), + parent_index, + } + } + } +} +/// Parameters for Inverse Kinematics using the Nullspace +pub struct InverseKinematicsNullSpaceParameters<'a> { + pub lower_limits: &'a [f64], + pub upper_limits: &'a [f64], + pub joint_ranges: &'a [f64], + /// Favor an IK solution closer to a given rest pose + pub rest_poses: &'a [f64], +} +/// Parameters for the [`calculate_inverse_kinematics()`](`crate::client::PhysicsClient::calculate_inverse_kinematics()`) +/// You can easily create them using the [`InverseKinematicsParametersBuilder`](`InverseKinematicsParametersBuilder`) +pub struct InverseKinematicsParameters<'a> { + /// end effector link index + pub end_effector_link_index: usize, + /// Target position of the end effector (its link coordinate, not center of mass coordinate!). + /// By default this is in Cartesian world space, unless you provide current_position joint angles. + pub target_position: Point3, + /// Target orientation in Cartesian world space. + /// If not specified, pure position IK will be used. + pub target_orientation: Option>, + /// Optional null-space IK + pub limits: Option>, + /// joint_damping allows to tune the IK solution using joint damping factors + pub joint_damping: Option<&'a [f64]>, + /// Solver which should be used for the Inverse Kinematics + pub solver: IkSolver, + /// By default RuBullet uses the joint positions of the body. + /// If provided, the target_position and target_orientation is in local space! + pub current_position: Option<&'a [f64]>, + /// Refine the IK solution until the distance between target and actual end effector position + /// is below the residual threshold, or the max_num_iterations is reached + pub max_num_iterations: Option, + /// Refine the IK solution until the distance between target and actual end effector position + /// is below this threshold, or the max_num_iterations is reached + pub residual_threshold: Option, +} +/// Specifies which Inverse Kinematics Solver to use in +/// [`calculate_inverse_kinematics()`](`crate::client::PhysicsClient::calculate_inverse_kinematics()`) +pub enum IkSolver { + /// Damped Least Squares + Dls = 0, + /// Selective Damped Least + Sdls = 1, +} + +impl From for i32 { + fn from(solver: IkSolver) -> Self { + solver as i32 + } +} + +impl<'a> Default for InverseKinematicsParameters<'a> { + fn default() -> Self { + InverseKinematicsParameters { + end_effector_link_index: 0, + target_position: Point3::new(0., 0., 0.), + target_orientation: None, + limits: None, + joint_damping: None, + solver: IkSolver::Dls, + current_position: None, + max_num_iterations: None, + residual_threshold: None, + } + } +} + +/// creates [`InverseKinematicsParameters`](`InverseKinematicsParameters`) using the Builder Pattern +/// which can then be used in [`calculate_inverse_kinematics()`](`crate::client::PhysicsClient::calculate_inverse_kinematics()`). +/// Use the [build()](`Self::build()`) method to get the parameters. +/// ```rust +/// # use rubullet::{InverseKinematicsParametersBuilder, BodyId, InverseKinematicsNullSpaceParameters, PhysicsClient, UrdfOptions}; +/// # use nalgebra::Isometry3; +/// const INITIAL_JOINT_POSITIONS: [f64; 9] = +/// [0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02]; +/// const PANDA_NUM_DOFS: usize = 7; +/// const PANDA_END_EFFECTOR_INDEX: usize = 11; +/// const LL: [f64; 9] = [-7.; 9]; // size is 9 = 7 DOF + 2 DOF for the gripper +/// const UL: [f64; 9] = [7.; 9]; // size is 9 = 7 DOF + 2 DOF for the gripper +/// const JR: [f64; 9] = [7.; 9]; // size is 9 = 7 DOF + 2 DOF for the gripper +/// const NULL_SPACE_PARAMETERS: InverseKinematicsNullSpaceParameters<'static> = +/// InverseKinematicsNullSpaceParameters { +/// lower_limits: &LL, +/// upper_limits: &UL, +/// joint_ranges: &JR, +/// rest_poses: &INITIAL_JOINT_POSITIONS, +/// }; +/// let inverse_kinematics_parameters = InverseKinematicsParametersBuilder::new( +/// PANDA_END_EFFECTOR_INDEX, +/// &Isometry3::translation(0.3,0.3,0.3), +/// ) +/// .set_max_num_iterations(5) +/// .use_null_space(NULL_SPACE_PARAMETERS) +/// .build(); +/// ``` +pub struct InverseKinematicsParametersBuilder<'a> { + params: InverseKinematicsParameters<'a>, +} + +impl<'a> InverseKinematicsParametersBuilder<'a> { + /// creates a new InverseKinematicsParametersBuilder + /// # Arguments + /// * `end_effector_link_index` - end effector link index + /// * `target_pose` - target pose of the end effector in its link coordinate (not CoM). + /// use [`ignore_orientation()`](`Self::ignore_orientation()`) if you do not want to consider the orientation + pub fn new(end_effector_link_index: usize, target_pose: &'a Isometry3) -> Self { + let target_position: Point3 = target_pose.translation.vector.into(); + let params = InverseKinematicsParameters { + end_effector_link_index, + target_position, + target_orientation: Some(target_pose.rotation), + ..Default::default() + }; + InverseKinematicsParametersBuilder { params } + } + /// Do not consider the orientation while calculating the IK + pub fn ignore_orientation(mut self) -> Self { + self.params.target_orientation = None; + self + } + /// Consider the nullspace when calculating the IK + pub fn use_null_space(mut self, limits: InverseKinematicsNullSpaceParameters<'a>) -> Self { + self.params.limits = Some(limits); + self + } + /// Allow to tune the IK solution using joint damping factors + pub fn set_joint_damping(mut self, joint_damping: &'a [f64]) -> Self { + self.params.joint_damping = Some(joint_damping); + self + } + /// Use a different IK-Solver. The default is DLS + pub fn set_ik_solver(mut self, solver: IkSolver) -> Self { + self.params.solver = solver; + self + } + /// Specify the current joint position if you do not want to use the position of the body. + /// If you use it the target pose will be in local space! + pub fn set_current_position(mut self, current_position: &'a [f64]) -> Self { + self.params.current_position = Some(current_position); + self + } + /// Sets the maximum number of iterations. The default is 20. + pub fn set_max_num_iterations(mut self, iterations: usize) -> Self { + self.params.max_num_iterations = Some(iterations); + self + } + /// Recalculate the IK until the distance between target and actual end effector is smaller than + /// the residual threshold or max_num_iterations is reached. + pub fn set_residual_threshold(mut self, residual_threshold: f64) -> Self { + self.params.residual_threshold = Some(residual_threshold); + self + } + /// creates the parameters + pub fn build(self) -> InverseKinematicsParameters<'a> { + self.params + } +} +/// Represents options for [`add_user_debug_text`](`crate::PhysicsClient::add_user_debug_text()`) +pub struct AddDebugTextOptions { + /// RGB color [Red, Green, Blue] each component in range [0..1]. Default is [1.,1.,1.] + pub text_color_rgb: [f64; 3], + /// size of the text. Default is 1. + pub text_size: f64, + /// Use 0 for permanent text, or positive time in seconds + /// (afterwards the line with be removed automatically). Default is 0. + pub life_time: f64, + /// If not specified the text will always face the camera (Default behavior). + /// By specifying a text orientation (quaternion), the orientation will be fixed in world space + /// or local space (when parent is specified). Note that a different implementation/shader is + /// used for camera facing text, with different appearance: camera facing text uses bitmap + /// fonts, text with specified orientation uses TrueType fonts. + pub text_orientation: Option>, + /// If specified the text will be drawn relative to the parents object coordinate system. + pub parent_object_id: Option, + /// When using "parent_object_id" you can also define in which link the coordinate system should be. + /// By default it is the base frame (-1) + pub parent_link_index: Option, + /// replace an existing text item (to avoid flickering of remove/add) + pub replace_item_id: Option, +} + +impl Default for AddDebugTextOptions { + fn default() -> Self { + AddDebugTextOptions { + text_color_rgb: [1.; 3], + text_size: 1., + life_time: 0., + text_orientation: None, + parent_object_id: None, + parent_link_index: None, + replace_item_id: None, + } + } +} +/// Represents options for [`add_user_debug_line`](`crate::PhysicsClient::add_user_debug_line()`) +pub struct AddDebugLineOptions { + /// RGB color [Red, Green, Blue] each component in range [0..1]. Default is [1.,1.,1.] + pub line_color_rgb: [f64; 3], + /// line width (limited by OpenGL implementation). Default is 1. + pub line_width: f64, + /// Use 0 for a permanent line, or positive time in seconds + /// (afterwards the line with be removed automatically). Default is 0. + pub life_time: f64, + /// If specified the line will be drawn relative to the parents object coordinate system. + pub parent_object_id: Option, + /// When using "parent_object_id" you can also define in which link the coordinate system should be. + /// By default it is the base frame (-1) + pub parent_link_index: Option, + /// replace an existing line (to improve performance and to avoid flickering of remove/add) + pub replace_item_id: Option, +} + +impl Default for AddDebugLineOptions { + fn default() -> Self { + AddDebugLineOptions { + line_color_rgb: [1.; 3], + line_width: 1., + life_time: 0., + parent_object_id: None, + parent_link_index: None, + replace_item_id: None, + } + } +} + +/// Specifies a jacobian with 6 rows. +/// The jacobian is split into a linear part and an angular part. +/// # Example +/// Jacobian can be multiplied with joint velocities to get a velocity in cartesian coordinates: +/// ```rust +/// # use rubullet::{Velocity, Jacobian}; +/// # use nalgebra::{Matrix6xX, DVector}; +/// let jacobian = Jacobian{jacobian:Matrix6xX::from_vec(vec![0.;12])}; +/// let velocity: Velocity = jacobian * DVector::from_vec(vec![1.;2]); +/// ``` +/// +/// # See also +/// * [`PhysicsClient::calculate_jacobian()`](`crate::PhysicsClient::calculate_jacobian()`) +#[derive(Debug, Clone)] +pub struct Jacobian { + pub jacobian: Matrix6xX, +} + +impl>> std::ops::Mul for Jacobian { + type Output = Velocity; + + fn mul(self, q_dot: T) -> Self::Output { + let vel = self.jacobian * q_dot.into(); + Velocity(vel) + } +} +impl Jacobian { + /// Linear part of the the jacobian (first 3 rows) + pub fn get_linear_jacobian(&self) -> Matrix3xX { + Matrix3xX::from(self.jacobian.fixed_rows::(0)) + } + /// Angular part of the the jacobian (last 3 rows) + pub fn get_angular_jacobian(&self) -> Matrix3xX { + Matrix3xX::from(self.jacobian.fixed_rows::(3)) + } +} +/// Frame for [`apply_external_torque()`](`crate::PhysicsClient::apply_external_torque()`) and +/// [`apply_external_force()`](`crate::PhysicsClient::apply_external_force()`) +pub enum ExternalForceFrame { + /// Local Link Coordinates + LinkFrame = 1, + /// Cartesian World Coordinates + WorldFrame = 2, +} +/// Represents a key press Event +#[derive(Debug, Copy, Clone, Default)] +pub struct KeyboardEvent { + /// specifies which key the event is about. + pub key: char, + pub(crate) key_state: i32, +} + +impl KeyboardEvent { + /// is true when the key goes from an "up" to a "down" state. + pub fn was_triggered(&self) -> bool { + self.key_state & 2 == 2 + } + /// is true when the key is currently pressed. + pub fn is_down(&self) -> bool { + self.key_state & 1 == 1 + } + /// is true when the key goes from a "down" to an "up" state. + pub fn is_released(&self) -> bool { + self.key_state & 4 == 4 + } +} +/// Mouse Events can either be a "Move" or a "Button" event. A "Move" event is when the mouse is moved +/// in the OpenGL window and a "Button" even is when a mouse button is clicked. +#[derive(Debug, Copy, Clone)] +pub enum MouseEvent { + /// Contains the mouse position + Move { + /// x-coordinate of the mouse pointer + mouse_pos_x: f32, + /// y-coordinate of the mouse pointer + mouse_pos_y: f32, + }, + /// Specifies Mouse Position and a Button event + Button { + /// x-coordinate of the mouse pointer + mouse_pos_x: f32, + /// y-coordinate of the mouse pointer + mouse_pos_y: f32, + /// button index for left/middle/right mouse button + button_index: i32, + /// state of the mouse button + button_state: MouseButtonState, + }, +} + +/// Represents the different possible states of a mouse button +#[derive(Debug, Copy, Clone)] +pub struct MouseButtonState { + pub(crate) flag: i32, +} +impl MouseButtonState { + /// is true when the button goes from an "unpressed" to a "pressed" state. + pub fn was_triggered(&self) -> bool { + self.flag & 2 == 2 + } + /// is true when the button is in a "pressed" state. + pub fn is_pressed(&self) -> bool { + self.flag & 1 == 1 + } + /// is true when the button goes from a "pressed" to an "unpressed" state. + pub fn is_released(&self) -> bool { + self.flag & 4 == 4 + } +} +/// Represents the current state of a joint. It can be retrieved via [`get_joint_state()`](`crate::PhysicsClient::get_joint_state()`) +/// # Note +/// joint_force_torque will be [0.;6] if the sensor is not enabled via +/// [`enable_joint_torque_sensor()`](`crate::PhysicsClient::enable_joint_torque_sensor()`) +/// # See also +/// * [`JointInfo`](`JointInfo`) - For basic information about a joint +#[derive(Debug, Default, Copy, Clone)] +pub struct JointState { + /// The position value of this joint. + pub joint_position: f64, + /// The velocity value of this joint. + pub joint_velocity: f64, + /// These are the joint reaction forces, if a torque sensor is enabled for this joint it is [Fx, Fy, Fz, Mx, My, Mz]. + /// Without torque sensor, it is \[0,0,0,0,0,0\]. + /// This is is NOT the motor torque/force, but the spatial reaction force vector at joint. + pub joint_force_torque: [f64; 6], + /// This is the motor torque applied during the last [`step_simulation()`](`crate::PhysicsClient::step_simulation()`). + /// Note that this only applies in velocity and position control. + /// If you use torque control then the applied joint motor torque is exactly what you provide, + /// so there is no need to report it separately. + pub joint_motor_torque: f64, +} +impl From for JointState { + fn from(b3: b3JointSensorState) -> Self { + let b3JointSensorState { + m_joint_position, + m_joint_velocity, + m_joint_force_torque, + m_joint_motor_torque, + } = b3; + JointState { + joint_position: m_joint_position, + joint_velocity: m_joint_velocity, + joint_force_torque: m_joint_force_torque, + joint_motor_torque: m_joint_motor_torque, + } + } +} + +/// Options for loading a URDF into the physics server. +pub struct UrdfOptions { + /// Creates the base of the object with the given transform. + pub base_transform: Isometry3, + + /// Forces the base of the loaded object to be static. + pub use_fixed_base: bool, + /// Experimental. By default, the joints in the URDF file are created using the reduced + /// coordinate method: the joints are simulated using the + /// Featherstone Articulated Body Algorithm (ABA, btMultiBody in Bullet 2.x). + /// The use_maximal_coordinates option will create a 6 degree of freedom rigid body for each link, + /// and constraints between those rigid bodies are used to model joints. + pub use_maximal_coordinates: Option, + + /// Flags for loading the model. + pub flags: LoadModelFlags, + /// Applies a scale factor to the model. + pub global_scaling: f64, +} + +impl Default for UrdfOptions { + fn default() -> UrdfOptions { + UrdfOptions { + base_transform: Isometry3::identity(), + use_fixed_base: false, + use_maximal_coordinates: None, + global_scaling: -1.0, + flags: LoadModelFlags::NONE, + } + } +} +/// Options for loading models from an SDF file into the physics server. +pub struct SdfOptions { + /// Experimental. By default, the joints in the URDF file are created using the reduced + /// coordinate method: the joints are simulated using the + /// Featherstone Articulated Body Algorithm (ABA, btMultiBody in Bullet 2.x). + /// The use_maximal_coordinates option will create a 6 degree of freedom rigid body for each link, + /// and constraints between those rigid bodies are used to model joints. + pub use_maximal_coordinates: bool, + /// Applies a scale factor to the model. + pub global_scaling: f64, +} + +impl Default for SdfOptions { + fn default() -> Self { + SdfOptions { + use_maximal_coordinates: false, + global_scaling: 1.0, + } + } +} +/// The Control Mode specifies how the robot should move (Position Control, Velocity Control, Torque Control) +/// Each Control Mode has its own set of Parameters. The Position mode for example takes a desired joint +/// position as input. It can be used in [`set_joint_motor_control()`](`crate::client::PhysicsClient::set_joint_motor_control()`) +/// +/// | Mode | Implementation | Component | Constraint error to be minimized | +/// |-------------------------|----------------|----------------------------------|-----------------------------------------------------------------------------------------------------------| +/// | Position,PositionWithPd | constraint | velocity and position constraint | error = position_gain*(desired_position-actual_position)+velocity_gain*(desired_velocity-actual_velocity) | +/// | Velocity | constraint | pure velocity constraint | error = desired_velocity - actual_velocity | +/// | Torque | External Force | | | +/// | Pd | ??? | ??? | ??? | +pub enum ControlMode { + /// Position Control with the desired joint position. + Position(f64), + /// Same as Position, but you can set your own gains + PositionWithPd { + /// desired target position + target_position: f64, + /// desired target velocity + target_velocity: f64, + /// position gain + position_gain: f64, + /// velocity gain + velocity_gain: f64, + /// limits the velocity of a joint + maximum_velocity: Option, + }, + /// Velocity control with the desired joint velocity + Velocity(f64), + /// Torque control with the desired joint torque. + Torque(f64), + /// PD Control + Pd { + /// desired target position + target_position: f64, + /// desired target velocity + target_velocity: f64, + /// position gain + position_gain: f64, + /// velocity gain + velocity_gain: f64, + /// limits the velocity of a joint + maximum_velocity: Option, + }, +} + +impl ControlMode { + pub(crate) fn get_int(&self) -> i32 { + match self { + ControlMode::Position(_) => 2, + ControlMode::Velocity(_) => 0, + ControlMode::Torque(_) => 1, + ControlMode::Pd { .. } => 3, + ControlMode::PositionWithPd { .. } => 2, + } + } +} +/// Can be used in [`set_joint_motor_control_array()`](`crate::client::PhysicsClient::set_joint_motor_control_array()`). +/// It is basically the same as [`ControlMode`](`ControlMode`) but with arrays. See [`ControlMode`](`ControlMode`) for details. +pub enum ControlModeArray<'a> { + /// Position Control with the desired joint positions. + Positions(&'a [f64]), + /// Same as Positions, but you can set your own gains + PositionsWithPd { + /// desired target positions + target_positions: &'a [f64], + /// desired target velocities + target_velocities: &'a [f64], + /// position gains + position_gains: &'a [f64], + /// velocity gains + velocity_gains: &'a [f64], + }, + /// Velocity control with the desired joint velocities + Velocities(&'a [f64]), + /// Torque control with the desired joint torques. + Torques(&'a [f64]), + /// PD Control + Pd { + /// desired target positions + target_positions: &'a [f64], + /// desired target velocities + target_velocities: &'a [f64], + /// position gains + position_gains: &'a [f64], + /// velocity gains + velocity_gains: &'a [f64], + }, +} + +impl ControlModeArray<'_> { + pub(crate) fn get_int(&self) -> i32 { + match self { + ControlModeArray::Positions(_) => 2, + ControlModeArray::Velocities(_) => 0, + ControlModeArray::Torques(_) => 1, + ControlModeArray::Pd { .. } => 3, + ControlModeArray::PositionsWithPd { .. } => 2, + } + } +} +/// Flags for [`configure_debug_visualizer()`](`crate::PhysicsClient::configure_debug_visualizer`) +pub enum DebugVisualizerFlag { + CovEnableGui = 1, + CovEnableShadows, + CovEnableWireframe, + CovEnableVrTeleporting, + CovEnableVrPicking, + CovEnableVrRenderControllers, + CovEnableRendering, + CovEnableSyncRenderingInternal, + CovEnableKeyboardShortcuts, + CovEnableMousePicking, + CovEnableYAxisUp, + CovEnableTinyRenderer, + CovEnableRgbBufferPreview, + CovEnableDepthBufferPreview, + CovEnableSegmentationMarkPreview, + CovEnablePlanarReflection, + CovEnableSingleStepRendering, +} + +/// Describes the State of a Link +/// # Kind of Frames +/// * `world_frame` - center of mass +/// * `local_intertial_frame` - offset to the CoM expressed in the URDF link frame +/// * `world_link_frame` - URDF link frame +/// ### Relationships between Frames +/// urdfLinkFrame = comLinkFrame * localInertialFrame.inverse() +/// ```rust +/// use rubullet::{PhysicsClient, UrdfOptions}; +/// use nalgebra::Isometry3; +/// use rubullet::Mode::Direct; +/// use anyhow::Result; +/// fn main() -> Result<()> { +/// let mut client = PhysicsClient::connect(Direct)?; +/// client.set_additional_search_path( +/// "../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data", +/// )?; +/// let panda_id = client.load_urdf("franka_panda/panda.urdf", UrdfOptions::default())?; +/// let link_state = client.get_link_state(panda_id, 11, true, true)?; +/// // urdfLinkFrame = comLinkFrame * localInertialFrame.inverse() +/// let urdf_frame = link_state.world_pose * link_state.local_inertial_pose.inverse(); +/// // print both frames to see that they are about the same +/// println!("{}", link_state.world_link_frame_pose); +/// println!("{}", urdf_frame); +/// // as they are both almost the same calculating the difference: +/// // urdfLinkFrame.inverse() * world_link_frame_pose +/// // should return something very close the identity matrix I. +/// let identity = urdf_frame.inverse() * link_state.world_link_frame_pose; +/// assert!(identity.translation.vector.norm() < 1e-7); +/// assert!(identity.rotation.angle() < 1e-7); +/// Ok(()) +/// } +/// ``` +/// +/// # See also +/// * [`get_link_state()`](`crate::client::PhysicsClient::get_link_state()`) +/// * [`get_link_states()`](`crate::client::PhysicsClient::get_link_states()`) +#[derive(Debug)] +pub struct LinkState { + /// Cartesian pose of the center of mass + pub world_pose: Isometry3, + /// local offset of the inertial frame (center of mass) express in the URDF link frame + pub local_inertial_pose: Isometry3, + /// world pose of the URDF link frame + pub world_link_frame_pose: Isometry3, + /// Cartesian world linear velocity. + pub world_velocity: Option, +} +impl LinkState { + /// conveniently returns the linear world velocity or an error if the velocity was not calculated + /// for the LinkState. Be sure to set `compute_link_velocity` to true in + /// [`get_link_state()`](`crate::client::PhysicsClient::get_link_state()`) + pub fn get_linear_world_velocity(&self) -> Result, Error> { + match &self.world_velocity { + None => {Err(Error::new("LinkState contains no velocity. You have to set compute_link_velocity to true in get_link_state() to get the velocity"))} + Some(velocity) => {Ok(velocity.get_linear_velocity())} + } + } + /// conveniently returns the angular world velocity or an error if the velocity was not calculated + /// for the LinkState. Be sure to set `compute_link_velocity` to true in + /// [`get_link_state()`](`crate::client::PhysicsClient::get_link_state()`) + pub fn get_angular_world_velocity(&self) -> Result, Error> { + match &self.world_velocity { + None => {Err(Error::new("LinkState contains no velocity. You have to set compute_link_velocity to true in get_link_state() to get the velocity"))} + Some(velocity) => {Ok(velocity.get_angular_velocity())} + } + } + /// conveniently returns the world velocity or an error if the velocity was not calculated + /// for the LinkState. Be sure to set `compute_link_velocity` to true in + /// [`get_link_state()`](`crate::client::PhysicsClient::get_link_state()`) + pub fn get_world_velocity(&self) -> Result<&Velocity, Error> { + match &self.world_velocity { + None => {Err(Error::new("LinkState contains no velocity. You have to set compute_link_velocity to true in get_link_state() to get the velocity"))} + Some(velocity) => {Ok(velocity)} + } + } + /// conveniently returns the world velocity vector (x,y,z,wx,w,wz) or an error if the velocity was not calculated + /// for the LinkState. Be sure to set `compute_link_velocity` to true in + /// [`get_link_state()`](`crate::client::PhysicsClient::get_link_state()`) + pub fn get_world_velocity_vector(&self) -> Result, Error> { + match &self.world_velocity { + None => {Err(Error::new("LinkState contains no velocity. You have to set compute_link_velocity to true in get_link_state() to get the velocity"))} + Some(velocity) => {Ok(velocity.to_vector())} + } + } +} +impl From<(b3LinkState, bool)> for LinkState { + fn from(b3: (b3LinkState, bool)) -> Self { + let ( + b3LinkState { + m_world_position, + m_world_orientation, + m_local_inertial_position, + m_local_inertial_orientation, + m_world_link_frame_position, + m_world_link_frame_orientation, + m_world_linear_velocity, + m_world_angular_velocity, + m_world_aabb_min: _, + m_world_aabb_max: _, + }, + velocity_valid, + ) = b3; + let mut state = LinkState { + world_pose: position_orientation_to_isometry(m_world_position, m_world_orientation), + local_inertial_pose: position_orientation_to_isometry( + m_local_inertial_position, + m_local_inertial_orientation, + ), + world_link_frame_pose: position_orientation_to_isometry( + m_world_link_frame_position, + m_world_link_frame_orientation, + ), + world_velocity: None, + }; + if velocity_valid { + let velocity: [f64; 6] = [ + m_world_linear_velocity[0], + m_world_linear_velocity[1], + m_world_linear_velocity[2], + m_world_angular_velocity[0], + m_world_angular_velocity[1], + m_world_angular_velocity[2], + ]; + state.world_velocity = Some(velocity.into()); + } + state + } +} + +fn position_orientation_to_isometry(position: [f64; 3], orientation: [f64; 4]) -> Isometry3 { + Isometry3::::from_parts( + Translation3::from(Vector3::from_column_slice(&position)), + UnitQuaternion::from_quaternion(Quaternion::from_parts( + orientation[3], + Vector3::from_column_slice(&orientation[0..3]), + )), + ) +} +fn combined_position_orientation_array_to_isometry(combined: [f64; 7]) -> Isometry3 { + let position = [combined[0], combined[1], combined[2]]; + let orientation = [combined[3], combined[4], combined[5], combined[6]]; + position_orientation_to_isometry(position, orientation) +} + +/// VisualShape options are for the [create_visual_shape](`crate::PhysicsClient::create_visual_shape`) +/// function to specify additional options like the color. +pub struct VisualShapeOptions { + /// offset of the shape with respect to the link frame + pub frame_offset: Isometry3, + /// color components for red, green, blue and alpha, each in range \[0,1\] + pub rgba_colors: [f64; 4], + /// specular reflection color, red, green, blue components in range \[0,1\] + pub specular_colors: [f64; 3], + /// Additional flags. Currently not used + #[doc(hidden)] + pub flags: Option, +} +impl Default for VisualShapeOptions { + fn default() -> Self { + VisualShapeOptions { + frame_offset: Isometry3::translation(0., 0., 0.), + rgba_colors: [1.; 4], + specular_colors: [1.; 3], + flags: None, + } + } +} +/// Collision shape which can be put +/// the [create_collision_shape](`crate::PhysicsClient::create_collision_shape`) method +pub enum GeometricCollisionShape { + /// A Sphere determined by the radius in meter + Sphere { + /// radius in meter + radius: f64, + }, + /// A Cuboid + Box { + /// \[x,y,z\] lengths starting from the middle of the box. + /// For example Vector3::new(0.5,0.5,0.5) would be a unit cube. + half_extents: Vector3, + }, + /// Like a cylinder but with a half sphere on each end. The total length of a capsule is + /// length + 2 * radius. + Capsule { + /// radius of the cylindric part of the capsule in meter. + radius: f64, + /// height of the cylindric part in meter. The half spheres are put on top on that + height: f64, + }, + /// A Cylinder + Cylinder { + /// radius in meter + radius: f64, + /// height in meter + height: f64, + }, + /// A Plane. + Plane { + /// normal of the plane. + plane_normal: Vector3, + }, + /// Load a .obj (Wavefront) file. Will create convex hulls for each object. + MeshFile { + /// Path to the .obj file. + filename: PathBuf, + /// Scaling of the Mesh.Use None if you do not want to apply any scaling. + mesh_scaling: Option>, + /// Set to 1 if you want to activate have the GEOM_FORCE_CONCAVE_TRIMESH Flag. + /// this will create a concave static triangle mesh. This should not be used with + /// dynamic / moving objects, only for static (mass = 0) terrain. + flags: Option, + }, + /// Create your own mesh. + Mesh { + /// list of \[x,y,z\] coordinates. + vertices: Vec<[f64; 3]>, + /// triangle indices, should be a multiple of 3 + indices: Option>, + /// Scaling of the Mesh. Use [1.;3] for normal scaling. + mesh_scaling: Option>, + }, + /// Loads a Heightfield from a file + HeightfieldFile { + /// Path to the .obj file. + filename: PathBuf, + /// Scaling of the Mesh.Use None if you do not want to apply any scaling. + mesh_scaling: Option>, + /// Texture scaling. Use 1. for original scaling. + texture_scaling: f64, + }, + /// Create your own Heightfield. See heightfield.rs for an example. + Heightfield { + /// Scaling of the Mesh. Use [1.;3] for normal scaling. + mesh_scaling: Option>, + /// Texture scaling. Use 1. for normal scaling. + texture_scaling: f64, + /// Heightfield data. Should be of size num_rows * num_columns + data: Vec, + /// number of rows in data + num_rows: usize, + /// number of columns in data + num_columns: usize, + /// replacing an existing heightfield (updating its heights) + /// (much faster than removing and re-creating a heightfield) + replace_heightfield: Option, + }, +} +/// Visual shapes to put into the [create_visual_shape](`crate::PhysicsClient::create_visual_shape`) +/// method together with [VisualShapeOptions](`VisualShapeOptions`) +pub enum GeometricVisualShape { + /// A Sphere determined by the radius in meter + Sphere { + /// radius in meter + radius: f64, + }, + /// A Cuboid + Box { + /// \[x,y,z\] lengths starting from the middle of the box. + /// For example Vector3::new(0.5,0.5,0.5) would be a unit cube. + half_extents: Vector3, + }, + /// Like a cylinder but with a half sphere on each end. The total length of a capsule is + /// length + 2 * radius. + Capsule { + /// radius of the cylindric part of the capsule in meter. + radius: f64, + /// length of the cylindric part in meter. The half spheres are put on top on that + length: f64, + }, + /// A Cylinder + Cylinder { + /// radius in meter + radius: f64, + /// length in meter + length: f64, + }, + /// A flat Plane. Note that you cannot use a Plane VisualShape in combination with a non Plane + /// CollisionShape. Also it seems like the visual plane is determined by the collision plane and + /// thus cannot be adapted through the normal of the visual. + Plane { + /// Normal of the plane. Seems to have no effect! + plane_normal: Vector3, + }, + /// Loads a .obj (Wavefront) file. Will create convex hulls for each object. + MeshFile { + /// Path to the .obj file. + filename: PathBuf, + /// Scaling of the Mesh.Use None if you do not want to apply any scaling. + mesh_scaling: Option>, + }, + /// Create your own mesh. + Mesh { + /// Scaling of the Mesh. Use [1.;3] for normal scaling. + mesh_scaling: Option>, + /// list of \[x,y,z\] coordinates. + vertices: Vec<[f64; 3]>, + /// triangle indices, should be a multiple of 3 + indices: Vec, + /// uv texture coordinates for vertices. + /// Use [change_visual_shape](`crate::PhysicsClient::change_visual_shape`) + /// to choose the texture image. The number of uvs should be equal to number of vertices + uvs: Option>, + /// vertex normals, number should be equal to number of vertices. + normals: Option>, + }, +} +/// Specifies all options for [create_multi_body](`crate::PhysicsClient::create_multi_body`). +/// Most of the the time you are probably fine using `MultiBodyOptions::default()` or just setting +/// the base_pose and/or mass +pub struct MultiBodyOptions { + /// mass of the base, in kg (if using SI units) + pub base_mass: f64, + /// Cartesian world pose of the base + pub base_pose: Isometry3, + /// Local pose of inertial frame + pub base_inertial_frame_pose: Isometry3, + /// List of the mass values, one for each link. + pub link_masses: Vec, + /// List of the collision shape unique id, one for each link. + /// Use [`CollisionId::NONE`](`crate::types::CollisionId::NONE`) if you do not want to have a collision shape. + pub link_collision_shapes: Vec, + /// List of the visual shape unique id, one for each link. + /// Use [`VisualId::NONE`](`crate::types::VisualId::NONE`) if you do not want to set a visual shape. + pub link_visual_shapes: Vec, + /// list of local link poses, with respect to parent + pub link_poses: Vec>, + /// list of local inertial frame poses, in the link frame + pub link_inertial_frame_poses: Vec>, + /// Link index of the parent link or 0 for the base. + pub link_parent_indices: Vec, + /// list of joint types, one for each link. + pub link_joint_types: Vec, + /// List of joint axis in local frame + pub link_joint_axis: Vec>, + /// experimental, best to leave it false. + pub use_maximal_coordinates: bool, + /// similar to the flags passed in load_urdf, for example URDF_USE_SELF_COLLISION. + /// See [`LoadModelFlags`](`LoadModelFlags`) for flags explanation. + pub flags: Option, + /// list of base positions, for fast batch creation of many multibodies. + /// See create_multi_body_batch.rs example. + pub batch_positions: Option>>, +} +impl Default for MultiBodyOptions { + fn default() -> Self { + MultiBodyOptions { + base_pose: Isometry3::translation(0., 0., 0.), + base_inertial_frame_pose: Isometry3::translation(0., 0., 0.), + base_mass: 0.0, + link_masses: Vec::new(), + link_collision_shapes: Vec::new(), + link_visual_shapes: Vec::new(), + link_poses: Vec::new(), + link_inertial_frame_poses: Vec::new(), + link_parent_indices: Vec::new(), + link_joint_types: Vec::new(), + link_joint_axis: Vec::new(), + use_maximal_coordinates: false, + flags: None, + + batch_positions: None, + } + } +} + +/// This struct keeps the information to change a visual shape with the +/// [change_visual_shape](`crate::PhysicsClient::change_visual_shape`) method. +pub struct ChangeVisualShapeOptions { + /// Experimental for internal use, recommended ignore shapeIndex or leave it -1. + /// Intention is to let you pick a specific shape index to modify, since URDF (and SDF etc) + pub shape: VisualId, + /// texture unique id, as returned by [load_texture](`crate::PhysicsClient::load_texture`) method + pub texture_id: Option, + /// color components for RED, GREEN, BLUE and ALPHA, each in range [0..1]. + /// Alpha has to be 0 (invisible) or 1 (visible) at the moment. + /// Note that TinyRenderer doesn't support transparency, but the GUI/EGL OpenGL3 renderer does. + pub rgba_color: Option<[f64; 4]>, + /// specular color components, RED, GREEN and BLUE, can be from 0 to large number (>100). + pub specular_color: Option<[f64; 3]>, + /// Not yet used anywhere. But it is in the code. + #[doc(hidden)] + pub flags: Option, +} +impl Default for ChangeVisualShapeOptions { + fn default() -> Self { + ChangeVisualShapeOptions { + shape: VisualId(-1), + texture_id: None, + rgba_color: None, + specular_color: None, + flags: None, + } + } +} +/// Contains the body name and base name of a Body. BodyInfo is returned by +/// [get_body_info](`crate::PhysicsClient::get_body_info`) +#[derive(Debug)] +pub struct BodyInfo { + /// base name (first link) as extracted from the URDF etc. + pub base_name: String, + /// body name (robot name) as extracted from the URDF etc. + pub body_name: String, +} + +impl From for BodyInfo { + fn from(info: b3BodyInfo) -> Self { + unsafe { + BodyInfo { + base_name: CStr::from_ptr(info.m_baseName.as_ptr()) + .to_string_lossy() + .into_owned(), + body_name: CStr::from_ptr(info.m_bodyName.as_ptr()) + .to_string_lossy() + .into_owned(), + } + } + } +} +/// Contains information about the visual shape of a body. It is returned by +/// [get_visual_shape_data](`crate::PhysicsClient::get_visual_shape_data`) +#[derive(Debug)] +pub struct VisualShapeData { + /// same id as in the input of [get_visual_shape_data](`crate::PhysicsClient::get_visual_shape_data`) + pub body_id: BodyId, + /// link index or None for the base + pub link_index: Option, + /// visual geometry type (TBD) + pub visual_geometry_type: i32, + /// dimensions (size, local scale) of the geometry + pub dimensions: [f64; 3], + /// path to the triangle mesh, if any. Typically relative to the URDF, SDF or + /// MJCF file location, but could be absolute. + pub mesh_asset_file_name: String, + /// of local visual frame relative to link/joint frame + pub local_visual_frame_pose: Isometry3, + /// URDF color (if any specified) in red/green/blue/alpha + pub rgba_color: [f64; 4], + /// Id of the texture. Is only some when request_texture_id was set to true + pub texture_id: Option, +} + +impl From for VisualShapeData { + fn from(b3: b3VisualShapeData) -> Self { + unsafe { + let link_index = match b3.m_linkIndex { + -1 => None, + index => Some(index as usize), + }; + VisualShapeData { + body_id: BodyId(b3.m_objectUniqueId), + link_index, + visual_geometry_type: b3.m_visualGeometryType, + dimensions: b3.m_dimensions, + mesh_asset_file_name: CStr::from_ptr(b3.m_meshAssetFileName.as_ptr()) + .to_string_lossy() + .into_owned(), + local_visual_frame_pose: combined_position_orientation_array_to_isometry( + b3.m_localVisualFrame, + ), + rgba_color: b3.m_rgbaColor, + texture_id: None, + } + } + } +} +/// Stores the images from [`get_camera_image()`](`crate::PhysicsClient::get_camera_image()`) +pub struct Images { + /// RGB image with additional alpha channel + pub rgba: RgbaImage, + /// Depth image. Every pixel represents a distance in meters + pub depth: ImageBuffer, Vec>, + /// Segmentation image. Every pixel represents a unique [`BodyId`](`crate::types::BodyId`) + pub segmentation: ImageBuffer, Vec>, +} + +/// Contains the cartesian velocity stored as Vector with 6 elements (x,y,z,wx,wy,wz). +/// # Example +/// ```rust +/// use rubullet::Velocity; +/// use nalgebra::Vector6; +/// let vel: Velocity = [0.; 6].into(); // creation from array +/// let vel: Velocity = Vector6::zeros().into(); // creation from vector +/// ``` +#[derive(Debug)] +pub struct Velocity(Vector6); + +impl Velocity { + /// returns the linear velocity (x,y,z) + pub fn get_linear_velocity(&self) -> Vector3 { + self.0.fixed_rows::(0).into() + } + /// returns the angular velocity (wx,wy,wz) + pub fn get_angular_velocity(&self) -> Vector3 { + self.0.fixed_rows::(3).into() + } + /// converts the velocity to a Vector6 (x,y,z,wx,wy,wz) + pub fn to_vector(&self) -> Vector6 { + self.0 + } +} +impl From<[f64; 6]> for Velocity { + fn from(input: [f64; 6]) -> Self { + Velocity(input.into()) + } +} +impl From> for Velocity { + fn from(input: Vector6) -> Self { + Velocity(input) + } +} +bitflags::bitflags! { + /// Use flag for loading the model. Flags can be combined with the `|`-operator. + /// Example: + /// ```rust + ///# use rubullet::LoadModelFlags; + /// let flags = LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES | LoadModelFlags::URDF_PRINT_URDF_INFO; + /// assert!(flags.contains(LoadModelFlags::URDF_PRINT_URDF_INFO)); + /// ``` + pub struct LoadModelFlags : i32 { + /// use no flags (Default) + const NONE = 0; + /// Use the inertia tensor provided in the URDF. + /// + /// By default, Bullet will recompute the inertial tensor based on the mass and volume of the + /// collision shape. Use this is you can provide a more accurate inertia tensor. + const URDF_USE_INERTIA_FROM_FILE = 2; + /// Enables self-collision. + const URDF_USE_SELF_COLLISION = 8; + const URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16; + /// will discard self-collisions between a child link and any of its ancestors + /// (parents, parents of parents, up to the base). + /// Needs to be used together with [`URDF_USE_SELF_COLLISION`](`Self::URDF_USE_SELF_COLLISION`). + const URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32; + const URDF_RESERVED = 64; + /// will use a smooth implicit cylinder. By default, Bullet will tesselate the cylinder + /// into a convex hull. + const URDF_USE_IMPLICIT_CYLINDER = 128; + const URDF_GLOBAL_VELOCITIES_MB = 256; + const MJCF_COLORS_FROM_FILE = 512; + /// Caches as reuses graphics shapes. This will decrease loading times for similar objects + const URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024; + /// Allow the disabling of simulation after a body hasn't moved for a while. + /// + /// Interaction with active bodies will re-enable simulation. + const URDF_ENABLE_SLEEPING = 2048; + /// will create triangle meshes for convex shapes. This will improve visualization and also + /// allow usage of the separating axis test (SAT) instead of GJK/EPA. + /// Requires to enable_SAT using set_physics_engine_parameter. TODO + const URDF_INITIALIZE_SAT_FEATURES = 4096; + /// will enable collision between child and parent, it is disabled by default. + /// Needs to be used together with [`URDF_USE_SELF_COLLISION`](`Self::URDF_USE_SELF_COLLISION`) flag. + const URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192; + const URDF_PARSE_SENSORS = 16384; + /// will use the RGB color from the Wavefront OBJ file, instead of from the URDF file. + const URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768; + const URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536; + /// Try to maintain the link order from the URDF file. + const URDF_MAINTAIN_LINK_ORDER = 131072; + const URDF_ENABLE_WAKEUP = 262144; + /// this will remove fixed links from the URDF file and merge the resulting links. + /// This is good for performance, since various algorithms + /// (articulated body algorithm, forward kinematics etc) have linear complexity + /// in the number of joints, including fixed joints. + const URDF_MERGE_FIXED_LINKS = 1 << 19; + const URDF_IGNORE_VISUAL_SHAPES = 1 << 20; + const URDF_IGNORE_COLLISION_SHAPES = 1 << 21; + const URDF_PRINT_URDF_INFO = 1 << 22; + const URDF_GOOGLEY_UNDEFINED_COLORS = 1 << 23; + } +} + +impl Default for LoadModelFlags { + fn default() -> Self { + LoadModelFlags::NONE + } +} +bitflags::bitflags! { + #[doc(hidden)] + pub struct JointInfoFlags : i32 { + const NONE = 0; + const JOINT_CHANGE_MAX_FORCE = 1; + const JOINT_CHANGE_CHILD_FRAME_POSITION = 2; + const JOINT_CHANGE_CHILD_FRAME_ORIENTATION = 4; + } +} + +impl Default for JointInfoFlags { + fn default() -> Self { + JointInfoFlags::NONE + } +} diff --git a/rubullet/tests/tests.rs b/rubullet/tests/tests.rs new file mode 100644 index 0000000..0657028 --- /dev/null +++ b/rubullet/tests/tests.rs @@ -0,0 +1,865 @@ +use nalgebra::{DVector, Isometry3, Matrix3xX, Translation3, UnitQuaternion, Vector3}; + +use anyhow::Result; +use rubullet::ControlModeArray::Torques; +use rubullet::Mode::Direct; +use rubullet::{ + BodyId, ControlMode, ControlModeArray, DebugVisualizerFlag, Error, + InverseKinematicsParametersBuilder, JointInfoFlags, JointType, LoadModelFlags, PhysicsClient, + UrdfOptions, +}; +use rubullet::{JointInfo, JointState}; +use std::f64::consts::PI; +use std::time::Duration; + +fn slice_compare(a: &[f64], b: &[f64], thresh: f64) { + assert_eq!(a.len(), b.len()); + for i in 0..a.len() { + float_compare(a[i], b[i], thresh); + } +} + +fn float_compare(a: f64, b: f64, thresh: f64) { + println!("{} {}", a, b); + assert!((a - b).abs() < thresh); +} +fn slice_compare_f32(a: &[f32], b: &[f32], thresh: f32) { + assert_eq!(a.len(), b.len()); + for i in 0..a.len() { + float32_compare(a[i], b[i], thresh); + } +} + +fn float32_compare(a: f32, b: f32, thresh: f32) { + println!("{} {}", a, b); + assert!((a - b).abs() < thresh); +} + +#[test] +fn test_connect() { + let _physics_client = PhysicsClient::connect(Direct).unwrap(); +} + +#[test] +fn test_load_urdf() { + let mut physics_client = PhysicsClient::connect(Direct).unwrap(); + physics_client + .set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + let _plane_id = physics_client.load_urdf("plane.urdf", None).unwrap(); +} +#[test] +fn test_add_and_remove_bodies() { + let mut physics_client = PhysicsClient::connect(Direct).unwrap(); + physics_client + .set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + assert_eq!(physics_client.get_num_bodies(), 0); + let _plane_id = physics_client.load_urdf("plane.urdf", None).unwrap(); + assert_eq!(physics_client.get_num_bodies(), 1); + let r2d2 = physics_client.load_urdf("r2d2.urdf", None).unwrap(); + assert_eq!(physics_client.get_num_bodies(), 2); + physics_client.remove_body(r2d2); + assert_eq!(physics_client.get_num_bodies(), 1); + physics_client.reset_simulation(); + assert_eq!(physics_client.get_num_bodies(), 0); +} +#[test] +fn test_get_and_reset_base_transformation() { + let mut physics_client = PhysicsClient::connect(Direct).unwrap(); + physics_client + .set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + let r2d2 = physics_client.load_urdf("r2d2.urdf", None).unwrap(); + let desired_transform = Isometry3::from_parts( + Translation3::new(0.2, 0.3, 0.4), + UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3), + ); + physics_client.reset_base_transform(r2d2, &desired_transform); + let actual_transform = physics_client.get_base_transform(r2d2).unwrap(); + slice_compare( + desired_transform.translation.vector.as_slice(), + actual_transform.translation.vector.as_slice(), + 1e-5, + ); + slice_compare( + desired_transform.rotation.coords.as_slice(), + actual_transform.rotation.coords.as_slice(), + 1e-5, + ); + + let desired_transform = Isometry3::from_parts( + Translation3::new(3.7, -0.23, 10.4), + UnitQuaternion::from_euler_angles(1.1, -0.2, 2.3), + ); + physics_client.reset_base_transform(r2d2, &desired_transform); + let actual_transform = physics_client.get_base_transform(r2d2).unwrap(); + slice_compare( + desired_transform.translation.vector.as_slice(), + actual_transform.translation.vector.as_slice(), + 1e-5, + ); + slice_compare( + desired_transform.rotation.coords.as_slice(), + actual_transform.rotation.coords.as_slice(), + 1e-5, + ); +} +#[test] +fn test_get_body_info() { + let mut physics_client = PhysicsClient::connect(Direct).unwrap(); + physics_client + .set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + let r2d2 = physics_client.load_urdf("r2d2.urdf", None).unwrap(); + let body_info = physics_client.get_body_info(r2d2).unwrap(); + assert_eq!(body_info.base_name.as_str(), "base_link"); + assert_eq!(body_info.body_name.as_str(), "physics"); +} +#[test] +// tests a fixed joint and a revolute joint +fn test_get_joint_info() { + let mut physics_client = PhysicsClient::connect(Direct).unwrap(); + physics_client + .set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + let r2d2 = physics_client.load_urdf("r2d2.urdf", None).unwrap(); + let joint_info = physics_client.get_joint_info(r2d2, 1); + assert_eq!(1, joint_info.joint_index); + assert_eq!("right_base_joint", joint_info.joint_name); + + assert_eq!(JointType::Fixed, joint_info.joint_type); + assert_eq!(-1, joint_info.q_index); + assert_eq!(-1, joint_info.u_index); + assert!(joint_info.flags.is_empty()); + float_compare(0., joint_info.joint_damping, 1e-10); + float_compare(0., joint_info.joint_friction, 1e-10); + float_compare(0., joint_info.joint_lower_limit, 1e-10); + float_compare(-1., joint_info.joint_upper_limit, 1e-10); + float_compare(0., joint_info.joint_max_force, 1e-10); + float_compare(0., joint_info.joint_max_velocity, 1e-10); + assert_eq!("right_base", joint_info.link_name); + slice_compare(&[0.; 3], joint_info.joint_axis.as_slice(), 1e-10); + println!("{}", joint_info.parent_frame_pose.rotation.quaternion()); + slice_compare( + &[0.2999999996780742, 0., -1.3898038463944216e-05], + joint_info.parent_frame_pose.translation.vector.as_slice(), + 1e-7, + ); + slice_compare( + &[0.0, 0.7070904020014416, 0.0, 0.7071231599922604], + joint_info.parent_frame_pose.rotation.coords.as_slice(), + 1e-7, + ); + assert_eq!(0, joint_info.parent_index.unwrap()); + let joint_info = physics_client.get_joint_info(r2d2, 2); + assert_eq!(2, joint_info.joint_index); + assert_eq!("right_front_wheel_joint", joint_info.joint_name); + + assert_eq!(JointType::Revolute, joint_info.joint_type); + assert_eq!(7, joint_info.q_index); + assert_eq!(6, joint_info.u_index); + assert_eq!(JointInfoFlags::JOINT_CHANGE_MAX_FORCE, joint_info.flags); + float_compare(0., joint_info.joint_damping, 1e-10); + float_compare(0., joint_info.joint_friction, 1e-10); + float_compare(0., joint_info.joint_lower_limit, 1e-10); + float_compare(-1., joint_info.joint_upper_limit, 1e-10); + float_compare(100., joint_info.joint_max_force, 1e-10); + float_compare(100., joint_info.joint_max_velocity, 1e-10); + assert_eq!("right_front_wheel", joint_info.link_name); + slice_compare(&[0., 0., 1.], joint_info.joint_axis.as_slice(), 1e-10); + println!("{}", joint_info.parent_frame_pose.rotation.quaternion()); + slice_compare( + &[0.0, 0.133333333333, -0.085], + joint_info.parent_frame_pose.translation.vector.as_slice(), + 1e-7, + ); + slice_compare( + &[0.0, -0.7070904020014416, 0.0, 0.7071231599922604], + joint_info.parent_frame_pose.rotation.coords.as_slice(), + 1e-7, + ); + assert_eq!(1, joint_info.parent_index.unwrap()); +} + +pub fn set_joint_positions(client: &mut PhysicsClient, robot: BodyId, position: &[f64]) { + let num_joints = client.get_num_joints(robot); + assert_eq!(num_joints, position.len()); + let indices = (0..num_joints).into_iter().collect::>(); + let zero_vec = vec![0.; num_joints]; + let position_gains = vec![1.; num_joints]; + let velocity_gains = vec![0.3; num_joints]; + client + .set_joint_motor_control_array( + robot, + indices.as_slice(), + ControlModeArray::PositionsWithPd { + target_positions: position, + target_velocities: zero_vec.as_slice(), + position_gains: position_gains.as_slice(), + velocity_gains: velocity_gains.as_slice(), + }, + None, + ) + .unwrap(); +} + +pub fn get_joint_states( + client: &mut PhysicsClient, + robot: BodyId, +) -> (Vec, Vec, Vec) { + let num_joints = client.get_num_joints(robot); + let indices = (0..num_joints).into_iter().collect::>(); + let joint_states = client.get_joint_states(robot, indices.as_slice()).unwrap(); + let pos = joint_states + .iter() + .map(|x| x.joint_position) + .collect::>(); + let vel = joint_states + .iter() + .map(|x| x.joint_velocity) + .collect::>(); + let torque = joint_states + .iter() + .map(|x| x.joint_motor_torque) + .collect::>(); + (pos, vel, torque) +} + +pub fn get_motor_joint_states( + client: &mut PhysicsClient, + robot: BodyId, +) -> (Vec, Vec, Vec) { + let num_joints = client.get_num_joints(robot); + let indices = (0..num_joints).into_iter().collect::>(); + let joint_states = client.get_joint_states(robot, indices.as_slice()).unwrap(); + let joint_infos: Vec = (0..num_joints) + .into_iter() + .map(|y| client.get_joint_info(robot, y)) + .collect::>(); + let joint_states = joint_states + .iter() + .zip(joint_infos.iter()) + .filter(|(_j, i)| i.q_index > -1) + .map(|(j, _i)| *j) + .collect::>(); + let pos = joint_states + .iter() + .map(|x| x.joint_position) + .collect::>(); + let vel = joint_states + .iter() + .map(|x| x.joint_velocity) + .collect::>(); + let torque = joint_states + .iter() + .map(|x| x.joint_motor_torque) + .collect::>(); + (pos, vel, torque) +} +pub fn multiply_jacobian( + client: &mut PhysicsClient, + robot: BodyId, + jacobian: &Matrix3xX, + vector: &[f64], +) -> Vector3 { + let mut result = Vector3::new(0., 0., 0.); + let mut i = 0; + for c in 0..vector.len() { + if client.get_joint_info(robot, c).q_index > -1 { + for r in 0..3 { + result[r] += jacobian[(r, i)] * vector[c]; + } + i += 1; + } + } + result +} + +#[test] +fn test_jacobian() { + let delta_t = Duration::from_secs_f64(0.001); + let mut p = PhysicsClient::connect(Direct).unwrap(); + p.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + p.set_time_step(delta_t); + + let kuka_id = p + .load_urdf( + "TwoJointRobot_w_fixedJoints.urdf", + UrdfOptions { + use_fixed_base: true, + ..Default::default() + }, + ) + .unwrap(); + + let num_joints = p.get_num_joints(kuka_id); + let kuka_end_effector_index = num_joints - 1; + set_joint_positions(&mut p, kuka_id, vec![0.1; num_joints].as_slice()); + p.step_simulation().unwrap(); + + let (_pos, vel, _torq) = get_joint_states(&mut p, kuka_id); + let (mpos, _mvel, _mtorq) = get_motor_joint_states(&mut p, kuka_id); + let result = p + .get_link_state(kuka_id, kuka_end_effector_index, true, true) + .unwrap(); + println!("{:?}", vel); + let zero_vec = vec![0.; mpos.len()]; + let jacobian = p + .calculate_jacobian( + kuka_id, + kuka_end_effector_index, + result.local_inertial_pose.translation, + mpos.as_slice(), + zero_vec.as_slice(), + zero_vec.as_slice(), + ) + .unwrap(); + let q_dot: DVector = DVector::from_vec(_mvel); + // println!("aaa{:?}", vel); + let cartesian_velocity = jacobian.clone() * q_dot; + println!("{:?}", cartesian_velocity); + let linear_vel = multiply_jacobian( + &mut p, + kuka_id, + &jacobian.get_linear_jacobian(), + vel.as_slice(), + ); + let angular_vel = multiply_jacobian( + &mut p, + kuka_id, + &jacobian.get_angular_jacobian(), + vel.as_slice(), + ); + slice_compare( + cartesian_velocity.get_linear_velocity().as_slice(), + linear_vel.as_slice(), + 1e-10, + ); + slice_compare( + cartesian_velocity.get_angular_velocity().as_slice(), + angular_vel.as_slice(), + 1e-10, + ); + let target_linear_jacobian = [ + -0.1618321740829912, + 1.9909341219607504, + 0.0, + -0.13073095940453022, + 0.991417881749755, + 0.0, + ]; + for (i, j) in jacobian + .get_linear_jacobian() + .as_slice() + .iter() + .zip(target_linear_jacobian.iter()) + { + println!("{} {}", i, j); + assert!((i - j).abs() < 1e-6); + } + let target_angluar_jacobian = [0., 0., 1.0, 0.0, 0., 1.]; + for (i, j) in jacobian + .get_angular_jacobian() + .as_slice() + .iter() + .zip(target_angluar_jacobian.iter()) + { + println!("{} {}", i, j); + assert!((i - j).abs() < 1e-6); + } +} + +#[test] +fn test_get_link_state() { + let delta_t = Duration::from_secs_f64(0.001); + let mut p = PhysicsClient::connect(Direct).unwrap(); + p.set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + + p.set_time_step(delta_t); + + let kuka_id = p + .load_urdf( + "TwoJointRobot_w_fixedJoints.urdf", + UrdfOptions { + use_fixed_base: true, + ..Default::default() + }, + ) + .unwrap(); + + let num_joints = p.get_num_joints(kuka_id); + let kuka_end_effector_index = num_joints - 1; + set_joint_positions(&mut p, kuka_id, vec![0.1; num_joints].as_slice()); + p.step_simulation().unwrap(); + + let result = p + .get_link_state(kuka_id, kuka_end_effector_index, true, true) + .unwrap(); + let m_world_position = [1.9909341219607506, 0.1618321740829912, 0.12500000000000003]; + let m_world_orientation = [-0.0, -0.0, 0.06550617623646283, 0.997852163837348]; + let m_local_inertial_position = [0.; 3]; + let m_local_inertial_orientation = [0., 0., 0., 1.]; + let m_world_link_frame_position = [1.990934133529663, 0.16183216869831085, 0.125]; + let m_world_link_frame_orientation = [0.0, 0.0, 0.06550618261098862, 0.9978521466255188]; + let m_world_linear_velocity = [-18.107084901818524, 161.0722445230232, 0.0]; + let m_world_angular_velocity = [0.0, 0.0, 131.10623082146793]; + slice_compare( + &result.world_pose.translation.vector.as_slice(), + &m_world_position, + 1e-6, + ); + slice_compare( + &result.world_pose.rotation.coords.as_slice(), + &m_world_orientation, + 1e-6, + ); + slice_compare( + &result.local_inertial_pose.translation.vector.as_slice(), + &m_local_inertial_position, + 1e-6, + ); + slice_compare( + &result.local_inertial_pose.rotation.coords.as_slice(), + &m_local_inertial_orientation, + 1e-6, + ); + slice_compare( + &result.world_link_frame_pose.translation.vector.as_slice(), + &m_world_link_frame_position, + 1e-6, + ); + slice_compare( + &result.world_link_frame_pose.rotation.coords.as_slice(), + &m_world_link_frame_orientation, + 1e-6, + ); + slice_compare( + &result.get_linear_world_velocity().unwrap().as_slice(), + &m_world_linear_velocity, + 1e-5, + ); + slice_compare( + &result.get_angular_world_velocity().unwrap().as_slice(), + &m_world_angular_velocity, + 1e-6, + ); + let link_states = p + .get_link_states(kuka_id, &[kuka_end_effector_index], true, true) + .unwrap(); + let link_state_from_link_states = link_states.get(0).unwrap(); + slice_compare( + link_state_from_link_states + .world_link_frame_pose + .to_homogeneous() + .as_slice(), + result.world_link_frame_pose.to_homogeneous().as_slice(), + 1e-10, + ); +} + +#[test] +pub fn inverse_dynamics_test() { + let target_torque = [ + [ + 2.789039373397827, + -4.350228309631348, + -9.806090354919434, + -11.364048957824707, + -8.452873229980469, + -2.4533324241638184, + 4.292827606201172, + 9.478754043579102, + 11.22978687286377, + 8.697705268859863, + ], + [ + 1.5324022769927979, + -0.3981592059135437, + -2.1556396484375, + -3.0122971534729004, + -2.61287522315979, + -1.1830209493637085, + 0.663445770740509, + 2.273959159851074, + 3.1140832901000977, + 2.8513741493225098, + ], + ]; + let delta_t = Duration::from_secs_f64(0.1); + let mut physics_client = PhysicsClient::connect(Direct).unwrap(); + physics_client + .set_additional_search_path("../rubullet-sys/bullet3/libbullet3/data") + .unwrap(); + physics_client.set_time_step(delta_t); + let id_revolute_joints = [0, 3]; + let id_robot = physics_client + .load_urdf( + "TwoJointRobot_w_fixedJoints.urdf", + UrdfOptions { + use_fixed_base: true, + ..Default::default() + }, + ) + .unwrap(); + physics_client.change_dynamics_angular_damping(id_robot, 0.); + physics_client.change_dynamics_linear_damping(id_robot, 0.); + physics_client + .set_joint_motor_control_array( + id_robot, + &id_revolute_joints, + ControlModeArray::Velocities(&[0., 0.]), + Some(&[0., 0.]), + ) + .unwrap(); + + // Target Positions: + let start = 0.; + let end = 1.; + + let steps = ((end - start) / delta_t.as_secs_f64()) as usize; + let mut t = vec![0.; steps]; + + let mut q_pos_desired = vec![vec![0.; steps]; 2]; + let mut q_vel_desired = vec![vec![0.; steps]; 2]; + let mut q_acc_desired = vec![vec![0.; steps]; 2]; + + for s in 0..steps { + t[s] = start + s as f64 * delta_t.as_secs_f64(); + q_pos_desired[0][s] = 1. / (2. * PI) * f64::sin(2. * PI * t[s]) - t[s]; + q_pos_desired[1][s] = -1. / (2. * PI) * f64::sin(2. * PI * t[s]) - 1.; + + q_vel_desired[0][s] = f64::cos(2. * PI * t[s]) - 1.; + q_vel_desired[1][s] = f64::sin(2. * PI * t[s]); + + q_acc_desired[0][s] = -2. * PI * f64::sin(2. * PI * t[s]); + q_acc_desired[1][s] = 2. * PI * f64::cos(2. * PI * t[s]); + } + let mut q_pos = vec![vec![0.; steps]; 2]; + let mut q_vel = vec![vec![0.; steps]; 2]; + let mut q_tor = vec![vec![0.; steps]; 2]; + + for i in 0..t.len() { + let joint_states = physics_client.get_joint_states(id_robot, &[0, 3]).unwrap(); + q_pos[0][1] = joint_states[0].joint_position; + let a = joint_states[1].joint_position; + q_pos[1][i] = a; + + q_vel[0][i] = joint_states[0].joint_velocity; + q_vel[1][i] = joint_states[1].joint_velocity; + + let obj_pos = [q_pos[0][i], q_pos[1][i]]; + let obj_vel = [q_vel[0][i], q_vel[1][i]]; + let obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]; + + let torque = physics_client + .calculate_inverse_dynamics(id_robot, &obj_pos, &obj_vel, &obj_acc) + .unwrap(); + + q_tor[0][i] = torque[0]; + q_tor[1][i] = torque[1]; + + physics_client + .set_joint_motor_control_array(id_robot, &id_revolute_joints, Torques(&torque), None) + .unwrap(); + physics_client.step_simulation().unwrap(); + } + slice_compare(q_tor[0].as_slice(), &target_torque[0], 1e-10); +} + +#[test] +fn test_mass_matrix_and_inverse_kinematics() -> Result<()> { + let mut physics_client = PhysicsClient::connect(Direct)?; + physics_client.configure_debug_visualizer(DebugVisualizerFlag::CovEnableYAxisUp, true); + physics_client.set_time_step(Duration::from_secs_f64(1. / 60.)); + physics_client.set_gravity(Vector3::new(0.0, -9.8, 0.))?; + + let mut panda = PandaSim::new(&mut physics_client, Vector3::zeros())?; + panda.step(&mut physics_client); + + Ok(()) +} + +pub struct PandaSim { + pub offset: Vector3, + pub id: BodyId, + pub t: Duration, +} + +impl PandaSim { + const INITIAL_JOINT_POSITIONS: [f64; 9] = + [0.98, 0.458, 0.31, -2.24, -0.30, 2.66, 2.32, 0.02, 0.02]; + const PANDA_NUM_DOFS: usize = 7; + const PANDA_END_EFFECTOR_INDEX: usize = 11; + pub fn new(client: &mut PhysicsClient, offset: Vector3) -> Result { + client.set_additional_search_path( + "../rubullet-sys/bullet3/libbullet3/examples/pybullet/gym/pybullet_data", + )?; + let cube_start_position = Isometry3::new( + Vector3::new(0., 0., 0.), + UnitQuaternion::from_euler_angles(-PI / 2., 0., 0.).scaled_axis(), + ); + + let urdf_options = UrdfOptions { + use_fixed_base: true, + base_transform: cube_start_position.clone(), + flags: LoadModelFlags::URDF_ENABLE_CACHED_GRAPHICS_SHAPES, + ..Default::default() + }; + let panda_id = client.load_urdf("franka_panda/panda.urdf", urdf_options)?; + client.change_dynamics_linear_damping(panda_id, 0.); + client.change_dynamics_angular_damping(panda_id, 0.); + let mut index = 0; + for i in 0..client.get_num_joints(panda_id) { + let info = client.get_joint_info(panda_id, i); + if info.joint_type == JointType::Revolute || info.joint_type == JointType::Prismatic { + client.reset_joint_state( + panda_id, + i, + PandaSim::INITIAL_JOINT_POSITIONS[index], + None, + )?; + index += 1; + } + } + let t = Duration::new(0, 0); + Ok(PandaSim { + offset, + id: panda_id, + t, + }) + } + pub fn step(&mut self, client: &mut PhysicsClient) { + let t = self.t.as_secs_f64(); + self.t += Duration::from_secs_f64(1. / 60.); + + let pose = Isometry3::from_parts( + Translation3::new( + 0.2 * f64::sin(1.5 * t), + 0.044, + -0.6 + 0.1 * f64::cos(1.5 * t), + ), + UnitQuaternion::::from_euler_angles(PI / 2., 0., 0.), + ); + let inverse_kinematics_parameters = + InverseKinematicsParametersBuilder::new(PandaSim::PANDA_END_EFFECTOR_INDEX, &pose) + .set_max_num_iterations(5) + .build(); + let joint_poses = client + .calculate_inverse_kinematics(self.id, inverse_kinematics_parameters) + .unwrap(); + + for i in 0..PandaSim::PANDA_NUM_DOFS { + client.set_joint_motor_control( + self.id, + i, + ControlMode::Position(joint_poses[i]), + Some(240. * 5.), + ); + } + let target_mass_matrix = [ + 1.2851012047449573, + 0.019937918309349063, + 1.099094168104455, + -0.13992071941819356, + -0.04530258995812824, + 0.015010766618204326, + -0.00419273685297444, + -0.004045701114304651, + 0.004045701114304651, + 0.019937918309349063, + 1.4200408634854977, + -0.13973846625926817, + -0.6766534143669977, + -0.011150292631844208, + -0.11222097289908575, + 7.963273507305265e-05, + 0.021288781819096075, + -0.021288781819096075, + 1.099094168104455, + -0.13973846625926817, + 1.0599485744387636, + -0.01927738076755258, + -0.03898176486608705, + 0.0367718209930567, + -0.0038394168220030464, + -0.008321730809750807, + 0.008321730809750807, + -0.13992071941819356, + -0.6766534143669977, + -0.01927738076755258, + 0.8883254282752625, + 0.03919655691643165, + 0.13355872722768167, + 0.0005209277856703768, + -0.04891378328717881, + 0.04891378328717881, + -0.04530258995812824, + -0.011150292631844208, + -0.03898176486608705, + 0.03919655691643165, + 0.028341271915447625, + -0.0001434455846943853, + 0.0037745850335795, + 1.8885374235014403e-05, + -1.8885374235014403e-05, + 0.015010766618204326, + -0.11222097289908575, + 0.0367718209930567, + 0.13355872722768167, + -0.0001434455846943853, + 0.047402314149303876, + 2.053797643165216e-14, + -0.018417574890008004, + 0.018417574890008004, + -0.00419273685297444, + 7.963273507305265e-05, + -0.0038394168220030464, + 0.0005209277856703768, + 0.0037745850335795, + 2.053797643165216e-14, + 0.004194301009161442, + 0.0, + 0.0, + -0.004045701114304651, + 0.021288781819096075, + -0.008321730809750807, + -0.04891378328717881, + 1.8885374235014403e-05, + -0.018417574890008004, + 0.0, + 0.1, + 0.0, + 0.004045701114304651, + -0.021288781819096075, + 0.008321730809750807, + 0.04891378328717881, + -1.8885374235014403e-05, + 0.018417574890008004, + 0.0, + 0.0, + 0.1, + ]; + let target_joint_poses = [ + 1.1029851000632531, + 0.43354557662855453, + 0.3608104666320187, + -2.3105861116521096, + -0.2888395010735958, + 2.6904095021250938, + 2.4711777602235387, + 0.02, + 0.02, + ]; + let mass = client + .calculate_mass_matrix(self.id, joint_poses.as_slice()) + .unwrap(); + slice_compare(joint_poses.as_slice(), &target_joint_poses, 1e-6); + slice_compare(mass.as_slice(), &target_mass_matrix, 1e-6); + } +} +#[test] +fn compute_view_matrix_test() { + let eye_position = [1.; 3]; + let target_position = [1., 0., 0.]; + let up_vector = [0., 1., 0.]; + let view_matrix = PhysicsClient::compute_view_matrix(eye_position, target_position, up_vector); + let desired_matrix = [ + 0.99999994, + 0.0, + -0.0, + 0.0, + -0.0, + 0.7071067, + 0.70710677, + 0.0, + 0.0, + -0.7071067, + 0.70710677, + 0.0, + -0.99999994, + -0.0, + -1.4142135, + 1.0, + ]; + slice_compare_f32(view_matrix.as_slice(), &desired_matrix, 1e-7); +} +#[test] +fn compute_view_matrix_from_yaw_pitch_roll_test() { + let target_position = [1., 0., 0.]; + let view_matrix = PhysicsClient::compute_view_matrix_from_yaw_pitch_roll( + target_position, + 0.6, + 0.2, + 0.3, + 0.5, + false, + ); + let desired_matrix = [ + -0.9999939799308777, + -1.8276923583471216e-05, + -0.0034906466025859118, + 0.0, + 2.2373569663614035e-10, + 0.9999864101409912, + -0.005235963501036167, + 0.0, + 0.003490694332867861, + -0.00523593183606863, + -0.9999802708625793, + 0.0, + 0.9999939799308777, + 1.8277205526828766e-05, + -0.5965093970298767, + 1.0, + ]; + slice_compare_f32(view_matrix.as_slice(), &desired_matrix, 1e-7); +} +#[test] +fn compute_projection_matrix_fov_test() { + let projection_matrix = PhysicsClient::compute_projection_matrix_fov(0.4, 0.6, 0.2, 0.6); + let desired_matrix = [ + 477.4628601074219, + 0.0, + 0.0, + 0.0, + 0.0, + 286.47772216796875, + 0.0, + 0.0, + 0.0, + 0.0, + -1.9999998807907104, + -1.0, + 0.0, + 0.0, + -0.5999999642372131, + 0.0, + ]; + slice_compare_f32(projection_matrix.as_slice(), &desired_matrix, 1e-7); +} +#[test] +fn compute_projection_matrix_test() { + let projection_matrix = PhysicsClient::compute_projection_matrix(0.1, 0.2, 0.3, 0.4, 0.2, 0.6); + let desired_matrix = [ + 4.0, + 0.0, + 0.0, + 0.0, + 0.0, + 4.000000476837158, + 0.0, + 0.0, + 3.0, + 7.000000953674316, + -1.9999998807907104, + -1.0, + 0.0, + 0.0, + -0.5999999642372131, + 0.0, + ]; + slice_compare_f32(projection_matrix.as_slice(), &desired_matrix, 1e-7); +} diff --git a/src/client.rs b/src/client.rs deleted file mode 100644 index d2be2c7..0000000 --- a/src/client.rs +++ /dev/null @@ -1,386 +0,0 @@ -//! The main physics client. -//! -//! This is largely modeled after the PyBullet API but utilizes Rust's more expressive type system -//! where available. -use std::{ffi::CString, os::raw::c_int, path::Path, ptr}; - -// I currently do not know the best way to represent the file operations for Windows. PyBullet uses -// raw C-strings but that doesn't seem appropriate here. I don't really have a Windows machine, so -// until then... -use std::os::unix::ffi::OsStrExt; - -use crate::{ffi, Error, Mode}; -use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion, Vector3}; - -/// The "handle" to the physics client. -/// -/// For whatever reason, the Bullet C API obfuscates the handle type by defining the handle type as -/// a pointer to an (essentially) anonymous struct. That's ugly to use here, and we know it isn't -/// going to be null, so we'll just do this alias. -type Handle = std::ptr::NonNull; - -/// Connection to a physics server. -/// -/// This serves as an abstraction over the possible physics servers, providing a unified interface. -pub struct PhysicsClient { - /// The underlying `b3PhysicsClientHandle` that is guaranteed to not be null. - handle: Handle, - - /// A marker indicating whether or not a GUI is in use by this client. - gui_marker: Option, -} - -impl PhysicsClient { - pub fn connect(mode: Mode) -> Result { - let (raw_handle, gui_marker) = match mode { - Mode::Direct => unsafe { (ffi::b3ConnectPhysicsDirect(), None) }, - Mode::Gui => { - // Only one GUI is allowed per process. Try to get the marker and fail if there is - // another. - let gui_marker = GuiMarker::acquire()?; - - // Looking at the source code for both of these functions, they do not assume - // anything about the size of `argv` beyond what is supplied by `argc`. So, and - // `argc` of zero should keep this safe. - let raw_handle = if cfg!(target_os = "macos") { - unsafe { - ffi::b3CreateInProcessPhysicsServerAndConnectMainThread(0, ptr::null_mut()) - } - } else { - unsafe { ffi::b3CreateInProcessPhysicsServerAndConnect(0, ptr::null_mut()) } - }; - - (raw_handle, Some(gui_marker)) - } - }; - - // Make sure the returned pointer is valid. - let handle = - Handle::new(raw_handle).ok_or_else(|| Error::new("Bullet returned a null handle"))?; - - // At this point, we need to disconnect the physics client at any error. So we create the - // Rust struct and allow the `Drop` implementation to take care of that. - let mut client = PhysicsClient { handle, gui_marker }; - - // Make sure it is up and running. - if !client.can_submit_command() { - return Err(Error::new("Physics server is not running")); - } - - // Now perform a series of commands to finish starting up the server. I don't know what they - // do but it's what PyBullet does. Note that PyBullet does not check these for `null` so I - // am assuming that they either can't be null or the consumer does the check. - unsafe { - let command = ffi::b3InitSyncBodyInfoCommand(client.handle.as_ptr()); - let status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(client.handle.as_ptr(), command); - let status_type = ffi::b3GetStatusType(status_handle); - - if status_type != ffi::EnumSharedMemoryServerStatus::CMD_SYNC_BODY_INFO_COMPLETED as _ { - return Err(Error::new("Connection terminated, couldn't get body info")); - } - - let command = ffi::b3InitSyncUserDataCommand(client.handle.as_ptr()); - let status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(client.handle.as_ptr(), command); - let status_type = ffi::b3GetStatusType(status_handle); - - if status_type != ffi::EnumSharedMemoryServerStatus::CMD_SYNC_USER_DATA_COMPLETED as _ { - return Err(Error::new("Connection terminated, couldn't get user data")); - } - } - - // The client is up and running - Ok(client) - } - - /// Sets an additional search path for loading assests. - pub fn set_additional_search_path>(&mut self, path: P) -> Result<(), Error> { - if !self.can_submit_command() { - return Err(Error::new("Not connected to physics server")); - } - - let path = CString::new(path.as_ref().as_os_str().as_bytes()) - .map_err(|_| Error::new("Invalid path"))?; - - unsafe { - // Based on PyBullet, it appears that this path is copied and it does not need to live - // after calling the function. - let command_handle = - ffi::b3SetAdditionalSearchPath(self.handle.as_ptr(), path.as_ptr()); - let _status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(self.handle.as_ptr(), command_handle); - } - - Ok(()) - } - - /// Sets the default gravity force for all objects. - /// - /// By default, there is no gravitational force enabled. - pub fn set_gravity(&mut self, gravity: Vector3) -> Result<(), Error> { - if !self.can_submit_command() { - return Err(Error::new("Not connected to physics server")); - } - - unsafe { - // PyBullet error checks none of these. Looking through the code, it looks like there is - // no possible way to return an error on them. - let command = ffi::b3InitPhysicsParamCommand(self.handle.as_ptr()); - let _ret = ffi::b3PhysicsParamSetGravity(command, gravity.x, gravity.y, gravity.z); - let _status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(self.handle.as_ptr(), command); - } - - Ok(()) - } - - /// Sends a command to the physics server to load a physics model from a Universal Robot - /// Description File (URDF). - pub fn load_urdf>( - &mut self, - file: P, - options: UrdfOptions, - ) -> Result { - if !self.can_submit_command() { - return Err(Error::new("Not connected to physics server")); - } - - let file = CString::new(file.as_ref().as_os_str().as_bytes()) - .map_err(|_| Error::new("Invalid path"))?; - - // There's probably a cleaner way to do it. The one-liner I came up with was neat but bad - // code. - let mut flags = 0; - if options.use_inertia_from_file { - flags |= ffi::eURDF_Flags::URDF_USE_INERTIA_FROM_FILE as c_int; - } - if options.use_self_collision { - flags |= ffi::eURDF_Flags::URDF_USE_SELF_COLLISION as c_int; - } - if options.enable_sleeping { - flags |= ffi::eURDF_Flags::URDF_ENABLE_SLEEPING as c_int; - } - if options.maintain_link_order { - flags |= ffi::eURDF_Flags::URDF_MAINTAIN_LINK_ORDER as c_int; - } - - unsafe { - // As always, PyBullet does not document and does not check return codes. - let command = ffi::b3LoadUrdfCommandInit(self.handle.as_ptr(), file.as_ptr()); - let _ret = ffi::b3LoadUrdfCommandSetFlags(command, flags); - let _ret = ffi::b3LoadUrdfCommandSetStartPosition( - command, - options.base_transform.translation.x, - options.base_transform.translation.y, - options.base_transform.translation.z, - ); - let _ret = ffi::b3LoadUrdfCommandSetStartOrientation( - command, - options.base_transform.rotation.coords.x, - options.base_transform.rotation.coords.y, - options.base_transform.rotation.coords.z, - options.base_transform.rotation.coords.w, - ); - - if options.use_fixed_base { - let _ret = ffi::b3LoadUrdfCommandSetUseFixedBase(command, 1); - } - - if options.global_scaling > 0.0 { - let _ret = - ffi::b3LoadUrdfCommandSetGlobalScaling(command, options.global_scaling as f64); - } - - let status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(self.handle.as_ptr(), command); - let status_type = ffi::b3GetStatusType(status_handle); - if status_type != ffi::EnumSharedMemoryServerStatus::CMD_URDF_LOADING_COMPLETED as c_int - { - return Err(Error::new("Cannot load URDF file")); - } - - Ok(BodyId(ffi::b3GetStatusBodyIndex(status_handle))) - } - } - - /// Performs all the actions in a single forward dynamics simulation step such as collision - /// detection, constraint solving, and integration. - // TODO: Mention changing step size and automatic steps. - // TODO: Return analytics data? - pub fn step_simulation(&mut self) -> Result<(), Error> { - if !self.can_submit_command() { - return Err(Error::new("Not connected to physics server")); - } - - unsafe { - let command = ffi::b3InitStepSimulationCommand(self.handle.as_ptr()); - let status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(self.handle.as_ptr(), command); - let status_type = ffi::b3GetStatusType(status_handle); - - if status_type - != ffi::EnumSharedMemoryServerStatus::CMD_STEP_FORWARD_SIMULATION_COMPLETED as i32 - { - return Err(Error::new("Failed to perform forward step")); - } - } - - Ok(()) - } - - /// Reports the current transform of the base. - pub fn get_base_transform(&mut self, body: BodyId) -> Result, Error> { - if !self.can_submit_command() { - return Err(Error::new("Not connected to physics server")); - } - - unsafe { - let cmd_handle = ffi::b3RequestActualStateCommandInit(self.handle.as_ptr(), body.0); - let status_handle = - ffi::b3SubmitClientCommandAndWaitStatus(self.handle.as_ptr(), cmd_handle); - let status_type = ffi::b3GetStatusType(status_handle); - - if status_type - != ffi::EnumSharedMemoryServerStatus::CMD_ACTUAL_STATE_UPDATE_COMPLETED as c_int - { - return Err(Error::new("Failed to get base transform")); - } - - // To be totally honest, I'm not sure this part is correct. - let actual_state_q: *mut f64 = ptr::null_mut(); - ffi::b3GetStatusActualState( - status_handle, - ptr::null_mut(), - ptr::null_mut(), - ptr::null_mut(), - ptr::null_mut(), - &actual_state_q as _, - ptr::null_mut(), - ptr::null_mut(), - ); - - assert!(!actual_state_q.is_null()); - - let tx = *actual_state_q; - let ty = *(actual_state_q.offset(1)); - let tz = *(actual_state_q.offset(2)); - - let rx = *(actual_state_q.offset(3)); - let ry = *(actual_state_q.offset(4)); - let rz = *(actual_state_q.offset(5)); - let rw = *(actual_state_q.offset(6)); - - let tra = Translation3::new(tx, ty, tz); - let rot = Quaternion::new(rw, rx, ry, rz); - - Ok(Isometry3::from_parts( - tra, - UnitQuaternion::from_quaternion(rot), - )) - } - } - - /// Returns whether or not this client can submit a command. - fn can_submit_command(&mut self) -> bool { - unsafe { ffi::b3CanSubmitCommand(self.handle.as_ptr()) != 0 } - } -} - -impl Drop for PhysicsClient { - fn drop(&mut self) { - unsafe { ffi::b3DisconnectSharedMemory(self.handle.as_ptr()) } - } -} - -/// The unique ID for a body within a physics server. -#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] -pub struct BodyId(c_int); - -/// Options for loading a URDF into the physics server. -pub struct UrdfOptions { - /// Creates the base of the object with the given transform. - pub base_transform: Isometry3, - - /// Forces the base of the loaded object to be static. - pub use_fixed_base: bool, - - /// Use the inertia tensor provided in the URDF. - /// - /// By default, Bullet will recompute the inertial tensor based on the mass and volume of the - /// collision shape. Use this is you can provide a more accurate inertia tensor. - pub use_inertia_from_file: bool, - - /// Enables or disables self-collision. - pub use_self_collision: bool, - - /// Allow the disabling of simulation after a body hasn't moved for a while. - /// - /// Interaction with active bodies will re-enable simulation. - pub enable_sleeping: bool, - - /// Try to maintain the link order from the URDF file. - pub maintain_link_order: bool, - - /// Applies a scale factor to the model. - pub global_scaling: f64, - - // Future proofs the struct. Unfortunately, `#[non_exhaustive]` doesn't apply to structs. - #[doc(hidden)] - pub _unused: (), -} - -impl Default for UrdfOptions { - fn default() -> UrdfOptions { - UrdfOptions { - base_transform: Isometry3::identity(), - use_fixed_base: false, - use_inertia_from_file: false, - use_self_collision: false, - enable_sleeping: false, - maintain_link_order: false, - global_scaling: -1.0, - - _unused: (), - } - } -} - -/// Module used to enforce the existence of only a single GUI -mod gui_marker { - use std::sync::atomic::{AtomicBool, Ordering}; - - /// A marker for whether or not a GUI has been started. - /// - /// PyBullet only allows a single GUI per process. I assume that this is a limitation of the - /// underlying C API, so it will also be enforced here. - static GUI_EXISTS: AtomicBool = AtomicBool::new(false); - - /// A marker type for keeping track of the existence of a GUI. - pub struct GuiMarker { - _unused: (), - } - - impl GuiMarker { - /// Attempts to acquire the GUI marker. - pub fn acquire() -> Result { - // We can probably use a weaker ordering but this will be called so little that we - // may as well be sure about it. - if GUI_EXISTS.compare_and_swap(false, true, Ordering::SeqCst) { - Err(crate::Error::new( - "Only one in-process GUI connection allowed", - )) - } else { - Ok(GuiMarker { _unused: () }) - } - } - } - - impl Drop for GuiMarker { - fn drop(&mut self) { - // We are the only marker so no need to CAS - GUI_EXISTS.store(false, Ordering::SeqCst) - } - } -} -use self::gui_marker::GuiMarker; diff --git a/src/ffi.rs b/src/ffi.rs deleted file mode 100644 index ce2ea05..0000000 --- a/src/ffi.rs +++ /dev/null @@ -1,257 +0,0 @@ -//! Foreign function interface for Bullet C API. -#![allow(non_camel_case_types, non_snake_case)] -use std::os::raw::{c_char, c_int}; - -#[repr(C)] -pub struct b3PhysicsClientHandle__ { - _unused: c_int, -} -pub type b3PhysicsClientHandle = *mut b3PhysicsClientHandle__; - -#[repr(C)] -pub struct b3SharedMemoryCommandHandle__ { - _unused: c_int, -} -pub type b3SharedMemoryCommandHandle = *mut b3SharedMemoryCommandHandle__; - -#[repr(C)] -pub struct b3SharedMemoryStatusHandle__ { - _unused: c_int, -} -pub type b3SharedMemoryStatusHandle = *mut b3SharedMemoryStatusHandle__; - -extern "C" { - pub fn b3ConnectPhysicsDirect() -> b3PhysicsClientHandle; - pub fn b3CreateInProcessPhysicsServerAndConnect( - argc: c_int, - argv: *mut *mut c_char, - ) -> b3PhysicsClientHandle; - pub fn b3CreateInProcessPhysicsServerAndConnectMainThread( - argc: c_int, - argv: *mut *mut c_char, - ) -> b3PhysicsClientHandle; - pub fn b3DisconnectSharedMemory(physClient: b3PhysicsClientHandle); - - pub fn b3CanSubmitCommand(physClient: b3PhysicsClientHandle) -> c_int; - pub fn b3SubmitClientCommandAndWaitStatus( - physClient: b3PhysicsClientHandle, - commandHandle: b3SharedMemoryCommandHandle, - ) -> b3SharedMemoryStatusHandle; - - pub fn b3GetStatusType(statusHandle: b3SharedMemoryStatusHandle) -> c_int; - - pub fn b3GetStatusActualState( - statusHandle: b3SharedMemoryStatusHandle, - bodyUniqueId: *mut c_int, - numDegreeOfFreedomQ: *mut c_int, - numDegreeOfFreedomU: *mut c_int, - rootLocalInertialFrame: *const *mut f64, - actualStateQ: *const *mut f64, - actualStateQdot: *const *mut f64, - jointReactionForces: *const *mut f64, - ) -> c_int; - - pub fn b3GetStatusBodyIndex(statusHandle: b3SharedMemoryStatusHandle) -> c_int; - - pub fn b3InitSyncBodyInfoCommand( - physClient: b3PhysicsClientHandle, - ) -> b3SharedMemoryCommandHandle; - pub fn b3InitSyncUserDataCommand( - physClient: b3PhysicsClientHandle, - ) -> b3SharedMemoryCommandHandle; - - pub fn b3InitPhysicsParamCommand( - physClient: b3PhysicsClientHandle, - ) -> b3SharedMemoryCommandHandle; - pub fn b3PhysicsParamSetGravity( - commandHandle: b3SharedMemoryCommandHandle, - gravx: f64, - gravy: f64, - gravz: f64, - ) -> c_int; - - pub fn b3InitStepSimulationCommand( - physClient: b3PhysicsClientHandle, - ) -> b3SharedMemoryCommandHandle; - - pub fn b3SetAdditionalSearchPath( - physClient: b3PhysicsClientHandle, - path: *const c_char, - ) -> b3SharedMemoryCommandHandle; - - pub fn b3LoadUrdfCommandInit( - physClient: b3PhysicsClientHandle, - urdfFileName: *const c_char, - ) -> b3SharedMemoryCommandHandle; - pub fn b3LoadUrdfCommandSetFlags( - commandHandle: b3SharedMemoryCommandHandle, - flags: c_int, - ) -> c_int; - pub fn b3LoadUrdfCommandSetStartPosition( - commandHandle: b3SharedMemoryCommandHandle, - startPosX: f64, - startPosY: f64, - startPosZ: f64, - ) -> c_int; - pub fn b3LoadUrdfCommandSetStartOrientation( - commandHandle: b3SharedMemoryCommandHandle, - startOrnX: f64, - startOrnY: f64, - startOrnZ: f64, - startOrnW: f64, - ) -> c_int; - pub fn b3LoadUrdfCommandSetUseFixedBase( - commandHandle: b3SharedMemoryCommandHandle, - useFixedBase: c_int, - ) -> c_int; - pub fn b3LoadUrdfCommandSetGlobalScaling( - commandHandle: b3SharedMemoryCommandHandle, - globalScaling: f64, - ) -> c_int; - - pub fn b3RequestActualStateCommandInit( - physClient: b3PhysicsClientHandle, - bodyUniqueId: c_int, - ) -> b3SharedMemoryCommandHandle; -} - -#[repr(C)] -pub enum eURDF_Flags { - URDF_USE_INERTIA_FROM_FILE = 2, - URDF_USE_SELF_COLLISION = 8, - URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16, - URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32, - URDF_RESERVED = 64, - URDF_USE_IMPLICIT_CYLINDER = 128, - URDF_GLOBAL_VELOCITIES_MB = 256, - MJCF_COLORS_FROM_FILE = 512, - URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024, - URDF_ENABLE_SLEEPING = 2048, - URDF_INITIALIZE_SAT_FEATURES = 4096, - URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, - URDF_PARSE_SENSORS = 16384, - URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768, - URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536, - URDF_MAINTAIN_LINK_ORDER = 131072, - URDF_ENABLE_WAKEUP = 262144, -} - -#[repr(C)] -pub enum EnumSharedMemoryServerStatus { - CMD_SHARED_MEMORY_NOT_INITIALIZED = 0, - CMD_WAITING_FOR_CLIENT_COMMAND, - CMD_CLIENT_COMMAND_COMPLETED, - CMD_UNKNOWN_COMMAND_FLUSHED, - CMD_SDF_LOADING_COMPLETED, - CMD_SDF_LOADING_FAILED, - CMD_URDF_LOADING_COMPLETED, - CMD_URDF_LOADING_FAILED, - CMD_BULLET_LOADING_COMPLETED, - CMD_BULLET_LOADING_FAILED, - CMD_BULLET_SAVING_COMPLETED, - CMD_BULLET_SAVING_FAILED, - CMD_MJCF_LOADING_COMPLETED, - CMD_MJCF_LOADING_FAILED, - CMD_REQUEST_INTERNAL_DATA_COMPLETED, - CMD_REQUEST_INTERNAL_DATA_FAILED, - CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, - CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, - CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, - CMD_RIGID_BODY_CREATION_COMPLETED, - CMD_SET_JOINT_FEEDBACK_COMPLETED, - CMD_ACTUAL_STATE_UPDATE_COMPLETED, - CMD_ACTUAL_STATE_UPDATE_FAILED, - CMD_DEBUG_LINES_COMPLETED, - CMD_DEBUG_LINES_OVERFLOW_FAILED, - CMD_DESIRED_STATE_RECEIVED_COMPLETED, - CMD_STEP_FORWARD_SIMULATION_COMPLETED, - CMD_RESET_SIMULATION_COMPLETED, - CMD_CAMERA_IMAGE_COMPLETED, - CMD_CAMERA_IMAGE_FAILED, - CMD_BODY_INFO_COMPLETED, - CMD_BODY_INFO_FAILED, - CMD_INVALID_STATUS, - CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, - CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, - CMD_CALCULATED_JACOBIAN_COMPLETED, - CMD_CALCULATED_JACOBIAN_FAILED, - CMD_CALCULATED_MASS_MATRIX_COMPLETED, - CMD_CALCULATED_MASS_MATRIX_FAILED, - CMD_CONTACT_POINT_INFORMATION_COMPLETED, - CMD_CONTACT_POINT_INFORMATION_FAILED, - CMD_REQUEST_AABB_OVERLAP_COMPLETED, - CMD_REQUEST_AABB_OVERLAP_FAILED, - CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED, - CMD_CALCULATE_INVERSE_KINEMATICS_FAILED, - CMD_SAVE_WORLD_COMPLETED, - CMD_SAVE_WORLD_FAILED, - CMD_VISUAL_SHAPE_INFO_COMPLETED, - CMD_VISUAL_SHAPE_INFO_FAILED, - CMD_VISUAL_SHAPE_UPDATE_COMPLETED, - CMD_VISUAL_SHAPE_UPDATE_FAILED, - CMD_LOAD_TEXTURE_COMPLETED, - CMD_LOAD_TEXTURE_FAILED, - CMD_USER_DEBUG_DRAW_COMPLETED, - CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, - CMD_USER_DEBUG_DRAW_FAILED, - CMD_USER_CONSTRAINT_COMPLETED, - CMD_USER_CONSTRAINT_INFO_COMPLETED, - CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED, - CMD_REMOVE_USER_CONSTRAINT_COMPLETED, - CMD_CHANGE_USER_CONSTRAINT_COMPLETED, - CMD_REMOVE_USER_CONSTRAINT_FAILED, - CMD_CHANGE_USER_CONSTRAINT_FAILED, - CMD_USER_CONSTRAINT_FAILED, - CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, - CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, - CMD_SYNC_BODY_INFO_COMPLETED, - CMD_SYNC_BODY_INFO_FAILED, - CMD_STATE_LOGGING_COMPLETED, - CMD_STATE_LOGGING_START_COMPLETED, - CMD_STATE_LOGGING_FAILED, - CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED, - CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED, - CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED, - CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, - CMD_REMOVE_BODY_COMPLETED, - CMD_REMOVE_BODY_FAILED, - CMD_GET_DYNAMICS_INFO_COMPLETED, - CMD_GET_DYNAMICS_INFO_FAILED, - CMD_CREATE_COLLISION_SHAPE_FAILED, - CMD_CREATE_COLLISION_SHAPE_COMPLETED, - CMD_CREATE_VISUAL_SHAPE_FAILED, - CMD_CREATE_VISUAL_SHAPE_COMPLETED, - CMD_CREATE_MULTI_BODY_FAILED, - CMD_CREATE_MULTI_BODY_COMPLETED, - CMD_REQUEST_COLLISION_INFO_COMPLETED, - CMD_REQUEST_COLLISION_INFO_FAILED, - CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, - CMD_CHANGE_TEXTURE_COMMAND_FAILED, - CMD_CUSTOM_COMMAND_COMPLETED, - CMD_CUSTOM_COMMAND_FAILED, - CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED, - CMD_SAVE_STATE_FAILED, - CMD_SAVE_STATE_COMPLETED, - CMD_RESTORE_STATE_FAILED, - CMD_RESTORE_STATE_COMPLETED, - CMD_COLLISION_SHAPE_INFO_COMPLETED, - CMD_COLLISION_SHAPE_INFO_FAILED, - CMD_LOAD_SOFT_BODY_FAILED, - CMD_LOAD_SOFT_BODY_COMPLETED, - - CMD_SYNC_USER_DATA_COMPLETED, - CMD_SYNC_USER_DATA_FAILED, - CMD_REQUEST_USER_DATA_COMPLETED, - CMD_REQUEST_USER_DATA_FAILED, - CMD_ADD_USER_DATA_COMPLETED, - CMD_ADD_USER_DATA_FAILED, - CMD_REMOVE_USER_DATA_COMPLETED, - CMD_REMOVE_USER_DATA_FAILED, - CMD_REMOVE_STATE_COMPLETED, - CMD_REMOVE_STATE_FAILED, - - CMD_REQUEST_MESH_DATA_COMPLETED, - CMD_REQUEST_MESH_DATA_FAILED, - - CMD_MAX_SERVER_COMMANDS, -} diff --git a/src/lib.rs b/src/lib.rs deleted file mode 100644 index fdfc92b..0000000 --- a/src/lib.rs +++ /dev/null @@ -1,24 +0,0 @@ -//! A Rust interface for Bullet physics inspired by PyBullet. -#![allow(dead_code)] - -/// A utility for creating C-string literals. -/* -macro_rules! cstr -{ - ($lit:expr) => { - concat!($lit, "\0") as *const str - as *const [::std::os::raw::c_char] - as *const ::std::os::raw::c_char - } -} -*/ -mod client; -mod error; -mod ffi; -mod mode; - -pub use crate::{ - client::{BodyId, PhysicsClient, UrdfOptions}, - error::Error, - mode::Mode, -};