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