def Az2R(theta):
"""
Convert (axis) angle along z axis Az to rotation matrix R
"""
if isinstance(theta, np.ndarray):
R = np.zeros(theta.shape + (3, 3))
R[...] = constants.eye_R()
else:
R = constants.eye_R()
c = np.cos(theta)
s = np.sin(theta)
R[..., 0, 0] = c
R[..., 0, 1] = -s
R[..., 1, 0] = s
R[..., 1, 1] = c
return R
def Az2R(theta):
"""
Convert (axis) angle along z axis Az to rotation matrix R
"""
R = constants.eye_R()
c = np.cos(theta)
s = np.sin(theta)
R[0, 0] = c
R[0, 1] = -s
R[1, 0] = s
R[1, 1] = c
return R
def Ay2R(theta):
"""
Convert (axis) angle along y axis Ay to rotation matrix R
"""
R = constants.eye_R()
c = np.cos(theta)
s = np.sin(theta)
R[0, 0] = c
R[0, 2] = s
R[2, 0] = -s
R[2, 2] = c
return R
def create_ground(self):
''' Create Plane '''
if np.allclose(np.array([0.0, 0.0, 1.0]), self._v_up):
R_plane = constants.eye_R()
else:
R_plane = math.R_from_vectors(np.array([0.0, 0.0, 1.0]),
self._v_up)
self._plane_id = \
self._pb_client.loadURDF(
"plane_implicit.urdf",
[0, 0, 0],
conversions.R2Q(R_plane),
useMaximalCoordinates=True)
self._pb_client.changeDynamics(self._plane_id,
linkIndex=-1,
lateralFriction=0.9)
''' Dynamics parameters '''
assert np.allclose(np.linalg.norm(self._v_up), 1.0)
gravity = -9.8 * self._v_up
self._pb_client.setGravity(gravity[0], gravity[1], gravity[2])
self._pb_client.setTimeStep(self._dt_sim)
self._pb_client.setPhysicsEngineParameter(numSubSteps=2)
self._pb_client.setPhysicsEngineParameter(numSolverIterations=10)
def p2T(p):
return Rp2T(constants.eye_R(), np.array(p))
def translate(self, dp, frame_local=False):
R = self.get_cam_rotation() if frame_local else constants.eye_R()
dt = np.dot(R, dp)
self.pos += dt
self.origin += dt