diff --git a/pyrobolearn/robots/sensors/camera.py b/pyrobolearn/robots/sensors/camera.py index a78a78c..6f5a1a1 100644 --- a/pyrobolearn/robots/sensors/camera.py +++ b/pyrobolearn/robots/sensors/camera.py @@ -6,6 +6,7 @@ Cameras have one of the most richest sensory inputs (i.e. visual). import numpy as np +from pyrobolearn.utils.orientation import get_rpy_from_quaternion from pyrobolearn.robots.sensors.links import LinkSensor __author__ = "Brian Delhaisse" @@ -44,7 +45,7 @@ class CameraSensor(LinkSensor): Examples: sim = BulletClient(connection_mode=p.GUI) cam = Camera(sim, width=400, height=400, target_position=(0,0,0), eyePosition=(2,0,1)) - img = cam.getRGBImage() + img = cam.get_rgb_image() plt.imshow(img) plt.show() @@ -104,12 +105,12 @@ class CameraSensor(LinkSensor): # compute projection matrix (orthographic or perspective matrix) if left is not None and right is not None and bottom is not None and top is not None: # orthographic - self._P = self.sim.computeProjectionMatrix(left, right, bottom, top, near, far) + self._P = self.sim.compute_projection_matrix(left, right, bottom, top, near, far) else: # perspective # The aspect ratio parameter is the width divided by the height of the canvas window if aspect is None: aspect = float(width) / height - self._P = self.sim.computeProjectionMatrixFOV(fovy, aspect, nearVal=near, farVal=far) + self._P = self.sim.compute_projection_matrix_fov(fovy, aspect, near=near, far=far) # compute view matrix self._V = self.getV() @@ -136,7 +137,7 @@ class CameraSensor(LinkSensor): """ Get the associated view matrix. """ - roll, pitch, yaw = self.sim.getEulerFromQuaternion(self.orientation_converter(self.orientation)) + roll, pitch, yaw = get_rpy_from_quaternion(self.orientation) self.up_vector = (0, 0, 1) up_axis_index = 2 # z axis @@ -146,11 +147,10 @@ class CameraSensor(LinkSensor): -np.sin(pitch)]) # compute view matrix - self._V = self.sim.computeViewMatrix(cameraEyePosition=self.position, - cameraTargetPosition=target_position, - cameraUpVector=self.up_vector) + self._V = self.sim.compute_view_matrix(eye_position=self.position, target_position=target_position, + up_vector=self.up_vector) - # self._V = self.sim.computeViewMatrixFromYawPitchRoll(cameraTargetPosition=target_position, + # self._V = self.sim.compute_view_matrixFromYawPitchRoll(cameraTargetPosition=target_position, # distance=distance, # yaw=yaw, # pitch=pitch, @@ -159,54 +159,54 @@ class CameraSensor(LinkSensor): return self._V - def getRGBImage(self): + def get_rgb_image(self): """ Return the captured RGB image. """ - return self.getRGBAImage()[:, :, :3] + return self.get_rgba_image()[:, :, :3] - def getRGBAImage(self): + def get_rgba_image(self): """ Return the captured RGBA image. 'A' stands for alpha channel (for opacity/transparency) """ - img = np.array(self.sim.getCameraImage(self.width, self.height, self.getV(), self._P, - shadow=1, # lightDirection=[1,1,1], - # renderer=self.sim.ER_TINY_RENDERER)[2]) - renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[2]) + img = np.array(self.sim.get_camera_image(self.width, self.height, self.getV(), self._P, + shadow=1, # lightDirection=[1,1,1], + # renderer=self.sim.ER_TINY_RENDERER)[2]) + renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[2]) img = img.reshape(self.width, self.height, 4) # RGBA return img - def getDepthImage(self): + def get_depth_image(self): """ Return the depth image. """ - img = np.array(self.sim.getCameraImage(self.width, self.height, self.getV(), self._P, - renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[3]) + img = np.array(self.sim.get_camera_image(self.width, self.height, self.getV(), self._P, + renderer=self.sim.ER_BULLET_HARDWARE_OPENGL)[3]) img = img.reshape(self.width, self.height) return img - def getRGBADImage(self, concatenate=True): + def get_rgbad_image(self, concatenate=True): """ Return the RGBA and depth images. """ - rgba, depth = self.sim.getCameraImage(self.width, self.height, self.getV(), self._P)[2:4] + rgba, depth = self.sim.get_camera_image(self.width, self.height, self.getV(), self._P)[2:4] rgba = np.array(rgba).reshape(self.width, self.height, 4) depth = np.array(depth).reshape(self.width, self.height) if concatenate: return np.dstack((rgba, depth)) return rgba, depth - _sense = getRGBADImage + _sense = get_rgbad_image class DepthCameraSensor(CameraSensor): r"""Depth Camera sensor. """ - _sense = CameraSensor.getDepthImage + _sense = CameraSensor.get_depth_image class Camera2DSensor(CameraSensor): r"""2D camera sensor """ - _sense = CameraSensor.getRGBImage + _sense = CameraSensor.get_rgb_image diff --git a/pyrobolearn/robots/sensors/contact.py b/pyrobolearn/robots/sensors/contact.py index abfde2f..e0d6c1c 100644 --- a/pyrobolearn/robots/sensors/contact.py +++ b/pyrobolearn/robots/sensors/contact.py @@ -29,7 +29,7 @@ class ContactSensor(LinkSensor): Returns: list: list of contacts """ - contacts = self.sim.getContactPoints(bodyA=self.body_id, linkIndexA=self.link_id) + contacts = self.sim.get_contact_points(bodyA=self.body_id, linkIndexA=self.link_id) return contacts def is_in_contact(self): @@ -55,9 +55,10 @@ class PressureSensor(LinkSensor): raise NotImplementedError -class TourchSensor(LinkSensor): +class TouchSensor(LinkSensor): r"""Touch Sensor """ + pass # class SkinPressureSensor diff --git a/pyrobolearn/robots/sensors/force_torque.py b/pyrobolearn/robots/sensors/force_torque.py index fa75912..fc9e440 100644 --- a/pyrobolearn/robots/sensors/force_torque.py +++ b/pyrobolearn/robots/sensors/force_torque.py @@ -24,7 +24,7 @@ class ForceTorqueSensor(JointSensor): def __init__(self, simulator, body_id, joint_id, position, orientation, refresh_rate=1): super(ForceTorqueSensor, self).__init__(simulator, body_id, joint_id, position, orientation, refresh_rate) - self.sim.enableJointForceTorqueSensor(body_id, joint_id, enableSensor=True) + self.sim.enable_joint_force_torque_sensor(body_id, joint_id, enableSensor=True) def _sense(self): return np.array(self.sim.getJointState(self.body_id, self.joint_id)[2]) diff --git a/pyrobolearn/robots/sensors/joints.py b/pyrobolearn/robots/sensors/joints.py index 812b11f..48f3251 100644 --- a/pyrobolearn/robots/sensors/joints.py +++ b/pyrobolearn/robots/sensors/joints.py @@ -44,7 +44,7 @@ class JointSensor(Sensor): """ Return the joint position """ - return self.sim.JointState(self.body_id, self.joint_id)[0] + return self.sim.get_joint_state(self.body_id, self.joint_id)[0] @abstractmethod def _sense(self): diff --git a/pyrobolearn/robots/sensors/light.py b/pyrobolearn/robots/sensors/light.py index ec2614f..4fd1d53 100644 --- a/pyrobolearn/robots/sensors/light.py +++ b/pyrobolearn/robots/sensors/light.py @@ -27,6 +27,7 @@ class LaserSensor(LinkSensor): """ + class ProximityLaserSensor(LaserSensor): r"""Proximity Laser Sensor diff --git a/pyrobolearn/robots/sensors/links.py b/pyrobolearn/robots/sensors/links.py index 6172123..f6f22fb 100644 --- a/pyrobolearn/robots/sensors/links.py +++ b/pyrobolearn/robots/sensors/links.py @@ -6,8 +6,10 @@ These include IMU, contact, Camera, and other sensors. from abc import ABCMeta, abstractmethod +from pyrobolearn.utils.orientation import get_quaternion_product from pyrobolearn.robots.sensors.sensor import Sensor + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] @@ -45,7 +47,7 @@ class LinkSensor(Sensor): """ Return the link position in the Cartesian world frame. """ - position = self.pos_converter(self.sim.getLinkState(self.body_id, self.link_id)[0]) + position = self.sim.get_link_state(self.body_id, self.link_id)[0] position += self.local_position return position @@ -54,8 +56,8 @@ class LinkSensor(Sensor): """ Return the link orientation in the Cartesian world frame. """ - orientation = self.orientation_converter(self.sim.getLinkState(self.body_id, self.link_id)[1]) - orientation = self.local_orientation * orientation + orientation = self.sim.get_link_state(self.body_id, self.link_id)[1] + orientation = get_quaternion_product(self.local_orientation, orientation) return orientation @abstractmethod diff --git a/pyrobolearn/robots/sensors/misc.py b/pyrobolearn/robots/sensors/misc.py index 7ec817f..20595fc 100644 --- a/pyrobolearn/robots/sensors/misc.py +++ b/pyrobolearn/robots/sensors/misc.py @@ -73,7 +73,7 @@ class HeightmapSensor(Sensor): np.array: Height map with shape [width, height] where the values are the heights (in meters). """ # calculate width and height - collisions = self.sim.rayTestBatch(self.get_ray_from_positions(), self.get_ray_to_positions()) + collisions = self.sim.ray_test_batch(self.get_ray_from_positions(), self.get_ray_to_positions()) return collisions diff --git a/pyrobolearn/robots/sensors/sensor.py b/pyrobolearn/robots/sensors/sensor.py index 88b434d..0375fe2 100644 --- a/pyrobolearn/robots/sensors/sensor.py +++ b/pyrobolearn/robots/sensors/sensor.py @@ -10,9 +10,8 @@ add some noise to the returned sense value. The type of noise can also be select from abc import ABCMeta, abstractmethod import numpy as np -import quaternion -from pyrobolearn.utils.converter import QuaternionListConverter, NumpyListConverter +from pyrobolearn.utils.orientation import get_quaternion_product __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -47,15 +46,13 @@ class Sensor(object): # sensor attached to a link or joint self.sim = simulator self.body_id = body_id - self.pos_converter = NumpyListConverter(convention=0) if position is None: - position = np.array([0., 0., 0.]) - self.local_position = self.pos_converter.convertTo(position) + position = [0., 0., 0.] + self.local_position = np.array(position) - self.orientation_converter = QuaternionListConverter(convention=1) if orientation is None: - orientation = quaternion.quaternion(1., 0., 0., 0.) - self.local_orientation = self.orientation_converter.convertTo(orientation) + orientation = [0., 0., 0., 1.] + self.local_orientation = np.array(orientation) self.rate = refresh_rate self.cnt = -1 @@ -68,7 +65,7 @@ class Sensor(object): # sensor attached to a link or joint """ Return the body's CoM position in the Cartesian world frame. """ - position = self.pos_converter(self.sim.getBasePositionAndOrientation(self.body_id)[0]) + position = self.sim.get_base_position(self.body_id) position += self.local_position return position @@ -77,8 +74,8 @@ class Sensor(object): # sensor attached to a link or joint """ Return the body's CoM orientation in the Cartesian world frame. """ - orientation = self.orientation_converter(self.sim.getBasePositionAndOrientation(self.body_id)[1]) - orientation = self.local_orientation * orientation + orientation = self.sim.get_base_orientation(self.body_id) + orientation = get_quaternion_product(self.local_orientation, orientation) return orientation @abstractmethod