def test_get_state(self):
"""Tests querying the state of multiple groups."""
sim_scene = MockSimScene(nq=10) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 3, 5],
'motor_ids': [10, 20, 12, 21],
'calib_scale': [0.5] * 4,
'calib_offset': [1] * 4,
},
'b': {
'qpos_indices': [2, 6],
},
'c': {
'motor_ids': [22, 24],
},
},
device_path='test')
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test']
dxl.write_desired_pos([10, 12, 20, 21, 22, 24], [1, 2, 3, 4, 5, 6])
a_state, b_state, c_state = robot.get_state(['a', 'b', 'c'])
np.testing.assert_allclose(a_state.qpos, [1.5, 2.5, 2., 3.])
np.testing.assert_allclose(a_state.qvel, [0., 0., 0., 0.])
self.assertIsNone(b_state.qpos)
self.assertIsNone(b_state.qvel)
np.testing.assert_allclose(c_state.qpos, [5., 6.])
np.testing.assert_allclose(c_state.qvel, [0., 0.])
np.testing.assert_allclose(sim_scene.data.qpos,
[1.5, 2.5, 0, 2., 0, 3., 0, 0, 0, 0])
def test_step(self):
"""Tests stepping with an action for multiple groups."""
sim_scene = MockSimScene(nq=10, ctrl_range=[-1, 1]) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 2],
'motor_ids': [10, 11, 12],
'calib_offset': [1] * 3,
'calib_scale': [-1] * 3,
'qpos_range':
[(-0.5, 0.5)] * 3,
},
'b': {
'qpos_indices': [2, 3],
},
},
device_path='test')
with patch_time('robel.components.robot.hardware_robot.time'):
robot.step({
'a': np.array([.2, .4, .6]),
'b': np.array([.1, .3]),
})
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test'] # type: Any
np.testing.assert_allclose(dxl.qpos, [.9, .8, .7])
def flail(robot: DynamixelRobotComponent):
"""Commands the robot to flail if it's stuck on an obstacle."""
for _ in range(6):
robot.set_state(
{'all': RobotState(qpos=(np.random.rand(12) - .5) * 3)},
**SET_PARAMS,
timeout=.15,
)
def test_engage_motors(self):
"""Tests engaging/disengaging subsets of motors."""
sim_scene = MockSimScene(nq=10) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'motor_ids': [10, 11, 12],
},
'b': {
'motor_ids': [13, 14, 15],
},
'c': {
'motor_ids': [12, 15],
}
},
device_path='test')
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test'] # type: Any
np.testing.assert_array_equal(dxl.enabled, [False] * 6)
robot.set_motors_engaged('a', True)
np.testing.assert_array_equal(dxl.enabled, [True] * 3 + [False] * 3)
robot.set_motors_engaged('c', False)
np.testing.assert_array_equal(dxl.enabled,
[True, True, False, False, False, False])
robot.set_motors_engaged(['a', 'b'], True)
np.testing.assert_array_equal(dxl.enabled, [True] * 6)
def reset_standup(robot: DynamixelRobotComponent, tracker: TrackerComponent):
"""Resets the D'Kitty to a standing position."""
start_time = time.time()
reset = False
num_attempts = 0
# Is it high, flat, and are all of it's joints at 0
while not is_standing(robot, tracker) and not reset:
# Splay the kitty out and let limp
robot.set_state(
{
'base':
RobotState(qpos=np.array(
[-np.pi / 2.1, np.pi / 2.1, np.pi / 2.1, -np.pi / 2.1])),
'feet':
RobotState(qpos=np.full(4, FOOTMAX)),
'middle':
RobotState(qpos=np.full(4, -MIDMAX))
},
**SET_PARAMS,
timeout=5,
)
time.sleep(1)
robot.set_motors_engaged('dkitty', engaged=False)
time.sleep(.3)
robot.set_motors_engaged('dkitty', engaged=True)
if tracker.is_hardware:
# If the robot has a tracker use it to do the reset
# Gravity tells us whether it's on its back
base_state = np.abs(robot.get_state('base').qpos)
if np.all(base_state > 1.75) and np.all(base_state < 2.45):
# Use base joints because tracker can't be seen
back_recover(robot)
elif (
is_flat(tracker) and get_height(tracker) < .17
and np.all(base_state < 1.75) and np.all(base_state > 1.1)
): # Tracker values tells us for sure whether it's on its chest
chest_recover(robot)
else:
flail(robot)
else:
# If the robot doesn't have a tracker, use gravity
base_state = np.abs(robot.get_state('base').qpos)
if np.all(base_state < 1.75) and np.all(base_state > 1.1):
chest_recover(robot)
reset = True
elif np.all(base_state > 1.75) and np.all(base_state < 2.45):
back_recover(robot)
reset = True
else:
flail(robot)
num_attempts += 1
logging.info('Total reset time: %1.2f over %d attempts',
time.time() - start_time, num_attempts)
def straighten_legs(robot: DynamixelRobotComponent, reverse: bool = False):
"""Straightens out the legs of the D'Kitty."""
front_state, back_state = robot.get_state(['front', 'back'])
frontpos = front_state.qpos
backpos = back_state.qpos
states = []
resolution = 4
for i in range(resolution, -1, -1):
if reverse:
states.append(dict(back=RobotState(qpos=backpos * i / resolution)))
states.append(
dict(front=RobotState(qpos=frontpos * i / resolution)))
else:
states.append(
dict(front=RobotState(qpos=frontpos * i / resolution)))
states.append(dict(back=RobotState(qpos=backpos * i / resolution)))
for state in states:
robot.set_state(state, **SET_PARAMS)
time.sleep(.2)
time.sleep(1)
def test_set_state(self):
"""Tests stepping with an action for multiple groups."""
sim_scene = MockSimScene(nq=10) # type: Any
robot = DynamixelRobotComponent(sim_scene,
groups={
'a': {
'qpos_indices': [0, 1, 2],
'motor_ids': [10, 11, 12],
'calib_offset': [-1] * 3,
'qpos_range':
[(-2.5, 2.5)] * 3,
},
},
device_path='test')
with patch_time('robel.components.robot.hardware_robot.time'):
robot.set_state({
'a': RobotState(qpos=np.array([1, 2, 3])),
})
dxl = DynamixelRobotComponent.DEVICE_CLIENTS['test'] # type: Any
np.testing.assert_allclose(dxl.qpos, [2, 3, 3.5])
def chest_recover(robot: DynamixelRobotComponent):
"""Recovers the robot when it's laying flat on its chest."""
robot.set_state(
{
'middle':
RobotState(qpos=np.array([-MIDMAX, -MIDMAX, -MIDMAX, -MIDMAX])),
'feet':
RobotState(qpos=np.array([FOOTMAX, FOOTMAX, FOOTMAX, FOOTMAX]))
}, **SET_PARAMS)
# Wiggle back and forth to get the feet fully underneath the dkitty.
base_position = np.array([BASEMAX, BASEMAX, BASEMAX, BASEMAX])
for i in range(2):
robot.set_state(
{
'base':
RobotState(qpos=(base_position if i %
2 == 0 else -base_position))
},
**SET_PARAMS,
timeout=1,
)
time.sleep(1)
robot.set_state({'base': RobotState(qpos=np.zeros(4))}, **SET_PARAMS)
straighten_legs(robot, True)
def build(self, *args, **kwargs):
"""Builds the component."""
if self._dxl_device_path:
if self._calibration_map is not None:
self._calibration_map.update_group_configs(self.group_configs)
from robel.components.robot.dynamixel_robot import (
DynamixelRobotComponent)
return DynamixelRobotComponent(*args,
groups=self.group_configs,
device_path=self._dxl_device_path,
**kwargs)
from robel.components.robot.robot import RobotComponent
return RobotComponent(*args, groups=self.group_configs, **kwargs)
def is_standing(robot: DynamixelRobotComponent, tracker: TrackerComponent):
"""Returns True if the D'Kitty is standing.
Checks the rotation and height of the tracker and whether all joints are 0
in case the other two fail.
"""
if tracker.is_hardware:
flat = is_flat(tracker)
h1 = get_height(tracker)
time.sleep(.2)
h2 = get_height(tracker)
tall = h1 != h2 and h1 > .175
straight_legs = np.all(abs(robot.get_state('all').qpos) < .15)
return flat and tall and straight_legs
return False
def reset_to_states(robot: DynamixelRobotComponent,
states: Dict[str, RobotState]):
"""Resets the D'Claw to the given states.
This is an multi-stage reset procedure that allows human intervention if
motors are not resetting properly.
Args:
robot: The robot component to reset.
states: The states to apply to the robot.
"""
assert robot.is_hardware
claw_state = states['dclaw']
has_object = 'object' in robot.groups
# Disable the top and bottom joints of the claw to help prevent tangling.
robot.set_motors_engaged('dclaw_top', False)
robot.set_motors_engaged('dclaw_bottom', False)
robot.set_state({'dclaw': claw_state}, block=False)
reset_start_time = time.time()
# Reset the object and guide motors while the claw is moving.
if has_object:
robot.set_motors_engaged(['object', 'guide'], True)
robot.set_state(
{
'object': states['object'],
'guide': states['guide'],
},
timeout=10,
)
# Wait a minimum time before fully resetting the claw.
reset_elapsed = time.time() - reset_start_time
if reset_elapsed < MINIMUM_CLAW_RESET_TIME:
time.sleep(MINIMUM_CLAW_RESET_TIME - reset_elapsed)
# Fully reset the D'Claw.
robot.set_motors_engaged('dclaw', True)
robot.set_state({'dclaw': claw_state})
# Check that the motors have actually reset.
reset_retries = 0
while True:
cur_state = robot.get_state('dclaw')
# Check positions one motor at a time for better diagnosing.
bad_motors = []
for i, motor_id in enumerate(robot.get_config('dclaw').motor_ids):
if abs(cur_state.qpos[i] - claw_state.qpos[i]) > GOOD_ERROR_TOL:
bad_motors.append(motor_id)
if not bad_motors:
break
# Attempt to reset again.
logging.error('[%d] Could not reset D\'Claw motors: %s', reset_retries,
str(bad_motors))
reset_retries += 1
# Wait for human assistance if too many resets have occurred.
if reset_retries > MAX_RESET_RETRIES:
print('\n' + '=' * 10)
print('Please fix motors: {}'.format(bad_motors))
print('=' * 10)
input('Press Enter to resume.')
reset_retries = 0
# Try to disentangle.
disentangle_dclaw(robot, claw_state.qpos)
# Re-attempt the reset.
robot.set_motors_engaged('dclaw', True)
robot.set_state({'dclaw': claw_state})
# Start the episode with the object disengaged.
if has_object:
robot.set_motors_engaged('object', False)
robot.reset_time()
def disentangle_dclaw(robot: DynamixelRobotComponent, goal_pos: np.ndarray):
"""Performs a disentangling process to move to the given goal position."""
assert goal_pos.shape == (9,)
# Let the motors rest.
robot.set_motors_engaged('dclaw', False)
time.sleep(DISENTANGLE_INTERVAL_TIME)
# Move the top joints upwards to free the lower joints.
robot.set_motors_engaged('dclaw_top', True)
robot.set_state({'dclaw_top': RobotState(qpos=TOP_DISENTANGLE_RESET_POS)},
block=False)
time.sleep(DISENTANGLE_INTERVAL_TIME)
# Reset the middle joints.
robot.set_motors_engaged('dclaw_middle', True)
robot.set_state({'dclaw_middle': RobotState(qpos=goal_pos[[1, 4, 7]])},
block=False)
time.sleep(DISENTANGLE_INTERVAL_TIME)
# Reset the lower joints.
robot.set_motors_engaged('dclaw_bottom', True)
robot.set_state({'dclaw_bottom': RobotState(qpos=goal_pos[[2, 5, 8]])},
block=False)
time.sleep(DISENTANGLE_INTERVAL_TIME)
def back_recover(robot: DynamixelRobotComponent):
"""Recovers the D'Kitty from a flipped over position."""
# Fixed legs in straight and put all base joints at same max angle.
# Illustration: _._._ -> _‾/
robot.set_state(
{
'base': RobotState(qpos=np.full(4, -BASEMAX)),
'middle': RobotState(qpos=np.zeros(4)),
'feet': RobotState(qpos=np.zeros(4))
}, **SET_PARAMS)
time.sleep(1)
# Use the legs to shift its CG and get onto its side. -> _‾|
robot.set_state({'base': RobotState(qpos=np.zeros(4))}, **SET_PARAMS)
robot.set_state(
{
'middle': RobotState(qpos=np.array([0, -MIDMAX, -MIDMAX, 0])),
'feet': RobotState(qpos=np.array([0, FOOTMAX, FOOTMAX, 0]))
}, **SET_PARAMS)
cbstate = np.array([BASEMAX, BASEMAX / 1.4, BASEMAX / 1.4, BASEMAX])
robot.set_state({'base': RobotState(qpos=cbstate)}, **SET_PARAMS)
robot.set_state(
{
'middle': RobotState(qpos=np.zeros(4)),
'feet': RobotState(qpos=np.zeros(4))
}, **SET_PARAMS)
# Fully fold legs causing the dkitty to tip onto its feet, doing this and
# the previous step separately decreases the possibility of binding. -> ̷‾̷
robot.set_state(
{
'base': RobotState(qpos=np.full(4, BASEMAX)),
'feet': RobotState(qpos=np.zeros(4)),
'middle': RobotState(qpos=np.array([-MIDMAX, 0, 0, -MIDMAX]))
}, **SET_PARAMS)
time.sleep(.5)
robot.set_state(
{
'feet': RobotState(qpos=np.full(4, FOOTMAX)),
'middle': RobotState(qpos=np.full(4, -MIDMAX))
}, **SET_PARAMS)
# Pivot the legs so that the kitty is now in a pouncing stance. -> |‾|
robot.set_state({'base': RobotState(qpos=np.zeros(4))}, **SET_PARAMS)
time.sleep(1)
straighten_legs(robot, True)