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 _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 quaternion(self): """Return the orientation expressed as a quaternion [x,y,z,w].""" return get_quaternion_from_rpy(self.rpy)
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 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)