//! Custom data types for RuBullet
use crate::Error;
use image::{ImageBuffer, Luma, RgbaImage};
use nalgebra::{
DVector, Isometry3, Matrix3xX, Matrix4, Matrix6xX, Quaternion, Translation3, UnitQuaternion,
Vector3, Vector6, U3,
};
use rubullet_sys::{
b3BodyInfo, b3ContactPointData, b3DynamicsInfo, b3JointInfo, b3JointSensorState, b3LinkState,
b3OpenGLVisualizerCameraInfo, b3PhysicsSimulationParameters, b3RayHitInfo, b3UserConstraint,
b3VisualShapeData,
};
use std::convert::TryFrom;
use std::ffi::CStr;
use std::os::raw::c_int;
use std::path::PathBuf;
use std::time::Duration;
/// 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);
/// The unique ID for a constraint.
#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)]
pub struct ConstraintId(pub(crate) c_int);
/// The unique ID for a Logging Object.
#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)]
pub struct LogId(pub(crate) c_int);
/// The unique ID for a State Object.
#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)]
pub struct StateId(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<i32> for JointType {
type Error = Error;
fn try_from(value: i32) -> Result<Self, Self::Error> {
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 <lower limit.
pub joint_upper_limit: f64,
/// Maximum force specified in URDF (possibly other file formats) Note that this value is not automatically used. You can use maxForce in 'setJointMotorControl2'.
pub joint_max_force: f64,
/// Maximum velocity specified in URDF. Note that the maximum velocity is not used in actual motor control commands at the moment.
pub joint_max_velocity: f64,
/// the name of the link, as specified in the URDF (or SDF etc.) file
pub link_name: String,
///joint axis in local frame (ignored for fixed joints)
pub joint_axis: Vector3<f64>,
/// joint pose in parent frame
pub parent_frame_pose: Isometry3<f64>,
/// parent link index. None means that the base is the parent link
pub parent_index: Option<usize>,
}
impl From<b3JointInfo> 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::<f64>::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: Vector3<f64>,
/// Target orientation in Cartesian world space.
/// If not specified, pure position IK will be used.
pub target_orientation: Option<UnitQuaternion<f64>>,
/// Optional null-space IK
pub limits: Option<InverseKinematicsNullSpaceParameters<'a>>,
/// 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<usize>,
/// 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<f64>,
}
/// 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<IkSolver> 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: Vector3::zeros(),
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<f64>) -> Self {
let target_position = target_pose.translation.vector;
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<UnitQuaternion<f64>>,
/// If specified the text will be drawn relative to the parents object coordinate system.
pub parent_object_id: Option<BodyId>,
/// 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<usize>,
/// replace an existing text item (to avoid flickering of remove/add)
pub replace_item_id: Option<ItemId>,
}
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<BodyId>,
/// 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<usize>,
/// replace an existing line (to improve performance and to avoid flickering of remove/add)
pub replace_item_id: Option<ItemId>,
}
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<f64>,
}
impl<T: Into<DVector<f64>>> std::ops::Mul<T> 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<f64> {
Matrix3xX::from(self.jacobian.fixed_rows::<U3>(0))
}
/// Angular part of the the jacobian (last 3 rows)
pub fn get_angular_jacobian(&self) -> Matrix3xX<f64> {
Matrix3xX::from(self.jacobian.fixed_rows::<U3>(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<b3JointSensorState> 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<f64>,
/// 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<bool>,
/// 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 ControlCommand specifies how the robot should move (Position Control, Velocity Control, Torque Control)
/// Each type of ControlCommand 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 ControlCommand {
/// 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<f64>,
},
/// 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<f64>,
},
}
impl ControlCommand {
pub(crate) fn get_int(&self) -> i32 {
match self {
ControlCommand::Position(_) => 2,
ControlCommand::Velocity(_) => 0,
ControlCommand::Torque(_) => 1,
ControlCommand::Pd { .. } => 3,
ControlCommand::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 [`ControlCommand`](`ControlCommand`) but with arrays. See [`ControlCommand`](`ControlCommand`) for details.
pub enum ControlCommandArray<'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 ControlCommandArray<'_> {
pub(crate) fn get_int(&self) -> i32 {
match self {
ControlCommandArray::Positions(_) => 2,
ControlCommandArray::Velocities(_) => 0,
ControlCommandArray::Torques(_) => 1,
ControlCommandArray::Pd { .. } => 3,
ControlCommandArray::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<f64>,
/// local offset of the inertial frame (center of mass) express in the URDF link frame
pub local_inertial_pose: Isometry3<f64>,
/// world pose of the URDF link frame
pub world_link_frame_pose: Isometry3<f64>,
/// Cartesian world linear velocity.
pub world_velocity: Option<Velocity>,
}
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<Vector3<f64>, 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<Vector3<f64>, 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<Vector6<f64>, 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
}
}
pub(crate) fn position_orientation_to_isometry(
position: [f64; 3],
orientation: [f64; 4],
) -> Isometry3<f64> {
Isometry3::<f64>::from_parts(
Translation3::from(Vector3::from_column_slice(&position)),
UnitQuaternion::from_quaternion(Quaternion::from_parts(
orientation[3],
Vector3::from_column_slice(&orientation[0..3]),
)),
)
}
pub(crate) fn combined_position_orientation_array_to_isometry(
combined: [f64; 7],
) -> Isometry3<f64> {
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<f64>,
/// 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<VisualShapeFlags>,
}
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<f64>,
},
/// 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<f64>,
},
/// 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<Vector3<f64>>,
/// 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<i32>,
},
/// 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<Vec<i32>>,
/// Scaling of the Mesh. Use [1.;3] for normal scaling.
mesh_scaling: Option<Vector3<f64>>,
},
/// 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<Vector3<f64>>,
/// 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<Vector3<f64>>,
/// Texture scaling. Use 1. for normal scaling.
texture_scaling: f64,
/// Heightfield data. Should be of size num_rows * num_columns
data: Vec<f32>,
/// 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<CollisionId>,
},
}
/// 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<f64>,
},
/// 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<f64>,
},
/// 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<Vector3<f64>>,
},
/// Create your own mesh.
Mesh {
/// Scaling of the Mesh. Use [1.;3] for normal scaling.
mesh_scaling: Option<Vector3<f64>>,
/// list of \[x,y,z\] coordinates.
vertices: Vec<[f64; 3]>,
/// triangle indices, should be a multiple of 3
indices: Vec<i32>,
/// 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<Vec<[f64; 2]>>,
/// vertex normals, number should be equal to number of vertices.
normals: Option<Vec<[f64; 3]>>,
},
}
/// 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<f64>,
/// Local pose of inertial frame
pub base_inertial_frame_pose: Isometry3<f64>,
/// List of the mass values, one for each link.
pub link_masses: Vec<f64>,
/// 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<CollisionId>,
/// 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<VisualId>,
/// list of local link poses, with respect to parent
pub link_poses: Vec<Isometry3<f64>>,
/// list of local inertial frame poses, in the link frame
pub link_inertial_frame_poses: Vec<Isometry3<f64>>,
/// Link index of the parent link or 0 for the base.
pub link_parent_indices: Vec<i32>,
/// list of joint types, one for each link.
pub link_joint_types: Vec<JointType>,
/// List of joint axis in local frame
pub link_joint_axis: Vec<Vector3<f64>>,
/// 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<LoadModelFlags>,
}
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,
}
}
}
/// 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<TextureId>,
/// 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<VisualShapeFlags>,
}
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<b3BodyInfo> 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<usize>,
/// 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<f64>,
/// 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<TextureId>,
}
impl From<b3VisualShapeData> 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 {
/// width image resolution in pixels (horizontal)
pub width: usize,
/// height image resolution in pixels (vertical)
pub height: usize,
/// RGB image with additional alpha channel
pub rgba: RgbaImage,
/// Depth image. Every pixel represents a distance in meters
pub depth: ImageBuffer<Luma<f32>, Vec<f32>>,
/// Segmentation image. Every pixel represents a unique [`BodyId`](`crate::types::BodyId`)
pub segmentation: ImageBuffer<Luma<i32>, Vec<i32>>,
}
/// 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<f64>);
impl Velocity {
/// returns the linear velocity (x,y,z)
pub fn get_linear_velocity(&self) -> Vector3<f64> {
self.0.fixed_rows::<U3>(0).into()
}
/// returns the angular velocity (wx,wy,wz)
pub fn get_angular_velocity(&self) -> Vector3<f64> {
self.0.fixed_rows::<U3>(3).into()
}
/// converts the velocity to a Vector6 (x,y,z,wx,wy,wz)
pub fn to_vector(&self) -> Vector6<f64> {
self.0
}
}
impl From<[f64; 6]> for Velocity {
fn from(input: [f64; 6]) -> Self {
Velocity(input.into())
}
}
impl From<Vector6<f64>> for Velocity {
fn from(input: Vector6<f64>) -> 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
}
}
/// contains the parameters for [`change_constraint`](`crate::PhysicsClient::change_constraint`) method.
#[derive(Default)]
pub struct ChangeConstraintOptions {
/// updated child pivot, see [`create_constraint`](`crate::PhysicsClient::create_constraint`)
pub joint_child_pivot: Option<Vector3<f64>>,
/// updated child frame orientation as quaternion
pub joint_child_frame_orientation: Option<UnitQuaternion<f64>>,
/// maximum force that constraint can apply
pub max_force: Option<f64>,
/// the ratio between the rates at which the two gears rotate
pub gear_ratio: Option<f64>,
/// In some cases, such as a differential drive, a third (auxiliary) link is used as reference pose.
pub gear_aux_link: Option<usize>,
/// the relative position target offset between two gears
pub relative_position_target: Option<f64>,
/// constraint error reduction parameter
pub erp: Option<f64>,
}
/// contains the parameters for [`change_constraint`](`crate::PhysicsClient::change_constraint`) method.
#[derive(Debug)]
pub struct ConstraintInfo {
/// the constraint for which this info is generated
pub id: ConstraintId,
/// parent body unique id
pub parent_body: BodyId,
/// parent body link index or `None` for base link.
pub parent_link_index: Option<usize>,
/// child body unique id or `None`or no body (specify a non-dynamic child frame in world coordinates)
pub child_body: Option<BodyId>,
/// child body link index or `None` for base link.
pub child_link_index: Option<usize>,
/// The [`JointType`](`crate::types::JointType`) for the constraint
pub constraint_type: JointType,
/// joint axis, in child link frame
pub joint_axis: Vector3<f64>,
/// pose of the joint frame relative to parent center of mass frame.
pub joint_parent_frame_pose: Isometry3<f64>,
/// updated child pose, see [`create_constraint`](`crate::PhysicsClient::create_constraint`)
pub joint_child_frame_pose: Isometry3<f64>,
/// maximum force that constraint can apply
pub max_applied_force: f64,
/// the ratio between the rates at which the two gears rotate
pub gear_ratio: f64,
/// In some cases, such as a differential drive, a third (auxiliary) link is used as reference pose.
pub gear_aux_link: Option<usize>,
/// the relative position target offset between two gears
pub relative_position_target: f64,
/// constraint error reduction parameter
pub erp: f64,
}
impl From<b3UserConstraint> for ConstraintInfo {
fn from(b3: b3UserConstraint) -> Self {
#[allow(non_snake_case)]
let b3UserConstraint {
m_parentBodyIndex,
m_parentJointIndex,
m_childBodyIndex,
m_childJointIndex,
m_parentFrame,
m_childFrame,
m_jointAxis,
m_jointType,
m_maxAppliedForce,
m_userConstraintUniqueId,
m_gearRatio,
m_gearAuxLink,
m_relativePositionTarget,
m_erp,
} = b3;
let parent_joint_index = {
if m_parentJointIndex >= 0 {
Some(m_parentJointIndex as usize)
} else {
None
}
};
let child_link_index = {
if m_childJointIndex >= 0 {
Some(m_childJointIndex as usize)
} else {
None
}
};
let gear_aux_link = {
if m_gearAuxLink >= 0 {
Some(m_gearAuxLink as usize)
} else {
None
}
};
let child_body = {
if m_childBodyIndex >= 0 {
Some(BodyId(m_childBodyIndex))
} else {
None
}
};
ConstraintInfo {
id: ConstraintId(m_userConstraintUniqueId),
parent_body: BodyId(m_parentBodyIndex),
parent_link_index: parent_joint_index,
child_body,
child_link_index,
constraint_type: JointType::try_from(m_jointType).unwrap(),
joint_axis: m_jointAxis.into(),
joint_parent_frame_pose: combined_position_orientation_array_to_isometry(m_parentFrame),
joint_child_frame_pose: combined_position_orientation_array_to_isometry(m_childFrame),
max_applied_force: m_maxAppliedForce,
gear_ratio: m_gearRatio,
gear_aux_link,
relative_position_target: m_relativePositionTarget,
erp: m_erp,
}
}
}
bitflags::bitflags! {
pub struct ActivationState : i32 {
const ENABLE_SLEEPING = 1;
const DISABLE_SLEEPING = 2;
const WAKE_UP = 4;
const SLEEP = 8;
const ENABLE_WAKEUP = 16;
const DISABLE_WAKEUP = 32;
}
}
/// Dynamics options for the [`change_dynamics`](`crate::PhysicsClient::`change_dynamics`) method.
/// Some options do not depend on the given link and apply to the whole body. These options are:
///
/// * `linear_damping`
/// * `angular_damping`
/// * `activation_state`
/// * `max_joint_velocity` - PyBullet claims that you can set it per joint, but that is not true
/// * `collision_margin`
#[derive(Default, Debug, Clone)]
pub struct ChangeDynamicsOptions {
/// change the mass of the link
pub mass: Option<f64>,
/// lateral (linear) contact friction
pub lateral_friction: Option<f64>,
/// torsional friction around the contact normal
pub spinning_friction: Option<f64>,
/// torsional friction orthogonal to contact normal (keep this value very close to zero,
/// otherwise the simulation can become very unrealistic
pub rolling_friction: Option<f64>,
/// bouncyness of contact. Keep it a bit less than 1, preferably closer to 0.
pub restitution: Option<f64>,
/// linear damping of the link (0.04 by default)
pub linear_damping: Option<f64>,
/// angular damping of the link (0.04 by default)
pub angular_damping: Option<f64>,
/// The contact stiffness and contact damping of the link encoded as tuple (contact_stiffness, contact_damping)
/// This overrides the value if it was specified in the URDF file in the contact section.
pub contact_stiffness_and_damping: Option<(f64, f64)>,
/// enable or disable a friction anchor: friction drift correction
/// (disabled by default, unless set in the URDF contact section)
pub friction_anchor: Option<bool>,
/// diagonal elements of the inertia tensor. Note that the base and links are centered around
/// the center of mass and aligned with the principal axes of inertia
/// so there are no off-diagonal elements in the inertia tensor.
pub local_inertia_diagonal: Option<Vector3<f64>>,
/// radius of the sphere to perform continuous collision detection.
pub ccd_swept_sphere_radius: Option<f64>,
/// contacts with a distance below this threshold will be processed by the constraint solver.
/// For example, if 0, then contacts with distance 0.01 will not be processed as a constraint
pub contact_processing_threshold: Option<f64>,
/// When sleeping is enabled, objects that don't move (below a threshold) will be disabled
/// as sleeping, if all other objects that influence it are also ready to sleep.
pub activation_state: Option<ActivationState>,
/// Joint damping coefficient applied at each joint. This coefficient is read from URDF joint damping field.
/// Keep the value close to 0.
/// Joint damping force = -damping_coefficient * joint_velocity
pub joint_damping: Option<f64>,
/// coefficient to allow scaling of friction in different directions.
pub anisotropic_friction: Option<f64>,
/// maximum joint velocity for the whole robot, if it is exceeded during constraint solving,
/// it is clamped. Default maximum joint velocity is 100 units.
pub max_joint_velocity: Option<f64>,
/// change the collision margin. dependent on the shape type, it may or may not add some padding to the collision shape.
pub collision_margin: Option<f64>,
/// changes the lower and upper limits of a joint. (lower_limit, upper_limit)
///
/// NOTE that at the moment, the joint limits are not updated in [`get_joint_info`](`crate::PhysicsClient::get_joint_info`)!
pub joint_limits: Option<(f64, f64)>,
/// change the maximum force applied to satisfy a joint limit.
pub joint_limit_force: Option<f64>,
}
/// Contains information about the mass, center of mass, friction and other properties of the base and links.
/// Is returned by [`get_dynamics_info`](`crate::PhysicsClient::get_dynamics_info`).
#[derive(Debug)]
pub struct DynamicsInfo {
/// mass in kg
pub mass: f64,
/// lateral (linear) contact friction
pub lateral_friction: f64,
/// spinning friction coefficient around contact normal
pub spinning_friction: f64,
/// rolling friction coefficient orthogonal to contact normal
pub rolling_friction: f64,
/// coefficient of restitution (bouncyness of contact).
pub restitution: f64,
/// The contact stiffness and contact damping of the link encoded as tuple (contact_stiffness, contact_damping).
/// Is `None` if not available
pub contact_stiffness_and_damping: Option<(f64, f64)>,
/// diagonal elements of the inertia tensor. Note that the base and links are centered around
/// the center of mass and aligned with the principal axes of inertia
/// so there are no off-diagonal elements in the inertia tensor.
pub local_inertia_diagonal: Vector3<f64>,
/// of inertial frame in local coordinates of the joint frame
pub local_inertial_pose: Isometry3<f64>,
/// body type of the object
pub body_type: BodyType,
/// collision margin of the collision shape. collision margins depend on the shape type, it is not consistent.
pub collision_margin: f64,
}
#[derive(Debug, PartialOrd, PartialEq)]
pub enum BodyType {
RigidBody = 1,
MultiBody = 2,
SoftBody = 3,
}
impl From<b3DynamicsInfo> for DynamicsInfo {
fn from(b3: b3DynamicsInfo) -> Self {
#[allow(unused, non_snake_case)]
let b3DynamicsInfo {
m_mass,
m_localInertialDiagonal,
m_localInertialFrame,
m_lateralFrictionCoeff,
m_rollingFrictionCoeff,
m_spinningFrictionCoeff,
m_restitution,
m_contactStiffness,
m_contactDamping,
m_activationState,
m_bodyType,
m_angularDamping,
m_linearDamping,
m_ccdSweptSphereRadius,
m_contactProcessingThreshold,
m_frictionAnchor,
m_collisionMargin,
m_dynamicType,
} = b3;
let contact_stiffness_and_damping = {
if m_contactStiffness <= 0. || m_contactDamping <= 0. {
None
} else {
Some((m_contactStiffness, m_contactDamping))
}
};
DynamicsInfo {
mass: m_mass,
lateral_friction: m_lateralFrictionCoeff,
spinning_friction: m_spinningFrictionCoeff,
rolling_friction: m_rollingFrictionCoeff,
restitution: m_restitution,
contact_stiffness_and_damping,
local_inertia_diagonal: m_localInertialDiagonal.into(),
local_inertial_pose: combined_position_orientation_array_to_isometry(
m_localInertialFrame,
),
body_type: match m_bodyType {
1 => BodyType::RigidBody,
2 => BodyType::MultiBody,
3 => BodyType::SoftBody,
_ => panic!("internal error: Unknown BodyType ({})", m_bodyType),
},
collision_margin: m_collisionMargin,
}
}
}
/// axis-aligned minimum bounding box
#[derive(Debug)]
pub struct Aabb {
/// minimum coordinates of the aabb
pub min: Vector3<f64>,
/// maximum coordinates of the aabb
pub max: Vector3<f64>,
}
/// Is the result of [`get_overlapping_objects`](`crate::PhysicsClient::get_overlapping_objects`).
/// Each object specifies a link of a body.
#[derive(Debug, Copy, Clone)]
pub struct OverlappingObject {
/// BodyID of the overlapping object
pub body: BodyId,
/// the index of the link which is overlapping. Is `None` for the base.
pub link_index: Option<usize>,
}
/// Is the result of the get_closest_points and get_contact_points methods.
#[derive(Debug, Copy, Clone)]
pub struct ContactPoint {
/// reserved
#[doc(hidden)]
pub contact_flag: i32,
/// body unique id of body A. Is `None` When a collision shape was used instead
pub body_a: Option<BodyId>,
/// body unique id of body B. Is `None` When a collision shape was used instead
pub body_b: Option<BodyId>,
/// link index of body A, `None` for base
pub link_index_a: Option<usize>,
/// link index of body A, `None` for base
pub link_index_b: Option<usize>,
/// contact position on A, in Cartesian world coordinates
pub position_on_a: Vector3<f64>,
/// contact position on B, in Cartesian world coordinates
pub position_on_b: Vector3<f64>,
/// contact normal on B, pointing towards A
pub contact_normal_on_b: Vector3<f64>,
/// contact distance, positive for separation, negative for penetration
pub contact_distance: f64,
/// normal force applied during the last 'stepSimulation'. Is `None` when used with one of the
/// get_closes_points methods
pub normal_force: Option<f64>,
/// first lateral friction
pub lateral_friction_1: Vector3<f64>,
/// second lateral friction
pub lateral_friction_2: Vector3<f64>,
}
impl From<b3ContactPointData> for ContactPoint {
fn from(b3: b3ContactPointData) -> Self {
#[allow(non_snake_case)]
let b3ContactPointData {
m_contactFlags,
m_bodyUniqueIdA,
m_bodyUniqueIdB,
m_linkIndexA,
m_linkIndexB,
m_positionOnAInWS,
m_positionOnBInWS,
m_contactNormalOnBInWS,
m_contactDistance,
m_normalForce,
m_linearFrictionForce1,
m_linearFrictionForce2,
m_linearFrictionDirection1,
m_linearFrictionDirection2,
} = b3;
let mut lateral_friction_1: Vector3<f64> = m_linearFrictionDirection1.into();
lateral_friction_1 *= m_linearFrictionForce1;
let mut lateral_friction_2: Vector3<f64> = m_linearFrictionDirection2.into();
lateral_friction_2 *= m_linearFrictionForce2;
let link_index_a = {
if m_linkIndexA.is_negative() {
None
} else {
Some(m_linkIndexA as usize)
}
};
let link_index_b = {
if m_linkIndexB.is_negative() {
None
} else {
Some(m_linkIndexB as usize)
}
};
let body_a = {
if m_bodyUniqueIdA < 0 {
None
} else {
Some(BodyId(m_bodyUniqueIdA))
}
};
let body_b = {
if m_bodyUniqueIdB < 0 {
None
} else {
Some(BodyId(m_bodyUniqueIdB))
}
};
ContactPoint {
contact_flag: m_contactFlags,
body_a,
body_b,
link_index_a,
link_index_b,
position_on_a: m_positionOnAInWS.into(),
position_on_b: m_positionOnBInWS.into(),
contact_normal_on_b: m_contactNormalOnBInWS.into(),
contact_distance: m_contactDistance,
normal_force: Some(m_normalForce),
lateral_friction_1,
lateral_friction_2,
}
}
}
pub enum LoggingType {
/// This will require to load the quadruped/quadruped.urdf and object unique
/// id from the quadruped. It logs the timestamp, IMU roll/pitch/yaw, 8 leg
/// motor positions (q0-q7), 8 leg motor torques (u0-u7), the forward speed of the
/// torso and mode (unused in simulation).
Minitaur = 0,
/// This will log a log of the data of either all objects or selected ones
/// (if [`object_ids`](`crate::types::StateLoggingOptions::object_ids`) in the
/// [`StateLoggingOptions`](`crate::types::StateLoggingOptions`) is not empty).
GenericRobot,
VrControllers,
/// this will open an MP4 file and start streaming the OpenGL 3D visualizer pixels to the file
/// using an ffmpeg pipe. It will require ffmpeg installed. You can also use
/// avconv (default on Ubuntu), just create a symbolic link so that ffmpeg points to avconv.
/// On Windows, ffmpeg has some issues that cause tearing/color artifacts in some cases.
VideoMp4,
Commands,
ContactPoints,
/// This will dump a timings file in JSON format that can be opened using Google Chrome about://tracing LOAD.
ProfileTimings,
AllCommands,
ReplayAllCommands,
CustomTimer,
}
#[derive(Debug, Default)]
pub struct StateLoggingOptions {
/// If left empty, the logger may log every object, otherwise the logger just logs the objects in the list.
pub object_ids: Vec<BodyId>,
/// Maximum number of joint degrees of freedom to log (excluding the base dofs).#
/// This applies to [`GenericRobot`](`crate::types::LoggingType::GenericRobot`)
/// Default value is 12. If a robot exceeds the number of dofs, it won't get logged at all.
pub max_log_dof: Option<usize>,
/// Applies to [`ContactPoints`](`crate::types::LoggingType::ContactPoints`).
/// If provided, only log contact points involving body_a.
pub body_a: Option<BodyId>,
/// Applies to [`ContactPoints`](`crate::types::LoggingType::ContactPoints`).
/// If provided, only log contact points involving link_index_a for body_a. Use `Some(None)` to
/// specify the base.
pub link_index_a: Option<Option<usize>>,
/// Applies to [`ContactPoints`](`crate::types::LoggingType::ContactPoints`).
/// If provided,only log contact points involving bodyUniqueIdB.
pub body_b: Option<BodyId>,
/// Applies to [`ContactPoints`](`crate::types::LoggingType::ContactPoints`).
/// If provided, only log contact points involving link_index_b for body_b. Use `Some(None)` to
/// specify the base.
pub link_index_b: Option<Option<usize>>,
#[doc(hidden)]
pub device_type_filter: Option<i32>,
/// Use JOINT_TORQUES to also log joint torques due to joint motors.
pub log_flags: Option<LogFlags>,
}
bitflags::bitflags! {
pub struct LogFlags : i32 {
const JOINT_MOTOR_TORQUES = 1;
const JOINT_USER_TORQUES = 2;
const JOINT_TORQUES = 3;
}
}
/// Options for the [`set_physics_engine_parameter`](`crate::PhysicsClient::set_physics_engine_parameter`) method.
#[derive(Default, Debug)]
pub struct SetPhysicsEngineParameterOptions {
/// See the warning in the [`set_time_step`](`crate::PhysicsClient::set_time_step`) section.
/// physics engine time step,
/// each time you call [`step_simulation`](`crate::PhysicsClient::step_simulation`) simulated
/// time will progress this amount. Same as [`set_time_step`](`crate::PhysicsClient::set_time_step`)
pub fixed_time_step: Option<Duration>,
///Choose the maximum number of constraint solver iterations.
/// If the solver_residual_threshold is reached,
/// the solver may terminate before the num_solver_iterations.
pub num_solver_iterations: Option<usize>,
/// Advanced feature, only when using maximal coordinates: split the positional
/// constraint solving and velocity constraint solving in two stages,
/// to prevent huge penetration recovery forces.
pub use_split_impulse: Option<bool>,
/// Related to `use_split_impulse`: if the penetration for a particular contact constraint is
/// less than this specified threshold, no split impulse will happen for that contact.
pub split_impulse_penetration_threshold: Option<f64>,
/// Subdivide the physics simulation step further by `num_sub_steps`.
/// This will trade performance over accuracy.
pub num_sub_steps: Option<usize>,
/// Use 0 for default collision filter: (group A&maskB) AND (groupB&maskA).
/// Use 1 to switch to the OR collision filter: (group A&maskB) OR (groupB&maskA)
pub collision_filter_mode: Option<usize>,
/// Contact points with distance exceeding this threshold are not processed by the LCP solver.
/// In addition, AABBs are extended by this number. Defaults to 0.02 in Bullet 2.x.
pub contact_breaking_threshold: Option<f64>,
/// Experimental: add 1ms sleep if the number of commands executed exceed this threshold.
/// setting the value to `-1` disables the feature.
pub max_num_cmd_per_1_ms: Option<i32>,
/// Set to `false` to disable file caching, such as .obj wavefront file loading
pub enable_file_caching: Option<bool>,
/// If relative velocity is below this threshold, restitution will be zero.
pub restitution_velocity_threshold: Option<f64>,
/// constraint error reduction parameter (non-contact, non-friction)
pub erp: Option<f64>,
/// contact error reduction parameter
pub contact_erp: Option<f64>,
/// friction error reduction parameter (when positional friction anchors are enabled)
pub friction_erp: Option<f64>,
/// Set to `false` to disable implicit cone friction and use pyramid approximation (cone is default).
/// NOTE: Although enabled by default, it is worth trying to disable this feature, in case there are friction artifacts.
pub enable_cone_friction: Option<bool>,
/// enables or disables sorting of overlapping pairs (backward compatibility setting).
pub deterministic_overlapping_pairs: Option<bool>,
/// If continuous collision detection (CCD) is enabled, CCD will not be used if the
/// penetration is below this threshold.
pub allowed_ccd_penetration: Option<f64>,
/// Specifcy joint feedback frame
pub joint_feedback_mode: Option<JointFeedbackMode>,
/// velocity threshold, if the maximum velocity-level error for each constraint is below this
/// threshold the solver will terminate (unless the solver hits the numSolverIterations).
/// Default value is 1e-7
pub solver_residual_threshold: Option<f64>,
/// Position correction of contacts is not resolved below this threshold,
/// to allow more stable contact.
pub contact_slop: Option<f64>,
/// if true, enable separating axis theorem based convex collision detection,
/// if features are available (instead of using GJK and EPA).
/// Requires [`URDF_INITIALIZE_SAT_FEATURES`](`LoadModelFlags::URDF_INITIALIZE_SAT_FEATURES`) in
/// the [`UrdfOptions`](`UrdfOptions`) in [`load_urdf`](`crate::PhysicsClient::load_urdf`).
pub enable_sat: Option<bool>,
/// Experimental (best to ignore): allow to use a direct LCP solver, such as Dantzig.
pub constraint_solver_type: Option<ConstraintSolverType>,
/// Experimental (best to ignore) global default constraint force mixing parameter.
pub global_cfm: Option<f64>,
/// Experimental (best to ignore), minimum size of constraint solving islands,
/// to avoid very small islands of independent constraints.
pub minimum_solver_island_size: Option<usize>,
/// when true, additional solve analytics is available.
pub report_solver_analytics: Option<bool>,
/// fraction of previous-frame force/impulse that is used to initialize the initial solver solution
pub warm_starting_factor: Option<f64>,
pub sparse_sdf_voxel_size: Option<f64>,
pub num_non_contact_inner_iterations: Option<usize>,
}
#[derive(Debug, PartialOrd, PartialEq)]
pub enum ConstraintSolverType {
None,
Si = 1,
Pgs,
Dantzig,
Lemke,
Nncg,
BlockPgs,
}
/// Specifies joint feedback frame. Is used in
/// [`SetPhysicsEngineParameterOptions::joint_feedback_mode`](`SetPhysicsEngineParameterOptions::joint_feedback_mode`)
#[derive(Debug, PartialOrd, PartialEq)]
pub enum JointFeedbackMode {
None,
/// gets the joint feedback in world space
WorldSpace = 1,
/// gets the joint feedback in the joint frame
JointFrame,
}
///
/// See [`SetPhysicsEngineParameterOptions`](`SetPhysicsEngineParameterOptions`) for a description of the parameters.
#[derive(Debug)]
pub struct PhysicsEngineParameters {
pub fixed_time_step: Duration,
pub simulation_time_stamp: Duration,
pub num_solver_iterations: usize,
pub use_split_impulse: bool,
pub split_impulse_penetration_threshold: f64,
pub num_sub_steps: usize,
pub collision_filter_mode: usize,
pub contact_breaking_threshold: f64,
pub enable_file_caching: bool,
pub restitution_velocity_threshold: f64,
pub erp: f64,
pub contact_erp: f64,
pub friction_erp: f64,
pub enable_cone_friction: bool,
pub deterministic_overlapping_pairs: bool,
pub allowed_ccd_penetration: f64,
pub joint_feedback_mode: JointFeedbackMode,
pub solver_residual_threshold: f64,
pub contact_slop: f64,
pub enable_sat: bool,
pub constraint_solver_type: ConstraintSolverType,
pub global_cfm: f64,
pub minimum_solver_island_size: usize,
pub report_solver_analytics: bool,
pub warm_starting_factor: f64,
pub sparse_sdf_voxel_size: f64,
pub num_non_contact_inner_iterations: usize,
pub use_real_time_simulation: bool,
pub gravity: Vector3<f64>,
pub articulated_warm_starting_factor: f64,
pub internal_sim_flags: i32,
pub friction_cfm: f64,
}
fn int_to_bool(int: i32) -> bool {
match int {
0 => false,
1 => true,
_ => panic!("could not convert \"{}\" to boolean", int),
}
}
impl From<b3PhysicsSimulationParameters> for PhysicsEngineParameters {
fn from(b3: b3PhysicsSimulationParameters) -> Self {
#[allow(non_snake_case)]
let b3PhysicsSimulationParameters {
m_deltaTime,
m_simulationTimestamp,
m_gravityAcceleration,
m_numSimulationSubSteps,
m_numSolverIterations,
m_warmStartingFactor,
m_articulatedWarmStartingFactor,
m_useRealTimeSimulation,
m_useSplitImpulse,
m_splitImpulsePenetrationThreshold,
m_contactBreakingThreshold,
m_internalSimFlags,
m_defaultContactERP,
m_collisionFilterMode,
m_enableFileCaching,
m_restitutionVelocityThreshold,
m_defaultNonContactERP,
m_frictionERP,
m_defaultGlobalCFM,
m_frictionCFM,
m_enableConeFriction,
m_deterministicOverlappingPairs,
m_allowedCcdPenetration,
m_jointFeedbackMode,
m_solverResidualThreshold,
m_contactSlop,
m_enableSAT,
m_constraintSolverType,
m_minimumSolverIslandSize,
m_reportSolverAnalytics,
m_sparseSdfVoxelSize,
m_numNonContactInnerIterations,
} = b3;
let joint_feedback_mode = {
match m_jointFeedbackMode {
0 => JointFeedbackMode::None,
1 => JointFeedbackMode::WorldSpace,
2 => JointFeedbackMode::JointFrame,
n => panic!("Unexpected JointFeedbackMode \"{}\"", n),
}
};
let constraint_solver_type = {
match m_constraintSolverType {
0 => ConstraintSolverType::None,
1 => ConstraintSolverType::Si,
2 => ConstraintSolverType::Pgs,
3 => ConstraintSolverType::Dantzig,
4 => ConstraintSolverType::Lemke,
5 => ConstraintSolverType::Nncg,
6 => ConstraintSolverType::BlockPgs,
n => panic!("Unexpected ConstraintSolverType \"{}\"", n),
}
};
PhysicsEngineParameters {
fixed_time_step: Duration::from_secs_f64(m_deltaTime),
simulation_time_stamp: Duration::from_secs_f64(m_simulationTimestamp),
num_solver_iterations: m_numSolverIterations as usize,
use_split_impulse: int_to_bool(m_useSplitImpulse),
split_impulse_penetration_threshold: m_splitImpulsePenetrationThreshold,
num_sub_steps: m_numSimulationSubSteps as usize,
collision_filter_mode: m_collisionFilterMode as usize,
contact_breaking_threshold: m_contactBreakingThreshold,
enable_file_caching: int_to_bool(m_enableFileCaching),
restitution_velocity_threshold: m_restitutionVelocityThreshold,
erp: m_defaultNonContactERP,
contact_erp: m_defaultContactERP,
friction_erp: m_frictionERP,
enable_cone_friction: int_to_bool(m_enableConeFriction),
deterministic_overlapping_pairs: int_to_bool(m_deterministicOverlappingPairs),
allowed_ccd_penetration: m_allowedCcdPenetration,
joint_feedback_mode,
solver_residual_threshold: m_solverResidualThreshold,
contact_slop: m_contactSlop,
enable_sat: int_to_bool(m_enableSAT),
constraint_solver_type,
global_cfm: m_defaultGlobalCFM,
minimum_solver_island_size: m_minimumSolverIslandSize as usize,
report_solver_analytics: int_to_bool(m_reportSolverAnalytics),
warm_starting_factor: m_warmStartingFactor,
sparse_sdf_voxel_size: m_sparseSdfVoxelSize,
num_non_contact_inner_iterations: m_numNonContactInnerIterations as usize,
use_real_time_simulation: int_to_bool(m_useRealTimeSimulation),
gravity: m_gravityAcceleration.into(),
articulated_warm_starting_factor: m_articulatedWarmStartingFactor,
internal_sim_flags: m_internalSimFlags,
friction_cfm: m_frictionCFM,
}
}
}
/// Contains the state of the Gui camera.
/// Is returned by [`get_debug_visualizer_camera`](`crate::PhysicsClient::get_debug_visualizer_camera`).
#[derive(Default, Debug)]
pub struct DebugVisualizerCameraInfo {
/// width of the camera image in pixels
pub width: usize,
/// height of the camera image in pixels
pub height: usize,
/// view matrix of the camera
pub view_matrix: Matrix4<f32>,
/// projection matrix of the camera
pub projection_matrix: Matrix4<f32>,
/// up axis of the camera, in Cartesian world space coordinates
pub camera_up: Vector3<f32>,
/// forward axis of the camera, in Cartesian world space coordinates
pub camera_forward: Vector3<f32>,
/// This is a horizontal vector that can be used to generate rays (for mouse picking or creating a simple ray tracer for example)
pub horizontal: Vector3<f32>,
/// This is a vertical vector that can be used to generate rays(for mouse picking or creating a simple ray tracer for example).
pub vertical: Vector3<f32>,
/// yaw angle of the camera (in degree), in Cartesian local space coordinates
pub yaw: f32,
/// pitch angle of the camera (in degree), in Cartesian local space coordinates
pub pitch: f32,
/// distance between the camera and the camera target
pub dist: f32,
/// target of the camera, in Cartesian world space coordinates
pub target: Vector3<f32>,
}
impl From<b3OpenGLVisualizerCameraInfo> for DebugVisualizerCameraInfo {
fn from(b3: b3OpenGLVisualizerCameraInfo) -> Self {
#[allow(non_snake_case)]
let b3OpenGLVisualizerCameraInfo {
m_width,
m_height,
m_viewMatrix,
m_projectionMatrix,
m_camUp,
m_camForward,
m_horizontal,
m_vertical,
m_yaw,
m_pitch,
m_dist,
m_target,
} = b3;
DebugVisualizerCameraInfo {
width: m_width as usize,
height: m_height as usize,
view_matrix: Matrix4::from_column_slice(&m_viewMatrix),
projection_matrix: Matrix4::from_column_slice(&m_projectionMatrix),
camera_up: m_camUp.into(),
camera_forward: m_camForward.into(),
horizontal: m_horizontal.into(),
vertical: m_vertical.into(),
yaw: m_yaw,
pitch: m_pitch,
dist: m_dist,
target: m_target.into(),
}
}
}
/// Options for [`ray_test`](`crate::PhysicsClient::ray_test`)
#[derive(Default, Debug)]
pub struct RayTestOptions {
/// instead of first closest hit, you can report the n-th hit
pub report_hit_number: Option<usize>,
/// only test hits if the bitwise and between collisionFilterMask and body collision
/// filter group is non-zero. See
/// set_collision_filter_group_mask on how to modify the body filter mask/group.
pub collision_filter_mask: Option<i32>,
}
/// Options for [`ray_test_batch`](`crate::PhysicsClient::ray_test_batch`)
#[derive(Default, Debug)]
pub struct RayTestBatchOptions {
/// ray from/to is in local space of a parent object
pub parent_object_id: Option<BodyId>,
/// ray from/to is in local space of a link.
pub parent_link_index: Option<usize>,
/// use multiple threads to compute ray tests
/// (0 = use all threads available, positive number = exactly this amoung of threads,
/// default = None = single-threaded)
pub num_threads: Option<usize>,
/// instead of first closest hit, you can report the n-th hit
pub report_hit_number: Option<usize>,
/// only useful when using report_hit_number: ignore duplicate hits if the fraction is
/// similar to an existing hit within this fractionEpsilon when hitting the same body.
/// For example, a ray may hit many co-planar triangles of one body,
/// you may only be interested in one of those hits.
pub fraction_epsilon: Option<f64>,
/// only test hits if the bitwise and between collisionFilterMask and body collision
/// filter group is non-zero. See
/// set_collision_filter_group_mask on how to modify the body filter mask/group.
pub collision_filter_mask: Option<i32>,
}
#[derive(Debug, Copy, Clone)]
pub struct RayHitInfo {
pub body_id: BodyId,
pub link_index: Option<usize>,
pub hit_fraction: f64,
pub hit_position: Vector3<f64>,
pub hit_normal: Vector3<f64>,
}
impl RayHitInfo {
pub fn new(ray: b3RayHitInfo) -> Option<Self> {
let link_index = {
assert!(ray.m_hitObjectLinkIndex >= -1);
if ray.m_hitObjectLinkIndex == -1 {
None
} else {
Some(ray.m_hitObjectLinkIndex as usize)
}
};
if ray.m_hitObjectUniqueId < 0 {
None
} else {
Some(RayHitInfo {
body_id: BodyId(ray.m_hitObjectUniqueId),
link_index,
hit_fraction: ray.m_hitFraction,
hit_position: ray.m_hitPositionWorld.into(),
hit_normal: ray.m_hitNormalWorld.into(),
})
}
}
}
/// options for [`load_soft_body`](`crate::PhysicsClient::load_soft_body`)
#[derive(Debug)]
pub struct SoftBodyOptions {
/// initial pose of the deformable object
pub base_pose: Isometry3<f64>,
/// scaling factor to resize the deformable (default = 1)
pub scale: Option<f64>,
/// total mass of the deformable, the mass is equally distributed among all vertices
pub mass: Option<f64>,
/// a collision margin extends the deformable, it can help avoiding penetrations, especially for thin (cloth) deformables
pub collision_margin: Option<f64>,
/// using mass spring
pub use_mass_spring: bool,
/// create bending springs to control bending of deformables
pub use_bending_springs: bool,
/// enable the Neo Hookean simulation
pub use_neo_hookean: bool,
/// stiffness parameter
pub spring_elastic_stiffness: f64,
/// damping parameter
pub spring_damping_stiffness: f64,
/// spring damping parameter
pub spring_damping_all_directions: bool,
/// parameters of bending stiffness
pub spring_bending_stiffness: f64,
/// parameters of the Neo Hookean model
pub neo_hookean_mu: f64,
/// parameters of the Neo Hookean model
pub neo_hookean_lambda: f64,
/// parameters of the Neo Hookean model
pub neo_hookean_damping: f64,
/// contact friction for deformables
pub friction_coeff: f64,
/// enable collisions internal to faces, not just at vertices.
pub use_face_contact: bool,
/// enable self collision for a deformable
pub use_self_collision: bool,
/// a parameter that helps avoiding penetration.
pub repulsion_stiffness: Option<f64>,
pub sim_filename: Option<PathBuf>,
}
impl Default for SoftBodyOptions {
fn default() -> Self {
SoftBodyOptions {
base_pose: Isometry3::identity(),
scale: None,
mass: None,
collision_margin: None,
use_mass_spring: false,
use_bending_springs: false,
use_neo_hookean: false,
spring_elastic_stiffness: 1.,
spring_damping_stiffness: 0.1,
spring_damping_all_directions: false,
spring_bending_stiffness: 0.1,
neo_hookean_mu: 1.,
neo_hookean_lambda: 1.,
neo_hookean_damping: 0.1,
friction_coeff: 0.,
use_face_contact: false,
use_self_collision: false,
repulsion_stiffness: None,
sim_filename: None,
}
}
}
bitflags::bitflags! {
/// Experimental flags, best to ignore.
pub struct ResetFlags : i32 {
const DEFORMABLE_WORLD = 1;
const DISCRETE_DYNAMICS_WORLD = 2;
const SIMPLE_BROADPHASE = 4;
}
}
bitflags::bitflags! {
/// Experimental flags, best to ignore.
pub struct VisualShapeFlags : i32 {
const TEXTURE_UNIQUE_IDS = 1;
const DOUBLE_SIDED = 4;
}
}
bitflags::bitflags! {
/// flags for camera rendering
pub struct RendererAuxFlags : i32 {
/// if used the pixels of the segmentation mask are calculated with this formula:
/// bodyId + (linkIndex+1)<<24
const SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1;
const USE_PROJECTIVE_TEXTURE = 2;
/// avoids calculating the segmentation mask
const NO_SEGMENTATION_MASK = 4;
}
}
#[derive(Debug)]
pub enum Renderer {
TinyRenderer = 1 << 16,
/// Direct mode has no OpenGL, so you can not use this setting in direct mode.
BulletHardwareOpenGl = 1 << 17,
}
/// Options for [`get_camera_image`](`crate::PhysicsClient::get_camera_image`)
#[derive(Debug, Default)]
pub struct CameraImageOptions {
/// view matrix, see [compute_view_matrix](`crate::PhysicsClient::compute_view_matrix`)
pub view_matrix: Option<Matrix4<f32>>,
/// projection matrix, see [compute_projection_matrix](`crate::PhysicsClient::compute_projection_matrix`)
pub projection_matrix: Option<Matrix4<f32>>,
/// specifies the world position of the light source, the direction is from the light source position to the origin of the world frame.
pub light_direction: Option<Vector3<f32>>,
/// directional light color in \[RED,GREEN,BLUE\] in range 0..1, only applies to [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`)
pub light_color: Option<[f32; 3]>,
/// distance of the light along the normalized lightDirection, only applies to [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`)
pub light_distance: Option<f32>,
/// enables disables shadows, only applies to [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`)
pub shadow: Option<bool>,
/// light ambient coefficient, only applies to [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`)
pub light_ambient_coeff: Option<f32>,
/// light diffuse coefficient, only applies to [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`)
pub light_diffuse_coeff: Option<f32>,
/// light specular coefficient, only applies to [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`)
pub light_specular_coeff: Option<f32>,
/// Note that Direct mode has no OpenGL, so it requires [`Renderer::TinyRenderer`](`Renderer::TinyRenderer`).
pub renderer: Option<Renderer>,
/// additional rendering flags
pub flags: Option<RendererAuxFlags>,
pub projective_texture_view: Option<Matrix4<f32>>,
pub projective_texture_proj: Option<Matrix4<f32>>,
}