p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=2.0, cameraYaw=90, cameraPitch=0, cameraTargetPosition=[0, 0, 1.]) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF(cwd + "/robot_model/atlas/atlas.urdf", INITIAL_POS_WORLD_TO_BASEJOINT, INITIAL_QUAT_WORLD_TO_BASEJOINT, useFixedBase=1) 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, INITIAL_POS_WORLD_TO_BASEJOINT, INITIAL_QUAT_WORLD_TO_BASEJOINT, PRINT_ROBOT_INFO) p.setPhysicsEngineParameter(numSolverIterations=10) p.changeDynamics(robot, -1, linearDamping=0, angularDamping=0) set_initial_config(robot, joint_id) pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.) pybullet_util.set_joint_friction(robot, joint_id, 0) nominal_sensor_data = pybullet_util.get_sensor_data( robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) print("initial joint_pos: ", nominal_sensor_data['joint_pos'])
numSubSteps=ManipulatorConfig.N_SUBSTEP) if ManipulatorConfig.VIDEO_RECORD: if not os.path.exists('video'): os.makedirs('video') for f in os.listdir('video'): os.remove('video/' + f) p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "video/atlas.mp4") # Create Robot, Ground p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF(cwd + "/robot_model/manipulator/three_link_manipulator.urdf", useFixedBase=True) 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) # Set Initial Config p.resetJointState(robot, 0, -np.pi / 6., 0.) p.resetJointState(robot, 1, np.pi / 6., 0.) p.resetJointState(robot, 2, np.pi / 3., 0.) # Link Damping pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.) # Joint Friction pybullet_util.set_joint_friction(robot, joint_id, 0.1) # Construct Interface interface = ManipulatorInterface()
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
import pybullet as p import time import pybullet_data print(pybullet_data.getDataPath()) p.connect(p.GUI) p.resetDebugVisualizerCamera(cameraDistance=0.2, cameraYaw=0, cameraPitch=-45, cameraTargetPosition=[0., 0., 0.]) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.loadURDF("plane.urdf", 0, 0, -2) wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0]) nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( wheelA, [0., 0., 0.], [0., 0., 0., 1.], True) for i in range(p.getNumJoints(wheelA)): print(p.getJointInfo(wheelA, i)) p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0) c = p.createConstraint(wheelA, 1, wheelA, 3, jointType=p.JOINT_GEAR, jointAxis=[0, 1, 0],
# if SimConfig.VIDEO_RECORD: # video_dir = 'video/atlas_pnc' # if os.path.exists(video_dir): # shutil.rmtree(video_dir) # os.makedirs(video_dir) # Create Robot, Ground p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) robot = p.loadURDF(cwd + "/robot_model/atlas/atlas.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, True) # Create Robot for Meshcat Visualization model, collision_model, visual_model = pin.buildModelsFromUrdf( cwd + "/robot_model/atlas/atlas.urdf", cwd + "/robot_model/atlas", pin.JointModelFreeFlyer()) viz = MeshcatVisualizer(model, collision_model, visual_model) try: viz.initViewer(open=True) except ImportError as err: print( "Error while initializing the viewer. It seems you should install Python meshcat" ) print(err) sys.exit(0)
p.connect(p.GUI) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.resetSimulation() p.setGravity(0, 0, -10) useRealTimeSim = 1 #for video recording (works best on Mac and Linux, not well on Windows) #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") p.setRealTimeSimulation(useRealTimeSim) # either this p.loadURDF("plane.urdf") #p.loadSDF("stadium.sdf") car = p.loadURDF( "racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) pybullet_util.get_robot_config(car, [0., 0., 0.], [0., 0., 0., 1.], True) # for i in range(p.getNumJoints(car)): # print(p.getJointInfo(car, i)) for wheel in range(p.getNumJoints(car)): p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.getJointInfo(car, wheel) wheels = [8, 15] print("----------------") #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
# gripper = p.loadURDF(cwd + "/robot_model/ezgripper/gripper.urdf", # useFixedBase=1, # basePosition=[0 + xOffset - 0.2, 0, 0.7]) gripper = p.loadURDF(cwd + "/robot_model/ezgripper/gripper.urdf", useFixedBase=1, basePosition=[0 + xOffset - 0.2, -0.7, 1.35]) gripper_joints = [ "left_ezgripper_knuckle_palm_L1_1", "left_ezgripper_knuckle_L1_L2_1", "left_ezgripper_knuckle_palm_L1_2", "left_ezgripper_knuckle_L1_L2_2" ] gripper_floating = {"basePosX": 0, "basePosY": 1, "basePosZ": 2} nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( gripper, [0, 0, 1], [0, 0, 0, 1], True) pybullet_util.set_joint_friction(gripper, gripper_floating, 0.) pybullet_util.set_link_damping(gripper, link_id.values(), 0., 0.) nominal_sensor_data = pybullet_util.get_sensor_data(gripper, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) gripper_pos = nominal_sensor_data['base_com_pos'] gripper_quat = nominal_sensor_data['base_com_quat'] gripper_command = dict() for gripper_joint in gripper_joints: gripper_command[gripper_joint] = nominal_sensor_data['joint_pos'][ gripper_joint]