def point_in_cylinders(self, point):
"""
Assumes the point is with respect to the world
:param point:
:return:
"""
vector = np.ones((4, 1))
vector[:-1] = point.reshape(3, 1)
link_mat = np.identity(4)
for idx, link in enumerate(self.robot_links):
link_handle = self.get_object_handle(link)
link_mat[:-1, :] = np.array(vrep.simGetObjectMatrix(link_handle, self.world_handle)).reshape(3, 4)
# link_mat_inverse = self.get_inverse(link_mat)
new_point = np.matmul(link_mat, vector)
new_point = new_point[:-1].flatten()
if idx != 3:
r = self.link_cylinders[idx].radius
h = self.link_cylinders[idx].height
if h / 2 > new_point[2] > -h / 2 and math.sqrt(new_point[0] ** 2 + new_point[1] ** 2) < r:
return True
else:
return False
else:
r = self.link_cylinders[idx].radius
if np.linalg.norm(new_point) < r:
return True
else:
return False
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
# 5 points in 3D space forming the trajectory
self.action_space = Box(low=-0.3, high=0.3, shape=(5,3), dtype=np.float32)
self.observation_space = Box(low=np.array([-0.1, -0.2, 0, -1.5, 0.0, 0.0, -0.5, -0.2, -0.2]),
high=np.array([0.1, 1.7, 2.5, 0, 0.0, 0.0, 0.5, 0.2, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1.2
self.joints = [Joint('UR10_joint1'), Joint('UR10_joint2'), Joint('UR10_joint3'), Joint('UR10_joint4'), Joint('UR10_joint5'), Joint('UR10_joint6')]
self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
for j in self.joints]
print(self.joints_limits)
self.agent_hand = Shape('hand')
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip().get_quaternion()
self.target = Dummy('UR10_target')
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
self.table_target = Dummy('table_target')
# objects
self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(np.array(self.initial_pollo_position)-np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(self.camera.get_handle(),-1)
self.np_camera_matrix_extrinsics = np.delete(np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width/2.0) / math.tan(angle/2.0)
focaly_px = (height/2.0) / math.tan(angle/2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()
def rotate(self, rotation: List[float]) -> None:
"""Rotates a transformation matrix.
:param rotation: The x, y, z rotation to perform (in radians).
"""
m = vrep.simGetObjectMatrix(self._handle, -1)
x_axis = [m[0], m[4], m[8]]
y_axis = [m[1], m[5], m[9]]
z_axis = [m[2], m[6], m[10]]
axis_pos = vrep.simGetObjectPosition(self._handle, -1)
m = vrep.simRotateAroundAxis(m, z_axis, axis_pos, rotation[2])
m = vrep.simRotateAroundAxis(m, y_axis, axis_pos, rotation[1])
m = vrep.simRotateAroundAxis(m, x_axis, axis_pos, rotation[0])
vrep.simSetObjectMatrix(self._handle, -1, m)
def get_base_actuation(self):
"""PIDV controller.
:return: A list with actuations including thrust and three other values,
and a bool representing target is reached.
"""
relative_targetPos = self.target_base.get_position(relative_to=self)
relative_targetOri = self.get_orientation(relative_to=self)
if (sqrt((relative_targetPos[0])**2 +
(relative_targetPos[1])**2) - self.dist1
) < 0.1 and relative_targetOri[-1] < 0.1 * np.pi / 180:
return [0, 0, 0, 0], True
# Vertical control:
targetPos = self.intermediate_target_base.get_position()
pos = self.get_position()
l, _ = vrep.simGetVelocity(self.get_handle())
e = (targetPos[2] - pos[2])
self.cumul = self.cumul + e
pv = self.pParam * e
thrust = self.static_propeller_velocity + pv + self.iParam * self.cumul + self.dParam * (
e - self.lastE) + l[2] * self.vParam
self.lastE = e
# Horizontal control:
sp = self.intermediate_target_base.get_position(relative_to=self)
m = vrep.simGetObjectMatrix(self.get_handle(), -1)
vx = [1, 0, 0, 1]
vx = np.matmul(np.array(m + [0, 0, 0, 1]).reshape(4, 4), vx)[:3]
vy = [0, 1, 0, 1]
vy = np.matmul(np.array(m + [0, 0, 0, 1]).reshape(4, 4), vy)[:3]
alphaE = (vy[2] - m[11])
alphaCorr = 0.25 * alphaE + 2.1 * (alphaE - self.pAlphaE)
betaE = (vx[2] - m[11])
betaCorr = -0.25 * betaE - 2.1 * (betaE - self.pBetaE)
self.pAlphaE = alphaE
self.pBetaE = betaE
alphaCorr = alphaCorr + sp[1] * 0.005 + 1 * (sp[1] - self.psp2)
betaCorr = betaCorr - sp[0] * 0.005 - 1 * (sp[0] - self.psp1)
self.psp2 = sp[1]
self.psp1 = sp[0]
# Rotational control:
euler = self.get_orientation(relative_to=self.intermediate_target_base)
rotCorr = euler[2] * 0.1 + 2 * (euler[2] - self.prevEuler)
self.prevEuler = euler[2]
return [thrust, alphaCorr, betaCorr, rotCorr], False
def get_matrix(self, relative_to=None) -> List[float]:
"""Retrieves the transformation matrix of this object.
:param relative_to: Indicates relative to which reference frame we want
the matrix. Specify None to retrieve the absolute transformation
matrix, or an Object relative to whose reference frame we want the
transformation matrix.
:return: A list of 12 float values (the last row of the 4x4 matrix (
0,0,0,1) is not needed).
The x-axis of the orientation component is (m[0], m[4], m[8])
The y-axis of the orientation component is (m[1], m[5], m[9])
The z-axis of the orientation component is (m[2], m[6], m[10])
The translation component is (m[3], m[7], m[11])
"""
relto = -1 if relative_to is None else relative_to.get_handle()
return vrep.simGetObjectMatrix(self._handle, relto)
def get_link_tf_masks(self, point_cloud):
mat = np.identity(4)
links = [list(), list(), list(), list()]
_, point_cloud = self.zero_filter(point_cloud)
for point in point_cloud:
for idx in range(len(self.link_handles)):
l = self.link_handles[idx]
world2link = np.array(vrep.simGetObjectMatrix(l, self.world_handle)).reshape(3, 4)
mat[:-1, :-1] = world2link
link2world = np.linalg.inv(mat)
point_tx = np.matmul(link2world, point)
c = self.link_cylinders[idx]
if self.point_in_cylinder(c, point_tx):
links[idx].append(True)
else:
links[idx].append(False)
def get_dist_transform(self, sensor_handle):
raw_dist, object_name = self.get_euc_distance(sensor_handle)
sensor_tf = vrep.simGetObjectMatrix(sensor_handle, self.world_handle)
sensor_mat = np.zeros((4, 4))
k = 0
# raw dist lower bound goes here
if raw_dist < 0.0:
raw_dist = 0.0
# populate the matrix
if raw_dist != 0:
for i in range(0, 3):
sensor_mat[i] = sensor_tf[k:k + 4]
k += 4
sensor_mat[-1] = [0, 0, 0, 1] # last row
distance_mat = np.identity(4)
distance_mat[2, 3] = raw_dist
# print(sensor_mat)
distance_mat = np.matmul(sensor_mat, distance_mat)
dist_point = distance_mat[:, -1][:-1] # last column till the last element is a point in (x,y,z) form
# print(distance_mat)
if dist_point[-1] == 0:
dist_point = np.zeros(3)
return distance_mat, dist_point, object_name
def __init__(self, ep_length=100):
"""
Pollos environment for testing purposes
:param dim: (int) the size of the dimensions you want to learn
:param ep_length: (int) the length of each episodes in timesteps
"""
self.vrep = vrep
logging.basicConfig(level=logging.DEBUG)
# they have to be simmetric. We interpret actions as angular increments
#self.action_space = Box(low=np.array([-0.1, -1.7, -2.5, -1.5, 0.0, 0.0]),
# high=np.array([0.1, 1.7, 2.5, 1.5, 0.0, 0.0]))
inc = 0.1
self.action_space = Box(low=np.array([-inc, -inc, -inc, -inc, 0,
-inc]),
high=np.array([inc, inc, inc, inc, 0, inc]))
self.observation_space = Box(low=np.array([-0.5, 0.0, -0.2]),
high=np.array([0.5, 1.0, 1.0]))
#
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = UR10()
self.agent.max_velocity = 1
self.agent.set_control_loop_enabled(True)
self.agent.set_motor_locked_at_zero_velocity(True)
self.joints = [
Joint('UR10_joint1'),
Joint('UR10_joint2'),
Joint('UR10_joint3'),
Joint('UR10_joint4'),
Joint('UR10_joint5'),
Joint('UR10_joint6')
]
#self.joints_limits = [[j.get_joint_interval()[1][0],j.get_joint_interval()[1][0]+j.get_joint_interval()[1][1]]
# for j in self.joints]
self.high_joints_limits = [0.1, 1.7, 2.7, 0.0, 0.02, 0.3]
self.low_joints_limits = [-0.1, -0.2, 0.0, -1.5, -0.02, -0.5]
#self.agent_hand = Shape('hand')
self.initial_agent_tip_position = self.agent.get_tip().get_position()
self.initial_agent_tip_quaternion = self.agent.get_tip(
).get_quaternion()
self.initial_agent_tip_euler = self.agent.get_tip().get_orientation()
self.target = Dummy('UR10_target')
self.initial_target_orientation = self.target.get_orientation()
self.initial_joint_positions = self.agent.get_joint_positions()
self.pollo_target = Dummy('pollo_target')
self.pollo = Shape('pollo')
#self.table_target = Dummy('table_target')
self.initial_pollo_position = self.pollo.get_position()
self.initial_pollo_orientation = self.pollo.get_quaternion()
#self.table_target = Dummy('table_target')
#self.succion = Shape('suctionPad')
self.waypoints = [Dummy('dummy_gancho%d' % i) for i in range(4)]
# objects
#self.scene_objects = [Shape('table0'), Shape('Plane'), Shape('Plane0'), Shape('ConcretBlock')]
#self.agent_tip = self.agent.get_tip()
self.initial_distance = np.linalg.norm(
np.array(self.initial_pollo_position) -
np.array(self.initial_agent_tip_position))
# camera
self.camera = VisionSensor('kinect_depth')
self.camera_matrix_extrinsics = vrep.simGetObjectMatrix(
self.camera.get_handle(), -1)
self.np_camera_matrix_extrinsics = np.delete(
np.linalg.inv(self.vrepToNP(self.camera_matrix_extrinsics)), 3, 0)
width = 640.0
height = 480.0
angle = math.radians(57.0)
focalx_px = (width / 2.0) / math.tan(angle / 2.0)
focaly_px = (height / 2.0) / math.tan(angle / 2.0)
self.np_camera_matrix_intrinsics = np.array([[-focalx_px, 0.0, 320.0],
[0.0, -focalx_px, 240.0],
[0.0, 0.0, 1.0]])
self.reset()