From cd2dc52d30417e2d02de2ad013924bdd75654235 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Sat, 13 Jul 2019 01:29:06 +0200 Subject: [PATCH] update simulators: played with mujoco --- pyrobolearn/filters/utils.py | 60 ++++ pyrobolearn/simulators/bullet.py | 413 +++++++++++++------------- pyrobolearn/simulators/mujoco.py | 438 +++++++++++++++++++++++++++- pyrobolearn/simulators/simulator.py | 394 +++++++++++++------------ pyrobolearn/simulators/vrep.py | 4 +- pyrobolearn/worlds/world.py | 25 ++ 6 files changed, 939 insertions(+), 395 deletions(-) diff --git a/pyrobolearn/filters/utils.py b/pyrobolearn/filters/utils.py index 58187e7..8dbb1c2 100644 --- a/pyrobolearn/filters/utils.py +++ b/pyrobolearn/filters/utils.py @@ -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 diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index c2986bb..8b45f09 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -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) diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index da33a33..ab0d5f0 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -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) diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index a36de20..cde020b 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -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 diff --git a/pyrobolearn/simulators/vrep.py b/pyrobolearn/simulators/vrep.py index dc7713c..bc1633a 100644 --- a/pyrobolearn/simulators/vrep.py +++ b/pyrobolearn/simulators/vrep.py @@ -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" diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index f6044c5..6a7731d 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -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.