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"])