def get_position(self, relative_to=None) -> List[float]:
"""Gets the position of this object.
:param relative_to: Indicates relative to which reference frame we want
the position. Specify None to retrieve the absolute position, or an
Object relative to whose reference frame we want the position.
:return: A list containing the x, y, z position of the object.
"""
relto = -1 if relative_to is None else relative_to.get_handle()
return sim.simGetObjectPosition(self._handle, relto)
def get_position(self, relative_to=None) -> np.ndarray:
"""Gets the position of this object.
:param relative_to: Indicates relative to which reference frame we want
the position. Specify None to retrieve the absolute position, or an
Object relative to whose reference frame we want the position.
:return: An array containing the x, y, z position of the object.
"""
relto = -1 if relative_to is None else relative_to.get_handle()
position = sim.simGetObjectPosition(self._handle, relto)
return np.array(position, dtype=np.float64)
def get_dense_reward(self):
state, points = sim.simCheckProximitySensor(self._initial_objs_in_scene[5][0]._handle,
self.robot.arm.get_tip().get_handle())
points_1 = sim.simGetObjectPosition(self._initial_objs_in_scene[5][0]._handle,
self.robot.arm.get_tip().get_handle())
reward = - np.sqrt(points_1[0] ** 2 + points_1[1] ** 2 + points_1[2] ** 2)
if state == 1:
reward = 1.
return reward
def rotate(self, rotation: List[float]) -> None:
"""Rotates a transformation matrix.
:param rotation: The x, y, z rotation to perform (in radians).
"""
m = sim.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 = sim.simGetObjectPosition(self._handle, -1)
m = sim.simRotateAroundAxis(m, z_axis, axis_pos, rotation[2])
m = sim.simRotateAroundAxis(m, y_axis, axis_pos, rotation[1])
m = sim.simRotateAroundAxis(m, x_axis, axis_pos, rotation[0])
sim.simSetObjectMatrix(self._handle, -1, m)