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

correct sensors

Browse Source
This commit is contained in:
Brian Delhaisse
2019-04-29 01:35:06 +02:00
parent 07248b3b02
commit d136069115
8 changed files with 43 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/robots/sensors/camera.py
+23 -23
View File
@@ -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
pyrobolearn/robots/sensors/contact.py
+3 -2
View File
@@ -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
pyrobolearn/robots/sensors/force_torque.py
+1 -1
View File
@@ -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])
pyrobolearn/robots/sensors/joints.py
+1 -1
View File
@@ -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):
pyrobolearn/robots/sensors/light.py
+1
View File
@@ -27,6 +27,7 @@ class LaserSensor(LinkSensor):
"""
class ProximityLaserSensor(LaserSensor):
r"""Proximity Laser Sensor
pyrobolearn/robots/sensors/links.py
+5 -3
View File
@@ -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
pyrobolearn/robots/sensors/misc.py
+1 -1
View File
@@ -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
pyrobolearn/robots/sensors/sensor.py
+8 -11
View File
@@ -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