rot_basejoint_to_basecom)
for gripper_joint in gripper_joints:
del sensor_data['joint_pos'][gripper_joint]
del sensor_data['joint_vel'][gripper_joint]
rf_height = pybullet_util.get_link_iso(robot,
link_id['r_foot_contact'])[2, 3]
lf_height = pybullet_util.get_link_iso(robot,
link_id['l_foot_contact'])[2, 3]
sensor_data['b_rf_contact'] = True if rf_height <= 0.01 else False
sensor_data['b_lf_contact'] = True if lf_height <= 0.01 else False
# Get Keyboard Event
keys = p.getKeyboardEvents()
if pybullet_util.is_key_triggered(keys, '8'):
interface.interrupt_logic.b_interrupt_button_eight = True
elif pybullet_util.is_key_triggered(keys, '5'):
interface.interrupt_logic.b_interrupt_button_five = True
elif pybullet_util.is_key_triggered(keys, '4'):
interface.interrupt_logic.b_interrupt_button_four = True
elif pybullet_util.is_key_triggered(keys, '2'):
interface.interrupt_logic.b_interrupt_button_two = True
elif pybullet_util.is_key_triggered(keys, '6'):
interface.interrupt_logic.b_interrupt_button_six = True
elif pybullet_util.is_key_triggered(keys, '7'):
interface.interrupt_logic.b_interrupt_button_seven = True
elif pybullet_util.is_key_triggered(keys, '9'):
interface.interrupt_logic.b_interrupt_button_nine = True
elif pybullet_util.is_key_triggered(keys, '0'):
interface.interrupt_logic.b_interrupt_button_zero = True
if count % (SimConfig.CAMERA_DT / SimConfig.CONTROLLER_DT) == 0:
pass ## TODO(SH)
sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id,
pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
rf_height = pybullet_util.get_link_iso(robot,
link_id['r_foot_contact'])[2, 3]
lf_height = pybullet_util.get_link_iso(robot,
link_id['l_foot_contact'])[2, 3]
sensor_data['b_rf_contact'] = True if rf_height <= 0.01 else False
sensor_data['b_lf_contact'] = True if lf_height <= 0.01 else False
# Get Keyboard Event
keys = p.getKeyboardEvents()
if pybullet_util.is_key_triggered(keys, '8'):
pass
elif pybullet_util.is_key_triggered(keys, '5'):
pass
elif pybullet_util.is_key_triggered(keys, '4'):
pass
elif pybullet_util.is_key_triggered(keys, '2'):
pass
elif pybullet_util.is_key_triggered(keys, '6'):
pass
elif pybullet_util.is_key_triggered(keys, '7'):
pass
elif pybullet_util.is_key_triggered(keys, '9'):
pass
# Compute Command
gripper_command = dict()
for gripper_joint in gripper_joints:
gripper_command[gripper_joint] = nominal_sensor_data['joint_pos'][
gripper_joint]
for gripper_fjoint in gripper_floating.keys():
gripper_command[gripper_fjoint] = nominal_sensor_data['joint_pos'][
gripper_fjoint]
#GRIPPER_DELTA_ANGLE = 1.94 / 3
GRIPPER_DELTA_ANGLE = 1.94 / 4
while (1):
keys = p.getKeyboardEvents()
if pybullet_util.is_key_triggered(keys, 'c'):
for k in gripper_joints:
gripper_command[k] += GRIPPER_DELTA_ANGLE
elif pybullet_util.is_key_triggered(keys, 'o'):
for k in gripper_joints:
gripper_command[k] -= GRIPPER_DELTA_ANGLE
elif pybullet_util.is_key_triggered(keys, 'u'):
# gripper_pos[2] += 0.02 # z
gripper_command['basePosZ'] += 0.02
elif pybullet_util.is_key_triggered(keys, 'd'):
# gripper_pos[2] -= 0.02 # z
gripper_command['basePosZ'] -= 0.02
elif pybullet_util.is_key_triggered(keys, 'f'):
# gripper_pos[0] += 0.02 # x
gripper_command['basePosX'] += 0.02
elif pybullet_util.is_key_triggered(keys, 'b'):
