wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update simulators: played with mujoco

Browse Source
This commit is contained in:
Brian Delhaisse
2019-07-13 01:29:06 +02:00
parent b7bf9a0d73
commit cd2dc52d30
6 changed files with 939 additions and 395 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/filters/utils.py
+60
View File
@@ -4,6 +4,7 @@
These filters can be useful to smooth signal trajectories.
"""
import numpy as np
from scipy.signal import butter, lfilter
@@ -32,3 +33,62 @@ def butter_bandpass_filter(data, lowcut, highcut, fs, order=5):
b, a = butter_bandpass(lowcut, highcut, fs, order=order)
y = lfilter(b, a, data)
return y
# Taken from https://scipy-cookbook.readthedocs.io/items/SignalSmooth.html
def smooth(x, window_len=11, window='hanning'):
"""smooth the data using a window with requested size.
This method is based on the convolution of a scaled window with the signal.
The signal is prepared by introducing reflected copies of the signal
(with the window size) in both ends so that transient parts are minimized
in the begining and end part of the output signal.
Args:
x: the input signal
window_len: the dimension of the smoothing window; should be an odd integer (>=3)
window: the type of window from 'flat', 'hanning', 'hamming', 'bartlett', 'blackman' flat window will produce
a moving average smoothing.
Returns:
the smoothed signal
example:
t = linspace(-2, 2, 0.1)
x = sin(t) + randn(len(t)) * 0.1
y = smooth(x)
see also:
numpy.hanning, numpy.hamming, numpy.bartlett, numpy.blackman, numpy.convolve, scipy.signal.lfilter
TODO: the window parameter could be the window itself if an array instead of a string
NOTE: length(output) != length(input), to correct this: return y[(window_len/2-1):-(window_len/2)] instead of just
y.
References:
- https://scipy-cookbook.readthedocs.io/items/SignalSmooth.html
"""
if x.ndim != 1:
raise ValueError("smooth only accepts 1 dimension arrays.")
if x.size < window_len:
raise ValueError("Input vector needs to be bigger than window size.")
if window_len < 3:
return x
if not window in set(['flat', 'hanning', 'hamming', 'bartlett', 'blackman']):
raise ValueError("Window is on of 'flat', 'hanning', 'hamming', 'bartlett', 'blackman'")
s = np.r_[x[window_len - 1:0:-1], x, x[-2:-window_len - 1:-1]]
if window == 'flat': # moving average
w = np.ones(window_len, 'd')
else:
w = eval('np.' + window + '(window_len)')
y = np.convolve(w / w.sum(), s, mode='valid')
return y
pyrobolearn/simulators/bullet.py
+214 -199
View File
@@ -235,6 +235,16 @@ class Bullet(Simulator):
@staticmethod
def simulate_soft_bodies():
"""Return True if the simulator can simulate soft bodies."""
# For the moment, this feature is not well supported in PyBullet
# You can check Jan Matas's work for soft bodies:
# - https://github.com/JanMatas/bullet3
# - https://www.imperial.ac.uk/media/imperial-college/faculty-of-engineering/computing/public/1718-ug-projects\
# /Jan-Matas-Learning-end-to-end-robotic-manipulation-of-deformable-objects.pdf
return False
@staticmethod
def supports_dynamic_loading():
"""Return True if the simulator supports the dynamic loading of models."""
return True
###########
@@ -946,8 +956,8 @@ class Bullet(Simulator):
collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape
for multiple multibodies (instancing)
mass (float): mass of the base, in kg (if using SI units)
position (np.float[3]): Cartesian world position of the base
orientation (np.float[4]): Orientation of base as quaternion [x,y,z,w]
position (np.array[3]): Cartesian world position of the base
orientation (np.array[4]): Orientation of base as quaternion [x,y,z,w]
Returns:
int: non-negative unique id or -1 for failure.
@@ -1028,14 +1038,19 @@ class Bullet(Simulator):
coordinates)
child_link_id (int): child link index, or -1 for the base
joint_type (int): joint type: JOINT_PRISMATIC (=1), JOINT_FIXED (=4), JOINT_POINT2POINT (=5),
JOINT_GEAR (=6)
joint_axis (np.float[3]): joint axis, in child link frame
parent_frame_position (np.float[3]): position of the joint frame relative to parent CoM frame.
child_frame_position (np.float[3]): position of the joint frame relative to a given child CoM frame (or
JOINT_GEAR (=6). If the JOINT_FIXED is set, the child body's link will not move with respect to the
parent body's link. If the JOINT_PRISMATIC is set, the child body's link will only be able to move
along the given joint axis with respect to the parent body's link. If the JOINT_POINT2POINT is set
(which should really be called spherical), the child body's link will be able to rotate along the 3
axis while maintaining the given position relative to the parent body's link. If the JOINT_GEAR can be
set between two links of the same body.
joint_axis (np.array[3]): joint axis, in child link frame
parent_frame_position (np.array[3]): position of the joint frame relative to parent CoM frame.
child_frame_position (np.array[3]): position of the joint frame relative to a given child CoM frame (or
world origin if no child specified)
parent_frame_orientation (np.float[4]): the orientation of the joint frame relative to parent CoM
parent_frame_orientation (np.array[4]): the orientation of the joint frame relative to parent CoM
coordinate frame
child_frame_orientation (np.float[4]): the orientation of the joint frame relative to the child CoM
child_frame_orientation (np.array[4]): the orientation of the joint frame relative to the child CoM
coordinate frame (or world origin frame if no child specified)
Examples:
@@ -1066,9 +1081,9 @@ class Bullet(Simulator):
Args:
constraint_id (int): constraint unique id.
child_joint_pivot (np.float[3]): updated position of the joint frame relative to a given child CoM frame
child_joint_pivot (np.array[3]): updated position of the joint frame relative to a given child CoM frame
(or world origin if no child specified)
child_frame_orientation (np.float[4]): updated child frame orientation as quaternion [x,y,z,w]
child_frame_orientation (np.array[4]): updated child frame orientation as quaternion [x,y,z,w]
max_force (float): maximum force that constraint can apply
gear_ratio (float): the ratio between the rates at which the two gears rotate
gear_auxiliary_link (int): In some cases, such as a differential drive, a third (auxilary) link is used as
@@ -1128,11 +1143,11 @@ class Bullet(Simulator):
int: child_body_id (if -1, no body; specify a non-dynamic child frame in world coordinates)
int: child_link_id (if -1, it is the base)
int: constraint/joint type
np.float[3]: joint axis
np.float[3]: joint pivot (position) in parent CoM frame
np.float[3]: joint pivot (position) in specified child CoM frame (or world frame if no specified child)
np.float[4]: joint frame orientation relative to parent CoM coordinate frame
np.float[4]: joint frame orientation relative to child CoM frame (or world frame if no specified child)
np.array[3]: joint axis
np.array[3]: joint pivot (position) in parent CoM frame
np.array[3]: joint pivot (position) in specified child CoM frame (or world frame if no specified child)
np.array[4]: joint frame orientation relative to parent CoM coordinate frame
np.array[4]: joint frame orientation relative to child CoM frame (or world frame if no specified child)
float: maximum force that constraint can apply
"""
return self.sim.getConstraintInfo(constraint_id)
@@ -1145,7 +1160,7 @@ class Bullet(Simulator):
constraint_id (int): constraint unique id.
Returns:
np.float[D]: applied constraint forces. Its dimension is the degrees of freedom that are affected by
np.array[D]: applied constraint forces. Its dimension is the degrees of freedom that are affected by
the constraint (a fixed constraint affects 6 DoF for example)
"""
return self.sim.getConstraintState(constraint_id)
@@ -1198,7 +1213,7 @@ class Bullet(Simulator):
of the specified body.
Returns:
np.float[3]: center of mass position in the Cartesian world coordinates
np.array[3]: center of mass position in the Cartesian world coordinates
"""
if link_ids is None:
link_ids = list(range(self.num_links(body_id)))
@@ -1219,7 +1234,7 @@ class Bullet(Simulator):
of the specified body.
Returns:
np.float[3]: center of mass linear velocity.
np.array[3]: center of mass linear velocity.
"""
if link_ids is None:
link_ids = list(range(self.num_links(body_id)))
@@ -1235,7 +1250,7 @@ class Bullet(Simulator):
Return the total linear momentum in the world space.
Returns:
np.float[3]: linear momentum
np.array[3]: linear momentum
"""
if link_ids is None:
link_ids = list(range(self.num_links(body_id)))
@@ -1251,8 +1266,8 @@ class Bullet(Simulator):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: base position
np.float[4]: base orientation (quaternion [x,y,z,w])
np.array[3]: base position
np.array[4]: base orientation (quaternion [x,y,z,w])
"""
pos, orientation = self.sim.getBasePositionAndOrientation(body_id)
return np.asarray(pos), np.asarray(orientation)
@@ -1265,7 +1280,7 @@ class Bullet(Simulator):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: base position.
np.array[3]: base position.
"""
return self.get_base_pose(body_id)[0]
@@ -1277,7 +1292,7 @@ class Bullet(Simulator):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[4]: base orientation in the form of a quaternion (x,y,z,w)
np.array[4]: base orientation in the form of a quaternion (x,y,z,w)
"""
return self.get_base_pose(body_id)[1]
@@ -1291,8 +1306,8 @@ class Bullet(Simulator):
Args:
body_id (int): unique object id.
position (np.float[3]): new base position.
orientation (np.float[4]): new base orientation (expressed as a quaternion [x,y,z,w])
position (np.array[3]): new base position.
orientation (np.array[4]): new base orientation (expressed as a quaternion [x,y,z,w])
"""
self.sim.resetBasePositionAndOrientation(body_id, position, orientation)
@@ -1302,7 +1317,7 @@ class Bullet(Simulator):
Args:
body_id (int): unique object id.
position (np.float[3]): new base position.
position (np.array[3]): new base position.
"""
orientation = self.get_base_orientation(body_id)
self.reset_base_pose(body_id, position, orientation)
@@ -1313,7 +1328,7 @@ class Bullet(Simulator):
Args:
body_id (int): unique object id.
orientation (np.float[4]): new base orientation (expressed as a quaternion [x,y,z,w])
orientation (np.array[4]): new base orientation (expressed as a quaternion [x,y,z,w])
"""
position = self.get_base_position(body_id)
self.reset_base_pose(body_id, position, orientation)
@@ -1326,8 +1341,8 @@ class Bullet(Simulator):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: linear velocity of the base in Cartesian world space coordinates
np.float[3]: angular velocity of the base in Cartesian world space coordinates
np.array[3]: linear velocity of the base in Cartesian world space coordinates
np.array[3]: angular velocity of the base in Cartesian world space coordinates
"""
lin_vel, ang_vel = self.sim.getBaseVelocity(body_id)
return np.asarray(lin_vel), np.asarray(ang_vel)
@@ -1340,7 +1355,7 @@ class Bullet(Simulator):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: linear velocity of the base in Cartesian world space coordinates
np.array[3]: linear velocity of the base in Cartesian world space coordinates
"""
return self.get_base_velocity(body_id)[0]
@@ -1352,7 +1367,7 @@ class Bullet(Simulator):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: angular velocity of the base in Cartesian world space coordinates
np.array[3]: angular velocity of the base in Cartesian world space coordinates
"""
return self.get_base_velocity(body_id)[1]
@@ -1362,8 +1377,8 @@ class Bullet(Simulator):
Args:
body_id (int): unique object id.
linear_velocity (np.float[3]): new linear velocity of the base.
angular_velocity (np.float[3]): new angular velocity of the base.
linear_velocity (np.array[3]): new linear velocity of the base.
angular_velocity (np.array[3]): new angular velocity of the base.
"""
if linear_velocity is not None and angular_velocity is not None:
self.sim.resetBaseVelocity(body_id, linearVelocity=linear_velocity, angularVelocity=angular_velocity)
@@ -1378,7 +1393,7 @@ class Bullet(Simulator):
Args:
body_id (int): unique object id.
linear_velocity (np.float[3]): new linear velocity of the base
linear_velocity (np.array[3]): new linear velocity of the base
"""
self.sim.resetBaseVelocity(body_id, linearVelocity=linear_velocity)
@@ -1388,7 +1403,7 @@ class Bullet(Simulator):
Args:
body_id (int): unique object id.
angular_velocity (np.float[3]): new angular velocity of the base
angular_velocity (np.array[3]): new angular velocity of the base
"""
self.sim.resetBaseVelocity(body_id, angularVelocity=angular_velocity)
@@ -1404,8 +1419,8 @@ class Bullet(Simulator):
Args:
body_id (int): unique body id.
link_id (int): unique link id. If -1, it will be the base.
force (np.float[3]): external force to be applied.
position (np.float[3], None): position on the link where the force is applied. See `flags` for coordinate
force (np.array[3]): external force to be applied.
position (np.array[3], None): position on the link where the force is applied. See `flags` for coordinate
systems. If None, it is the center of mass of the body (or the link if specified).
frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
@@ -1513,9 +1528,9 @@ class Bullet(Simulator):
[11] float: maximum velocity specified in URDF. Note that this value is not used in actual
motor control commands at the moment.
[12] str: name of the link (as specified in the URDF/SDF/etc file)
[13] np.float[3]: joint axis in local frame (ignored for JOINT_FIXED)
[14] np.float[3]: joint position in parent frame
[15] np.float[4]: joint orientation in parent frame
[13] np.array[3]: joint axis in local frame (ignored for JOINT_FIXED)
[14] np.array[3]: joint position in parent frame
[15] np.array[4]: joint orientation in parent frame
[16] int: parent link index, -1 for base
"""
info = list(self.sim.getJointInfo(body_id, joint_id))
@@ -1537,7 +1552,7 @@ class Bullet(Simulator):
Returns:
float: The position value of this joint.
float: The velocity value of this joint.
np.float[6]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is
np.array[6]: 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].
float: This is the motor torque applied during the last stepSimulation. Note that this only applies in
VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque
@@ -1558,7 +1573,7 @@ class Bullet(Simulator):
list:
float: The position value of this joint.
float: The velocity value of this joint.
np.float[6]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is
np.array[6]: 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].
float: This is the motor torque applied during the last `step`. Note that this only applies in
VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor
@@ -1629,8 +1644,8 @@ class Bullet(Simulator):
joint_ids ((list of) int): joint/link id, or list of joint ids.
control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD),
VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3).
positions (float, np.float[N]): target joint position(s) (used in POSITION_CONTROL).
velocities (float, np.float[N]): target joint velocity(ies). In VELOCITY_CONTROL and POSITION_CONTROL,
positions (float, np.array[N]): target joint position(s) (used in POSITION_CONTROL).
velocities (float, np.array[N]): target joint velocity(ies). In VELOCITY_CONTROL and POSITION_CONTROL,
the target velocity(ies) is(are) the desired velocity of the joint. Note that the target velocity(ies)
is(are) not the maximum joint velocity(ies). In PD_CONTROL and
POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using:
@@ -1688,15 +1703,15 @@ class Bullet(Simulator):
using forward kinematics.
Returns:
np.float[3]: Cartesian position of CoM
np.float[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.float[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.float[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
np.array[3]: Cartesian position of CoM
np.array[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.array[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
frame
np.float[3]: world position of the URDF link frame
np.float[4]: world orientation of the URDF link frame
np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
np.array[3]: world position of the URDF link frame
np.array[4]: world orientation of the URDF link frame
np.array[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.array[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
"""
results = self.sim.getLinkState(body_id, link_id, computeLinkVelocity=int(compute_velocity),
computeForwardKinematics=int(compute_forward_kinematics))
@@ -1715,15 +1730,15 @@ class Bullet(Simulator):
Returns:
list:
np.float[3]: Cartesian position of CoM
np.float[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.float[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.float[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF
np.array[3]: Cartesian position of CoM
np.array[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.array[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF
link frame
np.float[3]: world position of the URDF link frame
np.float[4]: world orientation of the URDF link frame
np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
np.array[3]: world position of the URDF link frame
np.array[4]: world orientation of the URDF link frame
np.array[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.array[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
"""
return [self.get_link_state(body_id, link_id, compute_velocity, compute_forward_kinematics)
for link_id in link_ids]
@@ -1790,9 +1805,9 @@ class Bullet(Simulator):
Returns:
if 1 link:
np.float[3]: the link CoM position in the world space
np.array[3]: the link CoM position in the world space
if multiple links:
np.float[N,3]: CoM position of each link in world space
np.array[N,3]: CoM position of each link in world space
"""
if isinstance(link_ids, int):
if link_ids == -1:
@@ -1819,9 +1834,9 @@ class Bullet(Simulator):
Returns:
if 1 link:
np.float[4]: Cartesian orientation of the link CoM (x,y,z,w)
np.array[4]: Cartesian orientation of the link CoM (x,y,z,w)
if multiple links:
np.float[N,4]: CoM orientation of each link (x,y,z,w)
np.array[N,4]: CoM orientation of each link (x,y,z,w)
"""
if isinstance(link_ids, int):
if link_ids == -1:
@@ -1848,9 +1863,9 @@ class Bullet(Simulator):
Returns:
if 1 link:
np.float[3]: linear velocity of the link in the Cartesian world space
np.array[3]: linear velocity of the link in the Cartesian world space
if multiple links:
np.float[N,3]: linear velocity of each link
np.array[N,3]: linear velocity of each link
"""
if isinstance(link_ids, int):
if link_ids == -1:
@@ -1874,9 +1889,9 @@ class Bullet(Simulator):
Returns:
if 1 link:
np.float[3]: angular velocity of the link in the Cartesian world space
np.array[3]: angular velocity of the link in the Cartesian world space
if multiple links:
np.float[N,3]: angular velocity of each link
np.array[N,3]: angular velocity of each link
"""
if isinstance(link_ids, int):
if link_ids == -1:
@@ -1901,9 +1916,9 @@ class Bullet(Simulator):
Returns:
if 1 link:
np.float[6]: linear and angular velocity of the link in the Cartesian world space
np.array[6]: linear and angular velocity of the link in the Cartesian world space
if multiple links:
np.float[N,6]: linear and angular velocity of each link
np.array[N,6]: linear and angular velocity of each link
"""
if isinstance(link_ids, int):
if link_ids == -1:
@@ -2035,7 +2050,7 @@ class Bullet(Simulator):
if 1 joint:
float: damping coefficient of the given joint
if multiple joints:
np.float[N]: damping coefficient for each specified joint
np.array[N]: damping coefficient for each specified joint
"""
if isinstance(joint_ids, int):
return self.sim.getJointInfo(body_id, joint_ids)[6]
@@ -2053,7 +2068,7 @@ class Bullet(Simulator):
if 1 joint:
float: friction coefficient of the given joint
if multiple joints:
np.float[N]: friction coefficient for each specified joint
np.array[N]: friction coefficient for each specified joint
"""
if isinstance(joint_ids, int):
return self.sim.getJointInfo(body_id, joint_ids)[7]
@@ -2069,9 +2084,9 @@ class Bullet(Simulator):
Returns:
if 1 joint:
np.float[2]: lower and upper limit
np.array[2]: lower and upper limit
if multiple joints:
np.float[N,2]: lower and upper limit for each specified joint
np.array[N,2]: lower and upper limit for each specified joint
"""
if isinstance(joint_ids, int):
return np.asarray(self.sim.getJointInfo(body_id, joint_ids)[8:10])
@@ -2091,7 +2106,7 @@ class Bullet(Simulator):
if 1 joint:
float: maximum force [N]
if multiple joints:
np.float[N]: maximum force for each specified joint [N]
np.array[N]: maximum force for each specified joint [N]
"""
if isinstance(joint_ids, int):
return self.sim.getJointInfo(body_id, joint_ids)[10]
@@ -2111,7 +2126,7 @@ class Bullet(Simulator):
if 1 joint:
float: maximum velocity [rad/s]
if multiple joints:
np.float[N]: maximum velocities for each specified joint [rad/s]
np.array[N]: maximum velocities for each specified joint [rad/s]
"""
if isinstance(joint_ids, int):
return self.sim.getJointInfo(body_id, joint_ids)[11]
@@ -2127,9 +2142,9 @@ class Bullet(Simulator):
Returns:
if 1 joint:
np.float[3]: joint axis
np.array[3]: joint axis
if multiple joint:
np.float[N,3]: list of joint axis
np.array[N,3]: list of joint axis
"""
if isinstance(joint_ids, int):
return np.asarray(self.sim.getJointInfo(body_id, joint_ids)[-4])
@@ -2142,11 +2157,11 @@ class Bullet(Simulator):
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
positions (float, np.float[N]): desired position, or list of desired positions [rad]
velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
kps (None, float, np.float[N]): position gain(s)
kds (None, float, np.float[N]): velocity gain(s)
forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values.
positions (float, np.array[N]): desired position, or list of desired positions [rad]
velocities (None, float, np.array[N]): desired velocity, or list of desired velocities [rad/s]
kps (None, float, np.array[N]): position gain(s)
kds (None, float, np.array[N]): velocity gain(s)
forces (None, float, np.array[N]): maximum motor force(s)/torque(s) used to reach the target values.
"""
self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, positions=positions,
velocities=velocities, forces=forces, kp=kps, kd=kds)
@@ -2163,7 +2178,7 @@ class Bullet(Simulator):
if 1 joint:
float: joint position [rad]
if multiple joints:
np.float[N]: joint positions [rad]
np.array[N]: joint positions [rad]
"""
if isinstance(joint_ids, int):
return self.sim.getJointState(body_id, joint_ids)[0]
@@ -2176,8 +2191,8 @@ class Bullet(Simulator):
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
max_force (None, float, np.float[N]): maximum motor forces/torques
velocities (float, np.array[N]): desired velocity, or list of desired velocities [rad/s]
max_force (None, float, np.array[N]): maximum motor forces/torques
"""
if isinstance(joint_ids, int):
if max_force is None:
@@ -2202,7 +2217,7 @@ class Bullet(Simulator):
if 1 joint:
float: joint velocity [rad/s]
if multiple joints:
np.float[N]: joint velocities [rad/s]
np.array[N]: joint velocities [rad/s]
"""
if isinstance(joint_ids, int):
return self.sim.getJointState(body_id, joint_ids)[1]
@@ -2216,7 +2231,7 @@ class Bullet(Simulator):
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
accelerations (float, np.float[N]): desired joint acceleration, or list of desired joint accelerations
accelerations (float, np.array[N]): desired joint acceleration, or list of desired joint accelerations
[rad/s^2]
q (None, list of float, float): current joint positions.
dq (None, list of float, float): current joint velocities.
@@ -2273,7 +2288,7 @@ class Bullet(Simulator):
if 1 joint:
float: joint acceleration [rad/s^2]
if multiple joints:
np.float[N]: joint accelerations [rad/s^2]
np.array[N]: joint accelerations [rad/s^2]
"""
# get the torques
torques = self.get_joint_torques(body_id, joint_ids)
@@ -2320,7 +2335,7 @@ class Bullet(Simulator):
if 1 joint:
float: torque [Nm]
if multiple joints:
np.float[N]: torques associated to the given joints [Nm]
np.array[N]: torques associated to the given joints [Nm]
"""
if isinstance(joint_ids, int):
return self.sim.getJointState(body_id, joint_ids)[3]
@@ -2337,9 +2352,9 @@ class Bullet(Simulator):
Returns:
if 1 joint:
np.float[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm]
np.array[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm]
if multiple joints:
np.float[N,6]: joint reaction forces [N, Nm]
np.array[N,6]: joint reaction forces [N, Nm]
"""
if isinstance(joint_ids, int):
return np.asarray(self.sim.getJointState(body_id, joint_ids)[2])
@@ -2357,7 +2372,7 @@ class Bullet(Simulator):
if 1 joint:
float: joint power [W]
if multiple joints:
np.float[N]: power at each joint [W]
np.array[N]: power at each joint [W]
"""
torque = self.get_joint_torques(body_id, joint_ids)
velocity = self.get_joint_velocities(body_id, joint_ids)
@@ -2378,24 +2393,24 @@ class Bullet(Simulator):
shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4),
GEOM_PLANE (=6), GEOM_MESH (=5)
radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER
half_extents (np.float[3], list/tuple of 3 floats): only for GEOM_BOX.
half_extents (np.array[3], list/tuple of 3 floats): only for GEOM_BOX.
length (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height).
filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each
object (marked as 'o') in the .obj file.
mesh_scale (np.float[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.float[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
mesh_scale (np.array[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.array[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
flags (int): unused / to be decided
rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1].
specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range
[0..1]
visual_frame_position (np.float[3]): translational offset of the visual shape with respect to the link frame
vertices (list of np.float[3]): Instead of creating a mesh from obj file, you can provide vertices, indices,
visual_frame_position (np.array[3]): translational offset of the visual shape with respect to the link frame
vertices (list of np.array[3]): Instead of creating a mesh from obj file, you can provide vertices, indices,
uvs and normals
indices (list of int): triangle indices, should be a multiple of 3.
uvs (list of np.float[2]): uv texture coordinates for vertices. Use changeVisualShape to choose the
uvs (list of np.array[2]): uv texture coordinates for vertices. Use changeVisualShape to choose the
texture image. The number of uvs should be equal to number of vertices
normals (list of np.float[3]): vertex normals, number should be equal to number of vertices.
visual_frame_orientation (np.float[4]): rotational offset (quaternion x,y,z,w) of the visual shape with
normals (list of np.array[3]): vertex normals, number should be equal to number of vertices.
visual_frame_orientation (np.array[4]): rotational offset (quaternion x,y,z,w) of the visual shape with
respect to the link frame
Returns:
@@ -2449,11 +2464,11 @@ class Bullet(Simulator):
int: object unique id.
int: link index or -1 for the base
int: visual geometry type (TBD)
np.float[3]: dimensions (size, local scale) of the geometry
np.array[3]: dimensions (size, local scale) of the geometry
str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but
could be absolute
np.float[3]: position of local visual frame, relative to link/joint frame
np.float[4]: orientation of local visual frame relative to link/joint frame
np.array[3]: position of local visual frame, relative to link/joint frame
np.array[4]: orientation of local visual frame relative to link/joint frame
list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha
int: texture unique id of the shape or -1 if None. This field only exists if using
VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag.
@@ -2513,12 +2528,12 @@ class Bullet(Simulator):
of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world.
Args:
eye_position (np.float[3]): eye position in Cartesian world coordinates
target_position (np.float[3]): position of the target (focus) point in Cartesian world coordinates
up_vector (np.float[3]): up vector of the camera in Cartesian world coordinates
eye_position (np.array[3]): eye position in Cartesian world coordinates
target_position (np.array[3]): position of the target (focus) point in Cartesian world coordinates
up_vector (np.array[3]): up vector of the camera in Cartesian world coordinates
Returns:
np.float[4,4]: the view matrix
np.array[4,4]: the view matrix
More info:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
@@ -2536,7 +2551,7 @@ class Bullet(Simulator):
of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world.
Args:
target_position (np.float[3]): target focus point in Cartesian world coordinates
target_position (np.array[3]): target focus point in Cartesian world coordinates
distance (float): distance from eye to focus point
yaw (float): yaw angle in radians left/right around up-axis
pitch (float): pitch in radians up/down.
@@ -2544,7 +2559,7 @@ class Bullet(Simulator):
up_axis_index (int): either 1 for Y or 2 for Z axis up.
Returns:
np.float[4,4]: the view matrix
np.array[4,4]: the view matrix
More info:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
@@ -2576,7 +2591,7 @@ class Bullet(Simulator):
far (float): far plane distance
Returns:
np.float[4,4]: the perspective projection matrix
np.array[4,4]: the perspective projection matrix
More info:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
@@ -2595,7 +2610,7 @@ class Bullet(Simulator):
far (float): far plane distance
Returns:
np.float[4,4]: the perspective projection matrix
np.array[4,4]: the perspective projection matrix
More info:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
@@ -2619,11 +2634,11 @@ class Bullet(Simulator):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -2639,7 +2654,7 @@ class Bullet(Simulator):
int: width image resolution in pixels (horizontal)
int: height image resolution in pixels (vertical)
np.int[width, height, 4]: RBGA pixels (each pixel is in the range [0..255] for each channel R, G, B, A)
np.float[width, heigth]: Depth buffer. Bullet uses OpenGL to render, and the convention is non-linear
np.array[width, height]: Depth buffer. Bullet uses OpenGL to render, and the convention is non-linear
z-buffer. See https://stackoverflow.com/questions/6652253/getting-the-true-z-value-from-the-depth-buffer
Using the projection matrix, the depth is computed as:
`depth = far * near / (far - (far - near) * depthImg)`, where `depthImg` is the depth from Bullet
@@ -2707,11 +2722,11 @@ class Bullet(Simulator):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -2780,11 +2795,11 @@ class Bullet(Simulator):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -2797,7 +2812,7 @@ class Bullet(Simulator):
segmentation mask.
Returns:
np.float[width, heigth]: Depth buffer. Bullet uses OpenGL to render, and the convention is non-linear
np.array[width, height]: Depth buffer. Bullet uses OpenGL to render, and the convention is non-linear
z-buffer. See https://stackoverflow.com/questions/6652253/getting-the-true-z-value-from-the-depth-buffer
Using the projection matrix, the depth is computed as:
`depth = far * near / (far - (far - near) * depthImg)`, where `depthImg` is the depth from Bullet
@@ -2858,11 +2873,11 @@ class Bullet(Simulator):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -2935,16 +2950,16 @@ class Bullet(Simulator):
shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4),
GEOM_PLANE (=6), GEOM_MESH (=5)
radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER
half_extents (np.float[3], list/tuple of 3 floats): only for GEOM_BOX.
half_extents (np.array[3], list/tuple of 3 floats): only for GEOM_BOX.
height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height).
filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each
object (marked as 'o') in the .obj file.
mesh_scale (np.float[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.float[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
mesh_scale (np.array[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.array[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
flags (int): unused / to be decided
collision_frame_position (np.float[3]): translational offset of the collision shape with respect to the
collision_frame_position (np.array[3]): translational offset of the collision shape with respect to the
link frame
collision_frame_orientation (np.float[4]): rotational offset (quaternion x,y,z,w) of the collision shape
collision_frame_orientation (np.array[4]): rotational offset (quaternion x,y,z,w) of the collision shape
with respect to the link frame
Returns:
@@ -2986,14 +3001,14 @@ class Bullet(Simulator):
int: object unique id.
int: link id.
int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
np.float[3]: depends on geometry type:
np.array[3]: depends on geometry type:
for GEOM_BOX: extents,
for GEOM_SPHERE: dimensions[0] = radius,
for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
For GEOM_MESH: dimensions is the scaling factor.
str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
np.float[4]: Local orientation of the collision frame with respect to the inertial frame
np.array[3]: Local position of the collision frame with respect to the center of mass/inertial frame
np.array[4]: Local orientation of the collision frame with respect to the inertial frame
"""
collision = self.sim.getCollisionShapeData(object_id, link_id)
if len(collision) == 0:
@@ -3010,8 +3025,8 @@ class Bullet(Simulator):
enlarges the AABBs a bit (extra margin and extruded along the velocity vector).
Args:
aabb_min (np.float[3]): minimum coordinates of the aabb
aabb_max (np.float[3]): maximum coordinates of the aabb
aabb_min (np.array[3]): minimum coordinates of the aabb
aabb_max (np.array[3]): maximum coordinates of the aabb
Returns:
list of int: list of object unique ids.
@@ -3028,8 +3043,8 @@ class Bullet(Simulator):
link_id (int): link index in range [0..`getNumJoints(..)]
Returns:
np.float[3]: minimum coordinates of the axis aligned bounding box
np.float[3]: maximum coordinates of the axis aligned bounding box
np.array[3]: minimum coordinates of the axis aligned bounding box
np.array[3]: maximum coordinates of the axis aligned bounding box
"""
aabb_min, aabb_max = self.sim.getAABB(body_id, link_id)
return np.asarray(aabb_min), np.asarray(aabb_max)
@@ -3052,15 +3067,15 @@ class Bullet(Simulator):
int: body unique id of body B
int: link index of body A, -1 for base
int: link index of body B, -1 for base
np.float[3]: contact position on A, in Cartesian world coordinates
np.float[3]: contact position on B, in Cartesian world coordinates
np.float[3]: contact normal on B, pointing towards A
np.array[3]: contact position on A, in Cartesian world coordinates
np.array[3]: contact position on B, in Cartesian world coordinates
np.array[3]: contact normal on B, pointing towards A
float: contact distance, positive for separation, negative for penetration
float: normal force applied during the last `step`
float: lateral friction force in the first lateral friction direction (see next returned value)
np.float[3]: first lateral friction direction
np.array[3]: first lateral friction direction
float: lateral friction force in the second lateral friction direction (see next returned value)
np.float[3]: second lateral friction direction
np.array[3]: second lateral friction direction
"""
kwargs = {}
if body1 is not None:
@@ -3098,15 +3113,15 @@ class Bullet(Simulator):
int: body unique id of body B
int: link index of body A, -1 for base
int: link index of body B, -1 for base
np.float[3]: contact position on A, in Cartesian world coordinates
np.float[3]: contact position on B, in Cartesian world coordinates
np.float[3]: contact normal on B, pointing towards A
np.array[3]: contact position on A, in Cartesian world coordinates
np.array[3]: contact position on B, in Cartesian world coordinates
np.array[3]: contact normal on B, pointing towards A
float: contact distance, positive for separation, negative for penetration
float: normal force applied during the last `step`. Always equal to 0.
float: lateral friction force in the first lateral friction direction (see next returned value)
np.float[3]: first lateral friction direction
np.array[3]: first lateral friction direction
float: lateral friction force in the second lateral friction direction (see next returned value)
np.float[3]: second lateral friction direction
np.array[3]: second lateral friction direction
"""
kwargs = {}
if link1_id is not None:
@@ -3125,16 +3140,16 @@ class Bullet(Simulator):
Performs a single raycast to find the intersection information of the first object hit.
Args:
from_position (np.float[3]): start of the ray in world coordinates
to_position (np.float[3]): end of the ray in world coordinates
from_position (np.array[3]): start of the ray in world coordinates
to_position (np.array[3]): end of the ray in world coordinates
Returns:
list:
int: object unique id of the hit object
int: link index of the hit object, or -1 if none/parent
float: hit fraction along the ray in range [0,1] along the ray.
np.float[3]: hit position in Cartesian world coordinates
np.float[3]: hit normal in Cartesian world coordinates
np.array[3]: hit position in Cartesian world coordinates
np.array[3]: hit normal in Cartesian world coordinates
"""
if isinstance(from_position, np.ndarray):
from_position = from_position.ravel().tolist()
@@ -3163,8 +3178,8 @@ class Bullet(Simulator):
int: object unique id of the hit object
int: link index of the hit object, or -1 if none/parent
float: hit fraction along the ray in range [0,1] along the ray.
np.float[3]: hit position in Cartesian world coordinates
np.float[3]: hit normal in Cartesian world coordinates
np.array[3]: hit position in Cartesian world coordinates
np.array[3]: hit normal in Cartesian world coordinates
"""
if isinstance(from_positions, np.ndarray):
from_positions = from_positions.tolist()
@@ -3224,10 +3239,10 @@ class Bullet(Simulator):
Returns:
float: mass in kg
float: lateral friction coefficient
np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
np.array[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
aligned with the principal axes of inertia.
np.float[3]: position of inertial frame in local coordinates of the joint frame
np.float[4]: orientation of inertial frame in local coordinates of joint frame
np.array[3]: position of inertial frame in local coordinates of the joint frame
np.array[4]: orientation of inertial frame in local coordinates of joint frame
float: coefficient of restitution
float: rolling friction coefficient orthogonal to contact normal
float: spinning friction coefficient around contact normal
@@ -3262,7 +3277,7 @@ class Bullet(Simulator):
section.
friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
default, unless set in the URDF contact section)
local_inertia_diagonal (np.float[3]): diagonal elements of the inertia tensor. Note that the base and
local_inertia_diagonal (np.array[3]): 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.
joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
@@ -3311,14 +3326,14 @@ class Bullet(Simulator):
Args:
body_id (int): unique body id.
link_id (int): link id.
local_position (np.float[3]): the point on the specified link to compute the Jacobian (in link local
local_position (np.array[3]): the point on the specified link to compute the Jacobian (in link local
coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
q (np.float[N]): joint positions of size N, where N is the number of DoFs.
dq (np.float[N]): joint velocities of size N, where N is the number of DoFs.
des_ddq (np.float[N]): desired joint accelerations of size N.
q (np.array[N]): joint positions of size N, where N is the number of DoFs.
dq (np.array[N]): joint velocities of size N, where N is the number of DoFs.
des_ddq (np.array[N]): desired joint accelerations of size N.
Returns:
np.float[6,N], np.float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of
np.array[6,N], np.array[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of
columns depends if the base is fixed or floating.
"""
# Note that q, dq, ddq have to be lists in PyBullet (it doesn't work with numpy arrays)
@@ -3353,10 +3368,10 @@ class Bullet(Simulator):
Args:
body_id (int): body unique id.
q (np.float[N]): joint positions of size N, where N is the total number of DoFs.
q (np.array[N]): joint positions of size N, where N is the total number of DoFs.
Returns:
np.float[N,N], np.float[6+N,6+N]: inertia matrix
np.array[N,N], np.array[6+N,6+N]: inertia matrix
"""
if isinstance(q, np.ndarray):
q = q.ravel().tolist() # Note that pybullet doesn't accept numpy arrays here
@@ -3378,20 +3393,20 @@ class Bullet(Simulator):
Args:
body_id (int): body unique id, as returned by `load_urdf`, etc.
link_id (int): end effector link index.
position (np.float[3]): target position of the end effector (its link coordinate, not center of mass
position (np.array[3]): 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 `q_curr` joint angles.
orientation (np.float[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not
orientation (np.array[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not
specified, pure position IK will be used.
lower_limits (np.float[N], list of N floats): lower joint limits. Optional null-space IK.
upper_limits (np.float[N], list of N floats): upper joint limits. Optional null-space IK.
joint_ranges (np.float[N], list of N floats): range of value of each joint.
rest_poses (np.float[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest
lower_limits (np.array[N], list of N floats): lower joint limits. Optional null-space IK.
upper_limits (np.array[N], list of N floats): upper joint limits. Optional null-space IK.
joint_ranges (np.array[N], list of N floats): range of value of each joint.
rest_poses (np.array[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest
pose.
joint_dampings (np.float[N], list of N floats): joint damping factors. Allow to tune the IK solution using
joint_dampings (np.array[N], list of N floats): joint damping factors. Allow to tune the IK solution using
joint damping factors.
solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as
described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics".
q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body.
q_curr (np.array[N]): list of joint positions. By default PyBullet uses the joint positions of the body.
If provided, the target_position and targetOrientation is in local space!
max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target
and actual end effector position is below this threshold, or the `max_iters` is reached.
@@ -3399,7 +3414,7 @@ class Bullet(Simulator):
end effector position is below this threshold, or the `max_iters` is reached.
Returns:
np.float[N]: joint positions (for each actuated joint).
np.array[N]: joint positions (for each actuated joint).
"""
kwargs = {}
if orientation is not None:
@@ -3461,12 +3476,12 @@ class Bullet(Simulator):
Args:
body_id (int): body unique id.
q (np.float[N]): joint positions
dq (np.float[N]): joint velocities
des_ddq (np.float[N]): desired joint accelerations
q (np.array[N]): joint positions
dq (np.array[N]): joint velocities
des_ddq (np.array[N]): desired joint accelerations
Returns:
np.float[N]: joint torques computed using the rigid-body equation of motion
np.array[N]: joint torques computed using the rigid-body equation of motion
References:
[1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
@@ -3518,12 +3533,12 @@ class Bullet(Simulator):
Args:
body_id (int): unique body id.
q (np.float[N]): joint positions
dq (np.float[N]): joint velocities
torques (np.float[N]): desired joint torques
q (np.array[N]): joint positions
dq (np.array[N]): joint velocities
torques (np.array[N]): desired joint torques
Returns:
np.float[N]: joint accelerations computed using the rigid-body equation of motion
np.array[N]: joint accelerations computed using the rigid-body equation of motion
References:
[1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
@@ -3555,9 +3570,9 @@ class Bullet(Simulator):
a line width and a duration in seconds.
Args:
from_pos (np.float[3]): starting point of the line in Cartesian world coordinates
to_pos (np.float[3]): end point of the line in Cartesian world coordinates
rgb_color (np.float[3]): RGB color (each channel in range [0,1])
from_pos (np.array[3]): starting point of the line in Cartesian world coordinates
to_pos (np.array[3]): end point of the line in Cartesian world coordinates
rgb_color (np.array[3]): RGB color (each channel in range [0,1])
width (float): line width (limited by OpenGL implementation).
lifetime (float): use 0 for permanent line, or positive time in seconds (afterwards the line with be
removed automatically)
@@ -3591,12 +3606,12 @@ class Bullet(Simulator):
Args:
text (str): text.
position (np.float[3]): 3d position of the text in Cartesian world coordinates.
position (np.array[3]): 3d position of the text in Cartesian world coordinates.
rgb_color (list/tuple of 3 floats): RGB color; each component in range [0..1]
size (float): text size
lifetime (float): use 0 for permanent text, or positive time in seconds (afterwards the text with be
removed automatically)
orientation (np.float[4]): By default, debug text will always face the camera, automatically rotation.
orientation (np.array[4]): By default, debug text will always face the camera, automatically rotation.
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
@@ -3796,18 +3811,18 @@ class Bullet(Simulator):
Returns:
int: width of the visualizer camera
int: height of the visualizer camera
np.float[4,4]: view matrix [4,4]
np.float[4,4]: perspective projection matrix [4,4]
np.float[3]: camera up vector expressed in the Cartesian world space
np.float[3]: forward axis of the camera expressed in the Cartesian world space
np.float[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating
np.array[4,4]: view matrix [4,4]
np.array[4,4]: perspective projection matrix [4,4]
np.array[3]: camera up vector expressed in the Cartesian world space
np.array[3]: forward axis of the camera expressed in the Cartesian world space
np.array[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating
a simple ray tracer for example)
np.float[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a
np.array[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a
simple ray tracer for example)
float: yaw angle (in radians) of the camera, in Cartesian local space coordinates
float: pitch angle (in radians) of the camera, in Cartesian local space coordinates
float: distance between the camera and the camera target
np.float[3]: target of the camera, in Cartesian world space coordinates
np.array[3]: target of the camera, in Cartesian world space coordinates
"""
width, height, view, proj, up_vec, forward_vec,\
horizontal, vertical, yaw, pitch, dist, target = self.sim.getDebugVisualizerCamera()
@@ -3836,7 +3851,7 @@ class Bullet(Simulator):
distance (float): distance from eye to camera target position
yaw (float): camera yaw angle (in radians) left/right
pitch (float): camera pitch angle (in radians) up/down
target_position (np.float[3]): target focus point of the camera
target_position (np.array[3]): target focus point of the camera
"""
self.sim.resetDebugVisualizerCamera(cameraDistance=distance, cameraYaw=np.rad2deg(yaw),
cameraPitch=np.rad2deg(pitch), cameraTargetPosition=target_position)
pyrobolearn/simulators/mujoco.py
+430 -8
View File
@@ -5,27 +5,51 @@ This is the main interface that communicates with the MuJoCo simulator [1]. By d
decouple the PyRoboLearn framework from the simulator. It also converts some data types to the ones required by
MuJoCo.
Warnings: The MuJoCo simulator requires a license in order to use it.
Warnings:
- The MuJoCo simulator requires a license in order to use it: https://www.roboti.us/license.html
- You have to install the MuJoCo simulator beforehand: https://www.roboti.us/index.html
- You have to install ``mujoco_py``:
- for Python 2: https://github.com/openai/mujoco-py/tree/0.5
- for Python 3: https://github.com/openai/mujoco-py
- The authors of MuJoCo are working on another simulator called ``Optico`` so it is likely that MuJoCo won't
be upgraded anymore.
- This wrapper works only with Python 3 as they provide more functionalities. Also, the Python 3 API is pretty
different from the Python 2.
- You might have several errors if you don't export the correct environment variables beforehand.
Dependencies in PRL:
* `pyrobolearn.simulators.simulator.Simulator`
References:
- [1] MuJoCo: http://www.mujoco.org/
- Documentation: http://mujoco.org/book
- [2] MuJoCo Python: https://github.com/openai/mujoco-py
- [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco
"""
# TODO
# import mujoco_py as mujoco
import os
import time
import pickle
import xml.etree.ElementTree as ET
try:
import mujoco_py as mujoco
except ImportError as e:
raise ImportError(str(e) + "\nTry to install `MuJoCo` and `mujoco_py`!")
# from dm_control import mujoco
from pyrobolearn.simulators.simulator import Simulator
# check Python version
import sys
if sys.version_info[0] < 3:
raise RuntimeError("You must use Python 3 with the MuJoCo simulator.")
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["MuJoCo (Emo Todorov et al.)", "Brian Delhaisse"]
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["MuJoCo (Emo Todorov et al.)", "Open AI (MuJoCo Python API)", "Brian Delhaisse (PyRoboLearn interface)"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
@@ -40,7 +64,18 @@ class Mujoco(Simulator):
to decouple the PyRoboLearn framework from the simulator. It also converts some data types to the ones required by
MuJoCo.
Warnings: The MuJoCo simulator requires a license in order to use it.
Warnings:
- The MuJoCo simulator requires a license in order to use it: https://www.roboti.us/license.html
- You have to install the MuJoCo simulator beforehand: https://www.roboti.us/index.html
- You have to install ``mujoco_py``:
- for Python 2: https://github.com/openai/mujoco-py/tree/0.5
- for Python 3: https://github.com/openai/mujoco-py
- The authors of MuJoCo are working on another simulator called ``Optico`` so it is likely that MuJoCo won't
be upgraded anymore.
Note that initially MuJoCo doesn't allow to load dynamically objects in the world. This is carried out here, where
we remember the current state of the world, and when asked to dynamically load an object we create a new world
which is the old world + the new object. This can cause glitches on the GUI side.
References:
- [1] MuJoCo: http://www.mujoco.org/
@@ -48,6 +83,393 @@ class Mujoco(Simulator):
- [3] DeepMind Control Suite: https://github.com/deepmind/dm_control/tree/master/dm_control/mujoco
"""
def __init__(self, render=True):
def __init__(self, render=True, load_at_the_end=False):
"""
Initialize the MuJoCo simulator.
Args:
render (bool): if True, it will open the GUI, otherwise, it will just run the server (i.e. in a headless
mode, i.e. without a GUI).
load_at_the_end (bool): if True, it will load at the end all the models that have been "loaded" in the
simulator. The reason is that the MuJoCo simulator does not allow to load dynamically models into the
world.
"""
super(Mujoco, self).__init__(render=render)
raise NotImplementedError
# define variables
self.load_at_the_end = load_at_the_end
self.viewer = None
# create empty world
xml_path = os.path.dirname(os.path.abspath(__file__)) + '/mujoco_empty_world.xml'
model = mujoco.load_model_from_path(xml_path)
self.model = model
self.sim = mujoco.MjSim(model)
# parse the world
self.world = self._parse()
# define saving states
self.__simulator_saving_states = {}
##############
# Properties #
##############
@property
def version(self):
"""Return the version of the simulator in a year-month-day format."""
return mujoco.get_version()
@property
def timestep(self):
"""Return the simulator time step."""
return self.dt
@property
def dt(self):
"""Return the simulator time step."""
return self.sim.model.opt.timestep
#############
# Operators #
#############
##################
# Static methods #
##################
@staticmethod
def simulate_soft_bodies():
"""Return True if the simulator can simulate soft bodies."""
return True
@staticmethod
def supports_acceleration():
"""Return True if the simulator provides acceleration (dynamic) information (such as joint accelerations, link
Cartesian accelerations, etc). If not, the `Robot` class will have to implement these using finite
difference."""
return True
###########
# Methods #
###########
##############
# Simulators #
##############
def _parse(self, xml_path):
"""
Parse the provided XML file.
Args:
xml_path (str): path to the MuJoCo xml file.
Returns:
xml.etree.ElementTree.Element: root element in the XML file.
"""
root = ET.parse(xml_path).getroot()
return root
def reset(self):
"""Reset the simulator.
Resets the simulation data and clears buffers.
"""
self.sim.reset()
def step(self, sleep_time=0.):
"""Perform a step in the simulator, and sleep the specified amount of time.
Args:
sleep_time (float): amount of time to sleep after performing one step in the simulation.
"""
self.sim.forward() # computes forward kinematics
self.sim.step() # advance the simulation
if self.is_rendering():
if self.viewer is None:
self.viewer = mujoco.MjViewer(self.sim)
self.viewer.render()
time.sleep(sleep_time)
def render(self, enable=True):
"""Render the simulation.
Args:
enable (bool): If True, it will render the simulator by enabling the GUI.
"""
self._render = enable
if self.viewer is None:
self.viewer = mujoco.MjViewer(self.sim)
def get_time_step(self):
"""Get the time step in the simulator.
Returns:
float: time step in the simulator
"""
return self.sim.model.opt.timestep
def set_time_step(self, time_step):
"""Set the specified time step in the simulator.
"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 'stepSimulation'. It is best to only call
this method at the start of a simulation. Don't change this time step regularly. setTimeStep can also be
achieved using the new setPhysicsEngineParameter API." [1]
Args:
time_step (float): Each time you call 'step' the time step will proceed with 'time_step'.
"""
self.sim.model.opt.timestep = time_step
def get_gravity(self):
"""Return the gravity set in the simulator."""
return self.sim.model.opt.gravity
def set_gravity(self, gravity=(0, 0, -9.81)):
"""Set the gravity in the simulator with the given acceleration.
By default, there is no gravitational force enabled in the simulator.
Args:
gravity (list, tuple of 3 floats): acceleration in the x, y, z directions.
"""
self.sim.model.opt.gravity = gravity
def save(self, filename=None, *args, **kwargs):
"""
Save the state of the simulator.
Args:
filename (None, str): path to file to store the state of the simulator. If None, it will save it in
memory instead of the disk.
Returns:
int / str: unique state id, or filename. This id / filename can be used to load the state.
"""
id_ = None
if filename is None:
# create unique state id
id_ = len(self.__simulator_saving_states)
# self.sim.save(filename, format='mjb') # format='xml'
model = self.model.get_mjb()
state = self.sim.get_state()
if id_ is None:
with open(filename, 'wb') as f:
pickle.dump((model, state), f)
return filename
self.__simulator_saving_states[id_] = (model, state)
return id_
def load(self, state, *args, **kwargs):
"""
Load/Restore the simulator to a previous state.
Args:
state (int, str): unique state id, or path to the file containing the state.
"""
if isinstance(state, int):
if state not in self.__simulator_saving_states:
raise ValueError("The given state (int) has not been saved.")
model, state = self.__simulator_saving_states.pop(state)
elif isinstance(state, str):
with open(state, 'wb') as f:
model, state = pickle.load(f)
else:
raise TypeError("Expecting the given state to be an int or string, instead got: {}".format(type(state)))
# restore the simulator
self.sim = mujoco.MjSim(model)
self.sim.set_state(state)
######################################
# loading URDFs, SDFs, MJCFs, meshes #
######################################
def load_urdf(self, filename, position, orientation, use_fixed_base=0, scale=1.0, *args, **kwargs):
"""Load a URDF file in the simulator.
Args:
filename (str): a relative or absolute path to the URDF file on the file system of the physics server.
position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z]
orientation (quat): create the base of the object at the specified orientation as world space quaternion
[x,y,z,w]
use_fixed_base (bool): force the base of the loaded object to be static
scale (float): scale factor to the URDF model.
Returns:
int (non-negative): unique id associated to the load model.
"""
# create xml file based on URDF file
pass
def load_sdf(self, filename, scaling=1., *args, **kwargs):
"""Load a SDF file in the simulator.
Args:
filename (str): a relative or absolute path to the SDF file on the file system of the physics server.
scaling (float): scale factor for the object
Returns:
list(int): list of object unique id for each object loaded
"""
pass
def load_mjcf(self, filename, scaling=1., *args, **kwargs):
"""Load a Mujoco file in the simulator.
Args:
filename (str): a relative or absolute path to the MJCF file on the file system of the physics server.
scaling (float): scale factor for the object
Returns:
list(int): list of object unique id for each object loaded
"""
# update the world
# load MJCF
self.model = mujoco.load_model_from_path(filename)
self.sim = mujoco.MjSim(self.model)
#################
# visualization #
#################
# TODO: change such that we don't return the width and height (the user already knows them)
# TODO: check for segmentation image
def get_camera_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None,
light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None,
light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None):
"""
The `get_camera_image` API will return a RGB image, a depth buffer and a segmentation mask buffer with body
unique ids of visible objects for each pixel.
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
light_diffuse_coeff (float): light diffuse coefficient
light_specular_coeff (float): light specular coefficient
renderer (int): renderer
flags (int): flags
Returns:
int: width image resolution in pixels (horizontal)
int: height image resolution in pixels (vertical)
np.int[width, height, 4]: RBGA pixels (each pixel is in the range [0..255] for each channel R, G, B, A)
np.array[width, heigth]: Depth buffer.
np.int[width, height]: Segmentation mask buffer. For each pixels the visible object unique id.
"""
# based on the arguments, check the camera name
camera_name = None
return width, height, self.sim.render(width, height, camera_name, depth=True), None
def get_rgba_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None,
light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None,
light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None):
"""
The `get_rgba_image` API will return a RGBA image.
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
light_diffuse_coeff (float): light diffuse coefficient
light_specular_coeff (float): light specular coefficient
renderer (int): renderer.
flags (int): flags.
Returns:
np.int[width, height, 4]: RBGA pixels (each pixel is in the range [0..255] for each channel R, G, B, A)
"""
# based on the arguments, check the camera name
camera_name = None
# return the RGB image
return self.sim.render(width, height, camera_name, depth=False)
def get_depth_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None,
light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None,
light_diffuse_coeff=None, light_specular_coeff=None, renderer=None, flags=None):
"""
The `get_depth_image` API will return a depth buffer.
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
light_diffuse_coeff (float): light diffuse coefficient
light_specular_coeff (float): light specular coefficient
renderer (int): renderer.
flags (int): flags.
Returns:
np.array[width, height]: Depth buffer.
"""
# based on the arguments, check the camera name
camera_name = None
# return the depth image
rgb, depth = self.sim.render(width, height, camera_name, depth=True)
return depth
##############
# Collisions #
##############
def ray_test(self, from_position, to_position):
"""
Performs a single raycast to find the intersection information of the first object hit.
Args:
from_position (np.array[3]): start of the ray in world coordinates
to_position (np.array[3]): end of the ray in world coordinates
Returns:
list:
int: object unique id of the hit object
int: link index of the hit object, or -1 if none/parent
float: hit fraction along the ray in range [0,1] along the ray.
np.array[3]: hit position in Cartesian world coordinates
np.array[3]: hit normal in Cartesian world coordinates
"""
vec = to_position - from_position
return self.sim.ray(pnt=from_position, vec=vec) # this return the distance and id of the geom
# Test
if __name__ == '__main__':
from itertools import count
sim = Mujoco(render=True)
for t in count():
sim.step(sim.dt)
pyrobolearn/simulators/simulator.py
+208 -186
View File
@@ -292,6 +292,28 @@ class Simulator(object):
"""Return True if the simulator has a middleware communication layer (like ROS, YARP, etc)."""
return False
@staticmethod
def supports_dynamic_loading():
"""Return True if the simulator supports the dynamic loading of models."""
return False
@staticmethod
def supports_acceleration():
"""Return True if the simulator provides acceleration (dynamic) information (such as joint accelerations, link
Cartesian accelerations, etc). If not, the `Robot` class will have to implement these using finite
difference."""
return False
@staticmethod
def supports_sensors(sensor_type=None):
"""Return True if the simulator provides supports for the specified sensor."""
return False
@staticmethod
def supports_urdf():
"""Return True if we can use URDFs."""
return False
###########
# Methods #
###########
@@ -569,8 +591,8 @@ class Simulator(object):
collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape
for multiple multibodies (instancing)
mass (float): mass of the base, in kg (if using SI units)
position (np.float[3]): Cartesian world position of the base
orientation (np.float[4]): Orientation of base as quaternion [x,y,z,w]
position (np.array[3]): Cartesian world position of the base
orientation (np.array[4]): Orientation of base as quaternion [x,y,z,w]
Returns:
int: non-negative unique id or -1 for failure.
@@ -632,13 +654,13 @@ class Simulator(object):
child_link_id (int): child link index, or -1 for the base
joint_type (int): joint type: JOINT_PRISMATIC (=1), JOINT_FIXED (=4), JOINT_POINT2POINT (=5),
JOINT_GEAR (=6)
joint_axis (np.float[3]): joint axis, in child link frame
parent_frame_position (np.float[3]): position of the joint frame relative to parent CoM frame.
child_frame_position (np.float[3]): position of the joint frame relative to a given child CoM frame (or
joint_axis (np.array[3]): joint axis, in child link frame
parent_frame_position (np.array[3]): position of the joint frame relative to parent CoM frame.
child_frame_position (np.array[3]): position of the joint frame relative to a given child CoM frame (or
world origin if no child specified)
parent_frame_orientation (np.float[4]): the orientation of the joint frame relative to parent CoM
parent_frame_orientation (np.array[4]): the orientation of the joint frame relative to parent CoM
coordinate frame
child_frame_orientation (np.float[4]): the orientation of the joint frame relative to the child CoM
child_frame_orientation (np.array[4]): the orientation of the joint frame relative to the child CoM
coordinate frame (or world origin frame if no child specified)
Returns:
@@ -753,7 +775,7 @@ class Simulator(object):
of the specified body.
Returns:
np.float[3]: center of mass position in the Cartesian world coordinates
np.array[3]: center of mass position in the Cartesian world coordinates
"""
pass
@@ -767,7 +789,7 @@ class Simulator(object):
of the specified body.
Returns:
np.float[3]: center of mass linear velocity.
np.array[3]: center of mass linear velocity.
"""
pass
@@ -779,8 +801,8 @@ class Simulator(object):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: base position
np.float[4]: base orientation (quaternion [x,y,z,w])
np.array[3]: base position
np.array[4]: base orientation (quaternion [x,y,z,w])
"""
pass
@@ -792,7 +814,7 @@ class Simulator(object):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: base position.
np.array[3]: base position.
"""
pass
@@ -804,7 +826,7 @@ class Simulator(object):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[4]: base orientation in the form of a quaternion (x,y,z,w)
np.array[4]: base orientation in the form of a quaternion (x,y,z,w)
"""
pass
@@ -814,8 +836,8 @@ class Simulator(object):
Args:
body_id (int): unique object id.
position (np.float[3]): new base position.
orientation (np.float[4]): new base orientation (expressed as a quaternion [x,y,z,w])
position (np.array[3]): new base position.
orientation (np.array[4]): new base orientation (expressed as a quaternion [x,y,z,w])
"""
pass
@@ -825,7 +847,7 @@ class Simulator(object):
Args:
body_id (int): unique object id.
position (np.float[3]): new base position.
position (np.array[3]): new base position.
"""
pass
@@ -835,7 +857,7 @@ class Simulator(object):
Args:
body_id (int): unique object id.
orientation (np.float[4]): new base orientation (expressed as a quaternion [x,y,z,w])
orientation (np.array[4]): new base orientation (expressed as a quaternion [x,y,z,w])
"""
pass
@@ -847,8 +869,8 @@ class Simulator(object):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: linear velocity of the base in Cartesian world space coordinates
np.float[3]: angular velocity of the base in Cartesian world space coordinates
np.array[3]: linear velocity of the base in Cartesian world space coordinates
np.array[3]: angular velocity of the base in Cartesian world space coordinates
"""
pass
@@ -860,7 +882,7 @@ class Simulator(object):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: linear velocity of the base in Cartesian world space coordinates
np.array[3]: linear velocity of the base in Cartesian world space coordinates
"""
pass
@@ -872,7 +894,7 @@ class Simulator(object):
body_id (int): object unique id, as returned from `load_urdf`.
Returns:
np.float[3]: angular velocity of the base in Cartesian world space coordinates
np.array[3]: angular velocity of the base in Cartesian world space coordinates
"""
pass
@@ -882,8 +904,8 @@ class Simulator(object):
Args:
body_id (int): unique object id.
linear_velocity (np.float[3]): new linear velocity of the base.
angular_velocity (np.float[3]): new angular velocity of the base.
linear_velocity (np.array[3]): new linear velocity of the base.
angular_velocity (np.array[3]): new angular velocity of the base.
"""
pass
@@ -893,7 +915,7 @@ class Simulator(object):
Args:
body_id (int): unique object id.
linear_velocity (np.float[3]): new linear velocity of the base
linear_velocity (np.array[3]): new linear velocity of the base
"""
pass
@@ -903,7 +925,7 @@ class Simulator(object):
Args:
body_id (int): unique object id.
angular_velocity (np.float[3]): new angular velocity of the base
angular_velocity (np.array[3]): new angular velocity of the base
"""
pass
@@ -914,8 +936,8 @@ class Simulator(object):
Args:
body_id (int): unique body id.
link_id (int): unique link id. If -1, it will be the base.
force (np.float[3]): external force to be applied.
position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate
force (np.array[3]): external force to be applied.
position (np.array[3]): position on the link where the force is applied. See `flags` for coordinate
systems. If None, it is the center of mass of the body (or the link if specified).
frame (int): if frame = 1, then the force / position is described in the link frame. If frame = 2, they
are described in the world frame.
@@ -1001,7 +1023,7 @@ class Simulator(object):
Returns:
float: The position value of this joint.
float: The velocity value of this joint.
np.float[6]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is
np.array[6]: 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].
float: This is the motor torque applied during the last stepSimulation. Note that this only applies in
VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque
@@ -1021,7 +1043,7 @@ class Simulator(object):
list:
float: The position value of this joint.
float: The velocity value of this joint.
np.float[6]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is
np.array[6]: 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].
float: This is the motor torque applied during the last `step`. Note that this only applies in
VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor
@@ -1071,8 +1093,8 @@ class Simulator(object):
joint_ids (int): joint/link id, or list of joint ids.
control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD),
VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3).
positions (float, np.float[N]): target joint position(s) (used in POSITION_CONTROL).
velocities (float, np.float[N]): target joint velocity(ies). In VELOCITY_CONTROL and POSITION_CONTROL,
positions (float, np.array[N]): target joint position(s) (used in POSITION_CONTROL).
velocities (float, np.array[N]): target joint velocity(ies). In VELOCITY_CONTROL and POSITION_CONTROL,
the target velocity(ies) is(are) the desired velocity of the joint. Note that the target velocity(ies)
is(are) not the maximum joint velocity(ies). In PD_CONTROL and
POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using:
@@ -1098,15 +1120,15 @@ class Simulator(object):
using forward kinematics.
Returns:
np.float[3]: Cartesian position of CoM
np.float[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.float[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.float[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
np.array[3]: Cartesian position of CoM
np.array[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.array[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF link
frame
np.float[3]: world position of the URDF link frame
np.float[4]: world orientation of the URDF link frame
np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
np.array[3]: world position of the URDF link frame
np.array[4]: world orientation of the URDF link frame
np.array[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.array[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
"""
pass
@@ -1123,15 +1145,15 @@ class Simulator(object):
Returns:
list:
np.float[3]: Cartesian position of CoM
np.float[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.float[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.float[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF
np.array[3]: Cartesian position of CoM
np.array[4]: Cartesian orientation of CoM, in quaternion [x,y,z,w]
np.array[3]: local position offset of inertial frame (center of mass) expressed in the URDF link frame
np.array[4]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF
link frame
np.float[3]: world position of the URDF link frame
np.float[4]: world orientation of the URDF link frame
np.float[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.float[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
np.array[3]: world position of the URDF link frame
np.array[4]: world orientation of the URDF link frame
np.array[3]: Cartesian world linear velocity. Only returned if `compute_velocity` is True.
np.array[3]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
"""
pass
@@ -1180,9 +1202,9 @@ class Simulator(object):
Returns:
if 1 link:
np.float[3]: the link CoM position in the world space
np.array[3]: the link CoM position in the world space
if multiple links:
np.float[N,3]: CoM position of each link in world space
np.array[N,3]: CoM position of each link in world space
"""
pass
@@ -1199,9 +1221,9 @@ class Simulator(object):
Returns:
if 1 link:
np.float[4]: Cartesian orientation of the link CoM (x,y,z,w)
np.array[4]: Cartesian orientation of the link CoM (x,y,z,w)
if multiple links:
np.float[N,4]: CoM orientation of each link (x,y,z,w)
np.array[N,4]: CoM orientation of each link (x,y,z,w)
"""
pass
@@ -1218,9 +1240,9 @@ class Simulator(object):
Returns:
if 1 link:
np.float[3]: linear velocity of the link in the Cartesian world space
np.array[3]: linear velocity of the link in the Cartesian world space
if multiple links:
np.float[N,3]: linear velocity of each link
np.array[N,3]: linear velocity of each link
"""
pass
@@ -1234,9 +1256,9 @@ class Simulator(object):
Returns:
if 1 link:
np.float[3]: angular velocity of the link in the Cartesian world space
np.array[3]: angular velocity of the link in the Cartesian world space
if multiple links:
np.float[N,3]: angular velocity of each link
np.array[N,3]: angular velocity of each link
"""
pass
@@ -1251,9 +1273,9 @@ class Simulator(object):
Returns:
if 1 link:
np.float[6]: linear and angular velocity of the link in the Cartesian world space
np.array[6]: linear and angular velocity of the link in the Cartesian world space
if multiple links:
np.float[N,6]: linear and angular velocity of each link
np.array[N,6]: linear and angular velocity of each link
"""
pass
@@ -1346,7 +1368,7 @@ class Simulator(object):
if 1 joint:
float: damping coefficient of the given joint
if multiple joints:
np.float[N]: damping coefficient for each specified joint
np.array[N]: damping coefficient for each specified joint
"""
pass
@@ -1362,7 +1384,7 @@ class Simulator(object):
if 1 joint:
float: friction coefficient of the given joint
if multiple joints:
np.float[N]: friction coefficient for each specified joint
np.array[N]: friction coefficient for each specified joint
"""
pass
@@ -1376,9 +1398,9 @@ class Simulator(object):
Returns:
if 1 joint:
np.float[2]: lower and upper limit
np.array[2]: lower and upper limit
if multiple joints:
np.float[N,2]: lower and upper limit for each specified joint
np.array[N,2]: lower and upper limit for each specified joint
"""
pass
@@ -1396,7 +1418,7 @@ class Simulator(object):
if 1 joint:
float: maximum force [N]
if multiple joints:
np.float[N]: maximum force for each specified joint [N]
np.array[N]: maximum force for each specified joint [N]
"""
pass
@@ -1414,7 +1436,7 @@ class Simulator(object):
if 1 joint:
float: maximum velocity [rad/s]
if multiple joints:
np.float[N]: maximum velocities for each specified joint [rad/s]
np.array[N]: maximum velocities for each specified joint [rad/s]
"""
pass
@@ -1428,9 +1450,9 @@ class Simulator(object):
Returns:
if 1 joint:
np.float[3]: joint axis
np.array[3]: joint axis
if multiple joint:
np.float[N,3]: list of joint axis
np.array[N,3]: list of joint axis
"""
pass
@@ -1441,11 +1463,11 @@ class Simulator(object):
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
positions (float, np.float[N]): desired position, or list of desired positions [rad]
velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
kps (None, float, np.float[N]): position gain(s)
kds (None, float, np.float[N]): velocity gain(s)
forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values.
positions (float, np.array[N]): desired position, or list of desired positions [rad]
velocities (None, float, np.array[N]): desired velocity, or list of desired velocities [rad/s]
kps (None, float, np.array[N]): position gain(s)
kds (None, float, np.array[N]): velocity gain(s)
forces (None, float, np.array[N]): maximum motor force(s)/torque(s) used to reach the target values.
"""
pass
@@ -1461,7 +1483,7 @@ class Simulator(object):
if 1 joint:
float: joint position [rad]
if multiple joints:
np.float[N]: joint positions [rad]
np.array[N]: joint positions [rad]
"""
pass
@@ -1472,8 +1494,8 @@ class Simulator(object):
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
max_force (None, float, np.float[N]): maximum motor forces/torques
velocities (float, np.array[N]): desired velocity, or list of desired velocities [rad/s]
max_force (None, float, np.array[N]): maximum motor forces/torques
"""
pass
@@ -1489,7 +1511,7 @@ class Simulator(object):
if 1 joint:
float: joint velocity [rad/s]
if multiple joints:
np.float[N]: joint velocities [rad/s]
np.array[N]: joint velocities [rad/s]
"""
pass
@@ -1501,7 +1523,7 @@ class Simulator(object):
Args:
body_id (int): unique body id.
joint_ids (int, list of int): joint id, or list of joint ids.
accelerations (float, np.float[N]): desired joint acceleration, or list of desired joint accelerations
accelerations (float, np.array[N]): desired joint acceleration, or list of desired joint accelerations
[rad/s^2]
"""
pass
@@ -1521,7 +1543,7 @@ class Simulator(object):
if 1 joint:
float: joint acceleration [rad/s^2]
if multiple joints:
np.float[N]: joint accelerations [rad/s^2]
np.array[N]: joint accelerations [rad/s^2]
"""
pass
@@ -1548,7 +1570,7 @@ class Simulator(object):
if 1 joint:
float: torque [Nm]
if multiple joints:
np.float[N]: torques associated to the given joints [Nm]
np.array[N]: torques associated to the given joints [Nm]
"""
pass
@@ -1562,9 +1584,9 @@ class Simulator(object):
Returns:
if 1 joint:
np.float[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm]
np.array[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm]
if multiple joints:
np.float[N,6]: joint reaction forces [N, Nm]
np.array[N,6]: joint reaction forces [N, Nm]
"""
pass
@@ -1579,7 +1601,7 @@ class Simulator(object):
if 1 joint:
float: joint power [W]
if multiple joints:
np.float[N]: power at each joint [W]
np.array[N]: power at each joint [W]
"""
pass
@@ -1596,24 +1618,24 @@ class Simulator(object):
shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4),
GEOM_PLANE (=6), GEOM_MESH (=5)
radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER
half_extents (np.float[3], list/tuple of 3 floats): only for GEOM_BOX.
half_extents (np.array[3], list/tuple of 3 floats): only for GEOM_BOX.
length (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height).
filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each
object (marked as 'o') in the .obj file.
mesh_scale (np.float[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.float[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
mesh_scale (np.array[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.array[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
flags (int): unused / to be decided
rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1].
specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range
[0..1]
visual_frame_position (np.float[3]): translational offset of the visual shape with respect to the link frame
vertices (list of np.float[3]): Instead of creating a mesh from obj file, you can provide vertices, indices,
visual_frame_position (np.array[3]): translational offset of the visual shape with respect to the link frame
vertices (list of np.array[3]): Instead of creating a mesh from obj file, you can provide vertices, indices,
uvs and normals
indices (list of int): triangle indices, should be a multiple of 3.
uvs (list of np.float[2]): uv texture coordinates for vertices. Use changeVisualShape to choose the
uvs (list of np.array[2]): uv texture coordinates for vertices. Use changeVisualShape to choose the
texture image. The number of uvs should be equal to number of vertices
normals (list of np.float[3]): vertex normals, number should be equal to number of vertices.
visual_frame_orientation (np.float[4]): rotational offset (quaternion x,y,z,w) of the visual shape with
normals (list of np.array[3]): vertex normals, number should be equal to number of vertices.
visual_frame_orientation (np.array[4]): rotational offset (quaternion x,y,z,w) of the visual shape with
respect to the link frame
Returns:
@@ -1634,11 +1656,11 @@ class Simulator(object):
int: object unique id.
int: link index or -1 for the base
int: visual geometry type (TBD)
np.float[3]: dimensions (size, local scale) of the geometry
np.array[3]: dimensions (size, local scale) of the geometry
str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but
could be absolute
np.float[3]: position of local visual frame, relative to link/joint frame
np.float[4]: orientation of local visual frame relative to link/joint frame
np.array[3]: position of local visual frame, relative to link/joint frame
np.array[4]: orientation of local visual frame relative to link/joint frame
list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha
int: texture unique id of the shape or -1 if None. This field only exists if using
VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag.
@@ -1683,12 +1705,12 @@ class Simulator(object):
of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world.
Args:
eye_position (np.float[3]): eye position in Cartesian world coordinates
target_position (np.float[3]): position of the target (focus) point in Cartesian world coordinates
up_vector (np.float[3]): up vector of the camera in Cartesian world coordinates
eye_position (np.array[3]): eye position in Cartesian world coordinates
target_position (np.array[3]): position of the target (focus) point in Cartesian world coordinates
up_vector (np.array[3]): up vector of the camera in Cartesian world coordinates
Returns:
np.float[4,4]: the view matrix
np.array[4,4]: the view matrix
"""
pass
@@ -1700,7 +1722,7 @@ class Simulator(object):
of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world.
Args:
target_position (np.float[3]): target focus point in Cartesian world coordinates
target_position (np.array[3]): target focus point in Cartesian world coordinates
distance (float): distance from eye to focus point
yaw (float): yaw angle in radians left/right around up-axis
pitch (float): pitch in radians up/down.
@@ -1708,7 +1730,7 @@ class Simulator(object):
up_axis_index (int): either 1 for Y or 2 for Z axis up.
Returns:
np.float[4,4]: the view matrix
np.array[4,4]: the view matrix
"""
pass
@@ -1733,7 +1755,7 @@ class Simulator(object):
far (float): far plane distance
Returns:
np.float[4,4]: the perspective projection matrix
np.array[4,4]: the perspective projection matrix
"""
pass
@@ -1747,7 +1769,7 @@ class Simulator(object):
far (float): far plane distance
Returns:
np.float[4,4]: the perspective projection matrix
np.array[4,4]: the perspective projection matrix
"""
pass
@@ -1761,11 +1783,11 @@ class Simulator(object):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -1778,7 +1800,7 @@ class Simulator(object):
int: width image resolution in pixels (horizontal)
int: height image resolution in pixels (vertical)
np.int[width, height, 4]: RBGA pixels (each pixel is in the range [0..255] for each channel R, G, B, A)
np.float[width, heigth]: Depth buffer.
np.array[width, height]: Depth buffer.
np.int[width, height]: Segmentation mask buffer. For each pixels the visible object unique id.
"""
pass
@@ -1792,11 +1814,11 @@ class Simulator(object):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -1819,11 +1841,11 @@ class Simulator(object):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -1833,7 +1855,7 @@ class Simulator(object):
flags (int): flags.
Returns:
np.float[width, heigth]: Depth buffer.
np.array[width, height]: Depth buffer.
"""
pass
@@ -1847,11 +1869,11 @@ class Simulator(object):
Args:
width (int): horizontal image resolution in pixels
height (int): vertical image resolution in pixels
view_matrix (np.float[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.float[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.float[3]): `light_direction` specifies the world position of the light source,
view_matrix (np.array[4,4]): 4x4 view matrix, see `compute_view_matrix`
projection_matrix (np.array[4,4]): 4x4 projection matrix, see `compute_projection`
light_direction (np.array[3]): `light_direction` specifies the world position of the light source,
the direction is from the light source position to the origin of the world frame.
light_color (np.float[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_color (np.array[3]): directional light color in [RED,GREEN,BLUE] in range 0..1
light_distance (float): distance of the light along the normalized `light_direction`
shadow (bool): True for shadows, False for no shadows
light_ambient_coeff (float): light ambient coefficient
@@ -1877,16 +1899,16 @@ class Simulator(object):
shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4),
GEOM_PLANE (=6), GEOM_MESH (=5)
radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER
half_extents (np.float[3], list/tuple of 3 floats): only for GEOM_BOX.
half_extents (np.array[3], list/tuple of 3 floats): only for GEOM_BOX.
height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (length = height).
filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each
object (marked as 'o') in the .obj file.
mesh_scale (np.float[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.float[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
mesh_scale (np.array[3], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH).
plane_normal (np.array[3], list/tuple of 3 floats): plane normal (only for GEOM_PLANE).
flags (int): unused / to be decided
collision_frame_position (np.float[3]): translational offset of the collision shape with respect to the
collision_frame_position (np.array[3]): translational offset of the collision shape with respect to the
link frame
collision_frame_orientation (np.float[4]): rotational offset (quaternion x,y,z,w) of the collision shape
collision_frame_orientation (np.array[4]): rotational offset (quaternion x,y,z,w) of the collision shape
with respect to the link frame
Returns:
@@ -1906,14 +1928,14 @@ class Simulator(object):
int: object unique id.
int: link id.
int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
np.float[3]: depends on geometry type:
np.array[3]: depends on geometry type:
for GEOM_BOX: extents,
for GEOM_SPHERE: dimensions[0] = radius,
for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
For GEOM_MESH: dimensions is the scaling factor.
str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
np.float[4]: Local orientation of the collision frame with respect to the inertial frame
np.array[3]: Local position of the collision frame with respect to the center of mass/inertial frame
np.array[4]: Local orientation of the collision frame with respect to the inertial frame
"""
pass
@@ -1925,8 +1947,8 @@ class Simulator(object):
enlarges the AABBs a bit (extra margin and extruded along the velocity vector).
Args:
aabb_min (np.float[3]): minimum coordinates of the aabb
aabb_max (np.float[3]): maximum coordinates of the aabb
aabb_min (np.array[3]): minimum coordinates of the aabb
aabb_max (np.array[3]): maximum coordinates of the aabb
Returns:
list of int: list of object unique ids.
@@ -1943,8 +1965,8 @@ class Simulator(object):
link_id (int): link index in range [0..`getNumJoints(..)]
Returns:
np.float[3]: minimum coordinates of the axis aligned bounding box
np.float[3]: maximum coordinates of the axis aligned bounding box
np.array[3]: minimum coordinates of the axis aligned bounding box
np.array[3]: maximum coordinates of the axis aligned bounding box
"""
pass
@@ -1966,15 +1988,15 @@ class Simulator(object):
int: body unique id of body B
int: link index of body A, -1 for base
int: link index of body B, -1 for base
np.float[3]: contact position on A, in Cartesian world coordinates
np.float[3]: contact position on B, in Cartesian world coordinates
np.float[3]: contact normal on B, pointing towards A
np.array[3]: contact position on A, in Cartesian world coordinates
np.array[3]: contact position on B, in Cartesian world coordinates
np.array[3]: contact normal on B, pointing towards A
float: contact distance, positive for separation, negative for penetration
float: normal force applied during the last `step`
float: lateral friction force in the first lateral friction direction (see next returned value)
np.float[3]: first lateral friction direction
np.array[3]: first lateral friction direction
float: lateral friction force in the second lateral friction direction (see next returned value)
np.float[3]: second lateral friction direction
np.array[3]: second lateral friction direction
"""
pass
@@ -1998,15 +2020,15 @@ class Simulator(object):
int: body unique id of body B
int: link index of body A, -1 for base
int: link index of body B, -1 for base
np.float[3]: contact position on A, in Cartesian world coordinates
np.float[3]: contact position on B, in Cartesian world coordinates
np.float[3]: contact normal on B, pointing towards A
np.array[3]: contact position on A, in Cartesian world coordinates
np.array[3]: contact position on B, in Cartesian world coordinates
np.array[3]: contact normal on B, pointing towards A
float: contact distance, positive for separation, negative for penetration
float: normal force applied during the last `step`. Always equal to 0.
float: lateral friction force in the first lateral friction direction (see next returned value)
np.float[3]: first lateral friction direction
np.array[3]: first lateral friction direction
float: lateral friction force in the second lateral friction direction (see next returned value)
np.float[3]: second lateral friction direction
np.array[3]: second lateral friction direction
"""
pass
@@ -2015,16 +2037,16 @@ class Simulator(object):
Performs a single raycast to find the intersection information of the first object hit.
Args:
from_position (np.float[3]): start of the ray in world coordinates
to_position (np.float[3]): end of the ray in world coordinates
from_position (np.array[3]): start of the ray in world coordinates
to_position (np.array[3]): end of the ray in world coordinates
Returns:
list:
int: object unique id of the hit object
int: link index of the hit object, or -1 if none/parent
float: hit fraction along the ray in range [0,1] along the ray.
np.float[3]: hit position in Cartesian world coordinates
np.float[3]: hit normal in Cartesian world coordinates
np.array[3]: hit position in Cartesian world coordinates
np.array[3]: hit normal in Cartesian world coordinates
"""
pass
@@ -2047,8 +2069,8 @@ class Simulator(object):
int: object unique id of the hit object
int: link index of the hit object, or -1 if none/parent
float: hit fraction along the ray in range [0,1] along the ray.
np.float[3]: hit position in Cartesian world coordinates
np.float[3]: hit normal in Cartesian world coordinates
np.array[3]: hit position in Cartesian world coordinates
np.array[3]: hit normal in Cartesian world coordinates
"""
pass
@@ -2092,10 +2114,10 @@ class Simulator(object):
Returns:
float: mass in kg
float: lateral friction coefficient
np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
np.array[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
aligned with the principal axes of inertia.
np.float[3]: position of inertial frame in local coordinates of the joint frame
np.float[4]: orientation of inertial frame in local coordinates of joint frame
np.array[3]: position of inertial frame in local coordinates of the joint frame
np.array[4]: orientation of inertial frame in local coordinates of joint frame
float: coefficient of restitution
float: rolling friction coefficient orthogonal to contact normal
float: spinning friction coefficient around contact normal
@@ -2127,7 +2149,7 @@ class Simulator(object):
section.
friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
default, unless set in the URDF contact section)
local_inertia_diagonal (np.float[3]): diagonal elements of the inertia tensor. Note that the base and
local_inertia_diagonal (np.array[3]): 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.
joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
@@ -2150,14 +2172,14 @@ class Simulator(object):
Args:
body_id (int): unique body id.
link_id (int): link id.
local_position (np.float[3]): the point on the specified link to compute the Jacobian (in link local
local_position (np.array[3]): the point on the specified link to compute the Jacobian (in link local
coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
q (np.float[N]): joint positions of size N, where N is the number of DoFs.
dq (np.float[N]): joint velocities of size N, where N is the number of DoFs.
des_ddq (np.float[N]): desired joint accelerations of size N.
q (np.array[N]): joint positions of size N, where N is the number of DoFs.
dq (np.array[N]): joint velocities of size N, where N is the number of DoFs.
des_ddq (np.array[N]): desired joint accelerations of size N.
Returns:
np.float[6,N], np.float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of
np.array[6,N], np.array[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of
columns depends if the base is fixed or floating.
"""
pass
@@ -2178,10 +2200,10 @@ class Simulator(object):
Args:
body_id (int): body unique id.
q (np.float[N]): joint positions of size N, where N is the total number of DoFs.
q (np.array[N]): joint positions of size N, where N is the total number of DoFs.
Returns:
np.float[N,N], np.float[6+N,6+N]: inertia matrix
np.array[N,N], np.array[6+N,6+N]: inertia matrix
"""
pass
@@ -2201,20 +2223,20 @@ class Simulator(object):
Args:
body_id (int): body unique id, as returned by `load_urdf`, etc.
link_id (int): end effector link index.
position (np.float[3]): target position of the end effector (its link coordinate, not center of mass
position (np.array[3]): 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 `q_curr` joint angles.
orientation (np.float[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not
orientation (np.array[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not
specified, pure position IK will be used.
lower_limits (np.float[N], list of N floats): lower joint limits. Optional null-space IK.
upper_limits (np.float[N], list of N floats): upper joint limits. Optional null-space IK.
joint_ranges (np.float[N], list of N floats): range of value of each joint.
rest_poses (np.float[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest
lower_limits (np.array[N], list of N floats): lower joint limits. Optional null-space IK.
upper_limits (np.array[N], list of N floats): upper joint limits. Optional null-space IK.
joint_ranges (np.array[N], list of N floats): range of value of each joint.
rest_poses (np.array[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest
pose.
joint_dampings (np.float[N], list of N floats): joint damping factors. Allow to tune the IK solution using
joint_dampings (np.array[N], list of N floats): joint damping factors. Allow to tune the IK solution using
joint damping factors.
solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as
described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics".
q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body.
q_curr (np.array[N]): list of joint positions. By default PyBullet uses the joint positions of the body.
If provided, the target_position and targetOrientation is in local space!
max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target
and actual end effector position is below this threshold, or the `max_iters` is reached.
@@ -2222,7 +2244,7 @@ class Simulator(object):
end effector position is below this threshold, or the `max_iters` is reached.
Returns:
np.float[N]: joint positions (for each actuated joint).
np.array[N]: joint positions (for each actuated joint).
"""
pass
@@ -2258,12 +2280,12 @@ class Simulator(object):
Args:
body_id (int): body unique id.
q (np.float[N]): joint positions
dq (np.float[N]): joint velocities
des_ddq (np.float[N]): desired joint accelerations
q (np.array[N]): joint positions
dq (np.array[N]): joint velocities
des_ddq (np.array[N]): desired joint accelerations
Returns:
np.float[N]: joint torques computed using the rigid-body equation of motion
np.array[N]: joint torques computed using the rigid-body equation of motion
References:
[1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
@@ -2306,12 +2328,12 @@ class Simulator(object):
Args:
body_id (int): unique body id.
q (np.float[N]): joint positions
dq (np.float[N]): joint velocities
torques (np.float[N]): desired joint torques
q (np.array[N]): joint positions
dq (np.array[N]): joint velocities
torques (np.array[N]): desired joint torques
Returns:
np.float[N]: joint accelerations computed using the rigid-body equation of motion
np.array[N]: joint accelerations computed using the rigid-body equation of motion
References:
[1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
@@ -2332,9 +2354,9 @@ class Simulator(object):
a line width and a duration in seconds.
Args:
from_pos (np.float[3]): starting point of the line in Cartesian world coordinates
to_pos (np.float[3]): end point of the line in Cartesian world coordinates
rgb_color (np.float[3]): RGB color (each channel in range [0,1])
from_pos (np.array[3]): starting point of the line in Cartesian world coordinates
to_pos (np.array[3]): end point of the line in Cartesian world coordinates
rgb_color (np.array[3]): RGB color (each channel in range [0,1])
width (float): line width (limited by OpenGL implementation).
lifetime (float): use 0 for permanent line, or positive time in seconds (afterwards the line with be
removed automatically)
@@ -2354,12 +2376,12 @@ class Simulator(object):
Args:
text (str): text.
position (np.float[3]): 3d position of the text in Cartesian world coordinates.
position (np.array[3]): 3d position of the text in Cartesian world coordinates.
rgb_color (list/tuple of 3 floats): RGB color; each component in range [0..1]
size (float): text size
lifetime (float): use 0 for permanent text, or positive time in seconds (afterwards the text with be
removed automatically)
orientation (np.float[4]): By default, debug text will always face the camera, automatically rotation.
orientation (np.array[4]): By default, debug text will always face the camera, automatically rotation.
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
@@ -2542,18 +2564,18 @@ class Simulator(object):
Returns:
float: width of the visualizer camera
float: height of the visualizer camera
np.float[4,4]: view matrix [4,4]
np.float[4,4]: perspective projection matrix [4,4]
np.float[3]: camera up vector expressed in the Cartesian world space
np.float[3]: forward axis of the camera expressed in the Cartesian world space
np.float[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating
np.array[4,4]: view matrix [4,4]
np.array[4,4]: perspective projection matrix [4,4]
np.array[3]: camera up vector expressed in the Cartesian world space
np.array[3]: forward axis of the camera expressed in the Cartesian world space
np.array[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating
a simple ray tracer for example)
np.float[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a
np.array[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a
simple ray tracer for example)
float: yaw angle (in radians) of the camera, in Cartesian local space coordinates
float: pitch angle (in radians) of the camera, in Cartesian local space coordinates
float: distance between the camera and the camera target
np.float[3]: target of the camera, in Cartesian world space coordinates
np.array[3]: target of the camera, in Cartesian world space coordinates
"""
pass
@@ -2567,7 +2589,7 @@ class Simulator(object):
distance (float): distance from eye to camera target position
yaw (float): camera yaw angle (in radians) left/right
pitch (float): camera pitch angle (in radians) up/down
target_position (np.float[3]): target focus point of the camera
target_position (np.array[3]): target focus point of the camera
"""
pass
pyrobolearn/simulators/vrep.py
+2 -2
View File
@@ -36,8 +36,8 @@ from pyrobolearn.simulators.simulator import Simulator
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["V-REP (Coppelia Robotics)", "PyRep (James et al.)", "Brian Delhaisse"]
__copyright__ = "Copyright 2019, PyRoboLearn"
__credits__ = ["V-REP (Coppelia Robotics)", "PyRep (James et al.)", "Brian Delhaisse (PyRoboLearn interface)"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
pyrobolearn/worlds/world.py
+25
View File
@@ -1082,6 +1082,31 @@ class World(object):
return True
return False
def are_attached(self, body1, body2, link1=None, link2=None):
"""
Return True if the given links/bodies are attached.
Args:
body1 (int, Body): body unique id, or a Body instance.
body2 (int, Body): body unique id, or a Body instance.
link1 (int, None): link id. By default, it will be the base (=-1). If None, all the links of the first
body that were attached to the second body will be detached.
link2 (int, None): link id. By default, it will be the base (=-1). If None, all the links of the second
body that were attached to the first body will be detached.
Returns:
bool: True if the bodies/links are attached.
"""
if (body1, body2) in self.constraints:
if link1 is None and link2 is None:
return True
else:
for constraint in self.constraints[(body1, body2)]:
link_1, link_2, constraint_id = constraint[-1]
if link1 == link_1 and link2 == link_2:
return True
return False
def load_floor(self, scaling=1.):
"""
Load a basic floor in the world.