def getV(self): """ Get the associated view matrix. """ roll, pitch, yaw = get_rpy_from_quaternion(self.orientation) self.up_vector = (0, 0, 1) up_axis_index = 2 # z axis # use the specified distance to calculate it. target_position = self.position + self.distance * np.array([ -np.cos(pitch) * np.cos(-yaw), -np.cos(pitch) * np.sin(-yaw), -np.sin(pitch) ]) # compute view matrix self._V = self.sim.compute_view_matrix(eye_position=self.position, target_position=target_position, up_vector=self.up_vector) # self._V = self.sim.compute_view_matrixFromYawPitchRoll(cameraTargetPosition=target_position, # distance=distance, # yaw=yaw, # pitch=pitch, # roll=roll, # upAxisIndex=up_axis_index) return self._V
def step(self, update_interface=False): """Perform a step: map the mouse-keyboard interface to the world""" # update interface if update_interface: self.interface() # check keyboard events self.check_key_events() # set camera view pitch, yaw = get_rpy_from_quaternion(self.robot.orientation)[1:] if self.fpv: # first-person view target_pos = self.robot.position + 2 * np.array([ np.cos(yaw) * np.cos(pitch), np.sin(yaw) * np.cos(pitch), np.sin(pitch) ]) self.camera.reset(distance=2, pitch=-pitch, yaw=yaw - np.pi / 2, target_position=target_pos) else: # third-person view self.camera.follow(body_id=self.robot.id, distance=2, yaw=yaw - np.pi / 2, pitch=self.camera_pitch)
def step(self, update_interface=False): """Perform a step: map the SpaceMouse interface to the end-effector.""" # update interface if update_interface: self.interface() # update the QP task desired position position = self.manipulator.get_link_positions( link_ids=self.distal_link, wrt_link_id=self.base_link) position += self.interface.translation self.task.desired_position = position # update the QP task desired orientation (if specified) if self.use_orientation: orientation = self.manipulator.get_link_orientations( link_ids=self.distal_link, wrt_link_id=self.base_link) orientation = get_rpy_from_quaternion(orientation) orientation += self.interface.rotation self.task.desired_orientation = get_quaternion_from_rpy( orientation) # update the QP task self.task.update(update_model=True) # solve QP and set the joint variables if self._control_type == ControlType.POSITION: # Position control # solve QP task dq = self.solver.solve() # set joint positions q = self.manipulator.get_joint_positions() q = q + dq * self.simulator.dt self.manipulator.set_joint_positions(q) else: # Impedance control # solve QP task torques = self.solver.solve() # set joint torques self.manipulator.set_joint_torques(torques) # check if gripper if self.gripper is not None: # check if button is pressed if self.interface.left_button_pressed: # open the gripper if self._control_type == ControlType.POSITION: # position control self.gripper.open(factor=1) else: # impedance control self.grasp_strength += 1 self.gripper.grasp(strength=self.grasp_strength) elif self.interface.right_button_pressed: # close the gripper if self._control_type == ControlType.POSITION: # position control self.gripper.close(factor=1) else: # impedance control self.grasp_strength -= 1 self.gripper.grasp(strength=self.grasp_strength)
def _write_continuous(self, data): """apply the action data on the robot.""" # get current orientations and convert them to RPY angles orientation = self.robot.get_link_world_orientations(link_ids=self.link) # (4,) orientation = get_rpy_from_quaternion(orientation) # (3,) # add change in orientations data += orientation # (3,) # convert them back to quaternions data = get_quaternion_from_rpy(data) # (4,) self.robot.set_link_positions(link_ids=self.link, orientations=data)
def _get_states(self): """Return the link orientation state.""" # get the link orientation if self.wrt_link is None: orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=True) else: orientation = self.robot.get_link_orientations(link_ids=self.link, wrt_link_id=self.wrt_link, flatten=True) # convert from quaternion to roll-pitch-yaw angles orientation = get_rpy_from_quaternion(orientation) # return the proper orientation if self.dim is None: return orientation return orientation[self.dim]
def _write_continuous(self, data): """apply the action data on the robot.""" # get current pose position = self.robot.get_link_world_poses(link_ids=self.link, flatten=False) # (3,) orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=False) # (4,) orientation = get_rpy_from_quaternion(orientation) # (3,) # add changes data[:3] += position # (3,) data[3:] += orientation # (3,) # convert back orientations to quaternions data = np.concatenate((data[:3], get_quaternion_from_rpy(data[3:]))) # (7,) # write poses self.robot.set_link_positions(link_ids=self.link, positions=data[:3], orientations=data[3:])
def orientation(self, orientation): """ Set the orientation (expressed as a quaternion, rotation matrix, or roll-pitch-yaw angles) of the camera. """ # convert the orientation to roll-pitch-yaw angle if orientation.shape == (4, ): # quaternion (x,y,z,w) rpy = get_rpy_from_quaternion(orientation) elif orientation.shape == (3, 3): # rotation matrix rpy = get_rpy_from_matrix(orientation) elif orientation.shape == (3, ): # roll-pitch-yaw angle rpy = orientation else: raise ValueError( "Expecting the given orientation to be a quaternion, rotation matrix, or Roll-Pitch-Yaw " "angles, instead got: {}".format(orientation)) # reset the camera distance, target_position = self.sim.get_debug_visualizer()[-2:] _, pitch, yaw = rpy self.reset(distance, yaw, pitch, target_position)
def _get_states(self): """Return the state.""" orientation = get_rpy_from_quaternion(self.body.orientation) if self.dim is None: return orientation return orientation[self.dim]
def rpy(self): """Return the orientation as the Roll-Pitch-Yaw angles.""" return get_rpy_from_quaternion(self.orientation)
# create robot scale = 1. # Warning: this does not scale the mass... position = [0., 0., np.sqrt(2) / 2. * scale + 0.001] orientation = [0.383, 0, 0, 0.924] robot = Cubli(sim, position, orientation, scale=scale) # print information about the robot robot.print_info() H = robot.get_mass_matrix(q_idx=slice( 6, 6 + len(robot.joints))) # floating base, thus keep only the last q print("Inertia matrix: H(q) = {}\n".format(H)) # PD control Kp = 600. Kd = 2 * np.sqrt(Kp) desired_roll = np.pi / 4. for i in count(): # get state quaternion = robot.get_base_orientation() w = robot.get_base_angular_velocity() euler = get_rpy_from_quaternion(quaternion) # PD control torques = [-Kp * (desired_roll - euler[0]) + Kd * w[0], 0., 0.] robot.set_joint_torques(torques) # step in simulation world.step(sleep_dt=1. / 240)
def step(self, update_interface=False): """Perform a step: map the Controller interface to the end-effector.""" # update interface if update_interface: self.interface() # update the QP task desired position left_joystick = self.interface.LJ[::-1] # (y,x) right_joystick = self.interface.RJ[::-1] # (y,x) translation = np.zeros(3) translation[:2] = left_joystick translation[2] = right_joystick[0] position = self.manipulator.get_link_positions( link_ids=self.distal_link, wrt_link_id=self.base_link) position += self.translation_scale * self.interface.translation self.task.desired_position = position # update the QP task desired orientation (if specified) if self.use_orientation: drpy = np.zeros(3) if self.interface.BTN_TL: drpy[0] = self.rotation_step elif self.interface.BTN_TR: drpy[0] = -self.rotation_step if self.interface.BTN_TL2: drpy[1] = self.rotation_step elif self.interface.BTN_TR2: drpy[1] = -self.rotation_step if self.interface.BTN_WEST: drpy[2] = self.rotation_step elif self.interface.BTN_EAST: drpy[2] = -self.rotation_step orientation = self.manipulator.get_link_orientations( link_ids=self.distal_link, wrt_link_id=self.base_link) orientation = get_rpy_from_quaternion(orientation) orientation += drpy self.task.desired_orientation = get_quaternion_from_rpy( orientation) # update the QP task self.task.update(update_model=True) # solve QP and set the joint variables if self._control_type == ControlType.POSITION: # Position control # solve QP task dq = self.solver.solve() # set joint positions q = self.manipulator.get_joint_positions() q = q + dq * self.simulator.dt self.manipulator.set_joint_positions(q) else: # Impedance control # solve QP task torques = self.solver.solve() # set joint torques self.manipulator.set_joint_torques(torques) # check if gripper if self.gripper is not None: # check if button is pressed if self.interface.BTN_NORTH: # open the gripper if self._control_type == ControlType.POSITION: # position control self.gripper.open(factor=1) else: # impedance control self.grasp_strength += 0.1 self.gripper.grasp(strength=self.grasp_strength) elif self.interface.BTN_SOUTH: # close the gripper if self._control_type == ControlType.POSITION: # position control self.gripper.close(factor=1) else: # impedance control self.grasp_strength -= 0.1 self.gripper.grasp(strength=self.grasp_strength)
def create_primitive_object(self, shape_type, position, mass, orientation=(0., 0., 0., 1.), radius=0.5, half_extents=(.5, .5, .5), height=1., filename=None, mesh_scale=(1., 1., 1.), plane_normal=(0., 0., 1.), rgba_color=None, specular_color=None, frame_position=None, frame_orientation=None, vertices=None, indices=None, uvs=None, normals=None, flags=-1): """Create a primitive object in the simulator. This is basically the combination of `create_visual_shape`, `create_collision_shape`, and `create_body`. Args: shape_type (int): type of shape; GEOM_SPHERE (=2), GEOM_BOX (=3), GEOM_CAPSULE (=7), GEOM_CYLINDER (=4), GEOM_PLANE (=6), GEOM_MESH (=5) position (np.array[float[3]]): Cartesian world position of the base mass (float): mass of the base, in kg (if using SI units) orientation (np.array[float[4]]): Orientation of base as quaternion [x,y,z,w] radius (float): only for GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER half_extents (np.array[float[3]], list/tuple of 3 floats): only for GEOM_BOX. height (float): only for GEOM_CAPSULE, GEOM_CYLINDER (height = length). filename (str): Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each object (marked as 'o') in the .obj file. mesh_scale (np.array[float[3]], list/tuple of 3 floats): scale of mesh (only for GEOM_MESH). plane_normal (np.array[float[3]], list/tuple of 3 floats): plane normal (only for GEOM_PLANE). rgba_color (list/tuple of 4 floats): color components for red, green, blue and alpha, each in range [0..1]. specular_color (list/tuple of 3 floats): specular reflection color, red, green, blue components in range [0..1] frame_position (np.array[float[3]]): translational offset of the visual and collision shape with respect to the link frame. frame_orientation (np.array[float[4]]): rotational offset (quaternion x,y,z,w) of the visual and collision shape with respect to the link frame. vertices (list[np.array[float[3]]]): Instead of creating a mesh from obj file, you can provide vertices, indices, uvs and normals indices (list[int]): triangle indices, should be a multiple of 3. uvs (list of np.array[2]): uv texture coordinates for vertices. Use `changeVisualShape` to choose the texture image. The number of uvs should be equal to number of vertices. normals (list[np.array[float[3]]]): vertex normals, number should be equal to number of vertices. flags (int): unused / to be decided Returns: int: non-negative unique id for primitive object, or -1 for failure """ # check given parameters color = rgba_color[:3] if rgba_color is not None else None orientation = get_rpy_from_quaternion( orientation).tolist() if orientation is not None else None static = True if mass == 0 else False mass = mass if mass <= 0 else mass position = list(position) # shape if shape_type == self.GEOM_BOX: size = [ 2 * half_extents[0], 2 * half_extents[1], 2 * half_extents[2] ] shape = Shape.create(PrimitiveShape.CUBOID, size=size, mass=mass, color=color, position=position, orientation=orientation, static=static) elif shape_type == self.GEOM_SPHERE: shape = Shape.create(PrimitiveShape.SPHERE, size=[1.], mass=mass, color=[1, 0, 0], position=position, orientation=orientation, static=static) elif shape_type == self.GEOM_CYLINDER: shape = Shape.create(PrimitiveShape.CYLINDER, size=[radius, height], mass=mass, color=color, position=position, orientation=orientation, static=static) elif shape_type == self.GEOM_CONE: shape = Shape.create(PrimitiveShape.CONE, size=[radius, height], mass=color, position=position, orientation=orientation, static=static) # elif shape_type == self.GEOM_MESH: # # TODO: use trimesh library # shape = Shape.create_mesh() else: raise NotImplementedError( "Primitive object not defined for the given shape type.") # save shape and return unique id self._body_cnt += 1 self._bodies[self._body_cnt] = shape return self._body_cnt
def step(self, update_interface=False): """Perform a step: map the SpaceMouse interface to the end-effector.""" # update interface if update_interface: self.interface() # get interface values key, pressed, down = self.interface.key, self.interface.key_pressed, self.interface.key_down # update the QP task desired position translation = np.zeros(3) if key.ctrl not in pressed: if key.top_arrow in down: translation[0] = self.translation_step elif key.bottom_arrow in down: translation[0] = -self.translation_step elif key.left_arrow in down: translation[1] = self.translation_step elif key.right_arrow in down: translation[1] = -self.translation_step elif key.enter in down: translation[2] = self.translation_step elif key.shift in down: translation[2] = -self.translation_step position = self.manipulator.get_link_positions( link_ids=self.distal_link, wrt_link_id=self.base_link) position += translation self.task.desired_position = position # update the QP task desired orientation (if specified) if self.use_orientation: drpy = np.zeros(3) if key.ctrl in pressed: if key.top_arrow in down: drpy[1] = self.rotation_step elif key.bottom_arrow in down: drpy[1] = -self.rotation_step elif key.left_arrow in down: drpy[2] = self.rotation_step elif key.right_arrow in down: drpy[2] = -self.rotation_step elif key.enter in down: drpy[0] = self.rotation_step elif key.shift in down: drpy[0] = -self.rotation_step orientation = self.manipulator.get_link_orientations( link_ids=self.distal_link, wrt_link_id=self.base_link) orientation = get_rpy_from_quaternion(orientation) orientation += drpy self.task.desired_orientation = get_quaternion_from_rpy( orientation) # update the QP task self.task.update(update_model=True) # solve QP and set the joint variables if self._control_type == ControlType.POSITION: # Position control # solve QP task dq = self.solver.solve() # set joint positions q = self.manipulator.get_joint_positions() q = q + dq * self.simulator.dt self.manipulator.set_joint_positions(q) else: # Impedance control # solve QP task torques = self.solver.solve() # set joint torques self.manipulator.set_joint_torques(torques) # check if gripper if self.gripper is not None: # check if button is pressed if key.n0 in down: # open the gripper if self._control_type == ControlType.POSITION: # position control self.gripper.open(factor=1) else: # impedance control self.grasp_strength += 0.1 self.gripper.grasp(strength=self.grasp_strength) elif key.n1 in down: # close the gripper if self._control_type == ControlType.POSITION: # position control self.gripper.close(factor=1) else: # impedance control self.grasp_strength -= 0.1 self.gripper.grasp(strength=self.grasp_strength)