def is_safe(robot, link_id, sensor_data):
safe_list = [True, True, True, True, True, True]
rfoot_pos = pybullet_util.get_link_iso(robot,
link_id['r_foot_contact'])[0:3, 3]
lfoot_pos = pybullet_util.get_link_iso(robot,
link_id['l_foot_contact'])[0:3, 3]
if sensor_data['base_com_pos'][1] <= rfoot_pos[1]:
logging.info("reasoning: com falling to the right")
safe_list[0] = False
if sensor_data['base_com_pos'][1] >= lfoot_pos[1]:
safe_list[1] = False
logging.info("reasoning: com falling to the left")
if sensor_data['base_com_pos'][0] >= 0.2:
logging.info("reasoning: com falling forward")
safe_list[2] = False
if sensor_data['base_com_pos'][0] <= -0.2:
logging.info("reasoning: com falling back")
safe_list[3] = False
if sensor_data['base_com_pos'][2] <= NOMINAL_BASE_COM_HEIGHT - 0.1:
logging.info("reasoning: com too low")
safe_list[4] = False
if sensor_data['base_com_pos'][2] >= NOMINAL_BASE_COM_HEIGHT + 0.1:
logging.info("reasoning: com too high")
safe_list[5] = False
return safe_list
def is_tracking_error_safe(goal_pos, angle, robot, link_id):
safe = True
lh_pos = pybullet_util.get_link_iso(robot, link_id['l_hand_contact'])[0:3,
3]
lh_ori = pybullet_util.get_link_iso(robot, link_id['l_hand_contact'])[0:3,
0:3]
for i in range(3):
if np.abs(lh_pos[i] - goal_pos[i]) >= TRACKING_ERROR_THRESHOLD[i]:
logging.info("reasoning: bad tracking, {}th error is {}".format(
i, lh_pos[i] - goal_pos[i]))
safe = False
# lh_angle =
return safe
count = 0
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
while (1):
# Get SensorData
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
# Run Sim
t = 0
dt = SimConfig.CONTROLLER_DT
count = 0
while (1):
# Get SensorData
if SimConfig.SIMULATE_CAMERA and count % (
SimConfig.CAMERA_DT / SimConfig.CONTROLLER_DT) == 0:
camera_img = pybullet_util.get_camera_image_from_link(
robot, link_id['head'], 50, 10, 60., 0.1, 10)
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_sole'])[2, 3]
lf_height = pybullet_util.get_link_iso(robot, link_id['l_sole'])[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'):
def run_sim(inp):
goal_pos = np.copy(inp)
angle = 0.
if not p.isConnected():
if not B_VISUALIZE:
p.connect(p.DIRECT)
else:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=2.0,
cameraYaw=180 + 45,
cameraPitch=-15,
cameraTargetPosition=[0.5, 0.5, 0.6])
p.resetSimulation()
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(fixedTimeStep=SimConfig.CONTROLLER_DT,
numSubSteps=SimConfig.N_SUBSTEP)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(
cwd + "/robot_model/draco3/draco3_gripper_mesh_updated.urdf",
SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT)
p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config(
robot, SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT, False)
if B_VISUALIZE:
lh_target_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf",
[0., 0, 0.], [0, 0, 0, 1])
lh_target_pos = np.array([0., 0., 0.])
lh_target_quat = np.array([0., 0., 0., 1.])
lh_waypoint_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf",
[0., 0, 0.], [0, 0, 0, 1])
lh_waypoint_pos = np.array([0., 0., 0.])
lh_waypoint_quat = np.array([0., 0., 0., 1.])
# Add Gear constraint
c = p.createConstraint(robot,
link_id['l_knee_fe_lp'],
robot,
link_id['l_knee_fe_ld'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10)
c = p.createConstraint(robot,
link_id['r_knee_fe_lp'],
robot,
link_id['r_knee_fe_ld'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10)
# Initial Config
set_initial_config(robot, joint_id)
# Link Damping
pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.)
# Joint Friction
pybullet_util.set_joint_friction(robot, joint_id, 0.)
gripper_attached_joint_id = OrderedDict()
gripper_attached_joint_id["l_wrist_pitch"] = joint_id["l_wrist_pitch"]
gripper_attached_joint_id["r_wrist_pitch"] = joint_id["r_wrist_pitch"]
pybullet_util.set_joint_friction(robot, gripper_attached_joint_id, 0.1)
# Construct Interface
interface = DracoManipulationInterface()
# Run Sim
t = 0
dt = SimConfig.CONTROLLER_DT
count = 0
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
if B_VISUALIZE:
pybullet_util.draw_link_frame(robot,
link_id['l_hand_contact'],
text="lh")
pybullet_util.draw_link_frame(lh_target_frame, -1, text="lh_target")
pybullet_util.draw_link_frame(lh_waypoint_frame, -1)
gripper_command = dict()
for gripper_joint in GRIPPER_JOINTS:
gripper_command[gripper_joint] = nominal_sensor_data['joint_pos'][
gripper_joint]
waiting_command = True
time_command_recved = 0.
while (1):
# Get SensorData
sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id,
pos_basejoint_to_basecom,
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
if t >= WalkingConfig.INIT_STAND_DUR + 0.1 and waiting_command:
global_goal_pos = np.copy(goal_pos)
global_goal_pos[0] += sensor_data['base_com_pos'][0]
global_goal_pos[1] += sensor_data['base_com_pos'][1]
lh_target_pos = np.copy(global_goal_pos)
lh_target_rot = np.dot(RIGHTUP_GRIPPER, x_rot(angle))
lh_target_quat = util.rot_to_quat(lh_target_rot)
lh_target_iso = liegroup.RpToTrans(lh_target_rot, lh_target_pos)
lh_waypoint_pos = generate_keypoint(lh_target_iso)[0:3, 3]
interface.interrupt_logic.lh_target_pos = lh_target_pos
interface.interrupt_logic.lh_waypoint_pos = lh_waypoint_pos
interface.interrupt_logic.lh_target_quat = lh_target_quat
interface.interrupt_logic.b_interrupt_button_one = True
waiting_command = False
time_command_recved = t
command = interface.get_command(copy.deepcopy(sensor_data))
if B_VISUALIZE:
p.resetBasePositionAndOrientation(lh_target_frame, lh_target_pos,
lh_target_quat)
p.resetBasePositionAndOrientation(lh_waypoint_frame,
lh_waypoint_pos, lh_target_quat)
# Exclude Knee Proximal Joints Command
del command['joint_pos']['l_knee_fe_jp']
del command['joint_pos']['r_knee_fe_jp']
del command['joint_vel']['l_knee_fe_jp']
del command['joint_vel']['r_knee_fe_jp']
del command['joint_trq']['l_knee_fe_jp']
del command['joint_trq']['r_knee_fe_jp']
# Apply Command
pybullet_util.set_motor_trq(robot, joint_id, command['joint_trq'])
pybullet_util.set_motor_pos(robot, joint_id, gripper_command)
p.stepSimulation()
t += dt
count += 1
## Safety On the run
safe_list = is_safe(robot, link_id, sensor_data)
if all(safe_list):
pass
else:
safe_list.append(False)
del interface
break
## Safety at the end
if t >= time_command_recved + ManipulationConfig.T_REACHING_DURATION + 0.2:
if is_tracking_error_safe(global_goal_pos, angle, robot, link_id):
safe_list.append(True)
else:
safe_list.append(False)
del interface
break
return safe_list
True)
else:
raise ValueError
# DataSaver
data_saver = DataSaver("atlas_crbi_validation.pkl")
# Run Sim
t = 0
dt = DT
count = 0
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
nominal_lf_iso = pybullet_util.get_link_iso(robot, link_id['l_sole'])
nominal_rf_iso = pybullet_util.get_link_iso(robot, link_id['r_sole'])
base_pos = np.copy(nominal_sensor_data['base_com_pos'])
base_quat = np.copy(nominal_sensor_data['base_com_quat'])
joint_pos = copy.deepcopy(nominal_sensor_data['joint_pos'])
s = 0.
b_ik = False
b_regressor_trained = False
while (1):
# Get Keyboard Event
keys = p.getKeyboardEvents()
# Set base_pos, base_quat, joint_pos here for visualization
if pybullet_util.is_key_triggered(keys, '8'):
# f, jac_f = generate_casadi_func(crbi_model, input_mean, input_std,
cwd + "/robot_model/valkyrie", False, True)
else:
raise ValueError
# DataSaver
data_saver = DataSaver("valkyrie_crbi_validation.pkl")
# Run Sim
t = 0
dt = DT
count = 0
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
nominal_lf_iso = pybullet_util.get_link_iso(robot,
link_id['leftCOP_Frame'])
nominal_rf_iso = pybullet_util.get_link_iso(robot,
link_id['rightCOP_Frame'])
base_pos = np.copy(nominal_sensor_data['base_com_pos'])
base_quat = np.copy(nominal_sensor_data['base_com_quat'])
joint_pos = copy.deepcopy(nominal_sensor_data['joint_pos'])
__import__('ipdb').set_trace()
s = 0.
b_ik = False
b_regressor_trained = False
while (1):
# Get Keyboard Event
keys = p.getKeyboardEvents()
# Set base_pos, base_quat, joint_pos here for visualization
joint_pos)
if b_crbi and VIDEO_RECORD:
robot_sys.update_system(
sensor_data["base_com_pos"], sensor_data["base_com_quat"],
sensor_data["base_com_lin_vel"],
sensor_data["base_com_ang_vel"], sensor_data["base_joint_pos"],
sensor_data["base_joint_quat"],
sensor_data["base_joint_lin_vel"],
sensor_data["base_joint_ang_vel"], sensor_data["joint_pos"],
sensor_data["joint_vel"], True)
rot_world_base = util.quat_to_rot(sensor_data['base_com_quat'])
world_I = robot_sys.Ig[0:3, 0:3]
local_I = np.dot(np.dot(rot_world_base.transpose(), world_I),
rot_world_base)
rf_iso = pybullet_util.get_link_iso(robot, link_id["r_sole"])
lf_iso = pybullet_util.get_link_iso(robot, link_id["l_sole"])
denormalized_output, output = evaluate_crbi_model_using_tf(
crbi_model, sensor_data["base_com_pos"], lf_iso[0:3, 3],
rf_iso[0:3, 3], input_mean, input_std, output_mean, output_std)
local_I_est = inertia_from_one_hot_vec(denormalized_output)
data_saver.add('gt_inertia', inertia_to_one_hot_vec(local_I))
data_saver.add(
'gt_inertia_normalized',
util.normalize(inertia_to_one_hot_vec(local_I), output_mean,
output_std))
data_saver.add('est_inertia_normalized',
np.copy(np.squeeze(output)))
data_saver.add('est_inertia', np.copy(denormalized_output))
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
command = interface.get_command(copy.deepcopy(sensor_data))
# Set motor cmd
# pybullet_util.set_motor_pos(quadruped, joint_id,
# nominal_sensor_data['joint_pos'])
pybullet_util.set_motor_trq(quadruped, joint_id, command['joint_trq'])
# pybullet_util.set_motor_pos(quadruped, joint_id, command['joint_pos'])
p.stepSimulation()
t += SimConfig.CONTROLLER_DT
count += 1
time.sleep(SimConfig.CONTROLLER_DT)
print("toeFL")
print(pybullet_util.get_link_iso(quadruped, link_id['toeFL']))
print("toeRR")
print(pybullet_util.get_link_iso(quadruped, link_id['toeRR']))
print("toeRL")
print(pybullet_util.get_link_iso(quadruped, link_id['toeRL']))
print("base pos")
print(sensor_data["base_com_pos"])
# print("base quat")
# print(sensor_data["base_com_quat"])
