def reset_arm_rnd(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
# Reset to uniform random state
# Joints 1, 4, 5 and 6 have a range of -10,000 to +10,000 degrees. Joint 2 has a range of +42 to +318 degrees. Joint 3 has a range of +17 to +343 degrees. (see http://wiki.ros.org/jaco)
reset_command.data = [
(random() * 2 - 1) * pi,
pi + (random() * 2 - 1) * pi / 2,
# Limit elbow joints to 180 +/-90 degrees to prevent getting stuck in the ground
pi + (random() * 2 - 1) * pi / 2,
(random() * 2 - 1) * pi,
(random() * 2 - 1) * pi,
(random() * 2 - 1) * pi
]
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
try:
self._reset_service.publish_and_wait(reset_command,
timeout=timeout)
except TimeoutException:
self.relax_arm(arm)
wait = raw_input(
'The robot arm seems to be stuck. Unstuck it and press <ENTER> to continue.'
)
self.reset_arm(arm, mode, data)
def reset_arm(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
reset_command.data = data
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
self._reset_service.publish_and_wait(reset_command, timeout=timeout)
def reset_arm(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
reset_command.data = data
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
self._reset_service.publish_and_wait(reset_command, timeout=timeout)
def reset_arm(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
reset_command.data = data
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
try:
self._reset_service.publish_and_wait(reset_command,
timeout=timeout)
except TimeoutException:
self.relax_arm(arm)
wait = raw_input(
'The robot arm seems to be stuck. Unstuck it and press <ENTER> to continue.'
)
self.reset_arm(arm, mode, data)