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)
