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]
