def main():
args = parse_args()
pybullet.connect(pybullet.SHARED_MEMORY)
pybullet.resetSimulation()
set_minimal_environment()
if args.use_kuka:
robot_path = args.robot_path or DEFAULT_KUKA_ROBOT_PATH
robot = Kuka(robot_path)
else:
robot_path = args.robot_path or DEFAULT_BLUE_ROBOT_PATH
robot = BlueArm(robot_path)
robot.startup(is_real_time=False)
robot.debug_arm_idx()
robot.get_mass()
while 1:
for _ in tqdm(range(1000), desc='Running simulation'):
events = pybullet.getVREvents()
for event in events:
_, position, orientation = event[:3]
buttons = event[6]
trigger_button = buttons[33]
robot.move(position, orientation)
if args.debug_position:
debug_position(position, robot.get_pose()[0])
if not args.use_kuka:
if trigger_button:
robot.close_clamp()
else:
robot.open_clamp()
pybullet.stepSimulation()
def main():
args = parse_args()
pybullet.connect(pybullet.SHARED_MEMORY)
pybullet.resetSimulation()
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("plane.urdf")
flags = pybullet.URDF_USE_SELF_COLLISION | pybullet.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
robot = pybullet.loadURDF(args.robot_path, [0, 0, 0],
useFixedBase=1,
flags=flags)
pybullet.setGravity(0, 0, -9.81)
pybullet.setRealTimeSimulation(1) #this makes the simulation real time
n_joints = pybullet.getNumJoints(robot)
reference_link = n_joints - 2
position, orientation = get_link_state(robot, reference_link)
print('The number of joints in the robot is: %i' % n_joints)
while 1:
events = pybullet.getVREvents()
for event in events:
_, position, orientation = event[:3]
target_positions = pybullet.calculateInverseKinematics(
robot, reference_link, position, orientation)
pybullet.setJointMotorControlArray(
robot,
range(len(target_positions)),
pybullet.POSITION_CONTROL,
targetPositions=target_positions)
current_position, _ = get_link_state(robot, reference_link)
debug_position(position, current_position, 0.2)
def get_new_command():
try:
if USE_VR:
global POS
global ORI
global GRIPPER
events = p.getVREvents()
e = events[0]
POS = list(e[POSITION])
ORI = list(e[ORIENTATION])
GRIPPER = e[ANALOG]
print(p.getEulerFromQuaternion(ORI))
else:
POS = [p.readUserDebugParameter(i) for i in range(0, 3)]
except:
pass
def get_vr_output():
nonlocal prev_vr_theta
ee_pos, ee_theta = bullet.get_link_state(env.robot_id,
env.end_effector_index)
events = p.getVREvents()
# detect input from controllers
assert events, "no input from controller!"
e = events[0]
# obtain gripper state from controller trigger
trigger = get_gripper_input(e)
# pass controller position and orientation into the environment
cont_pos = e[POSITION]
cont_orient = bullet.deg_to_quat([180, 0, 0])
if ORIENTATION_ENABLED:
cont_orient = e[ORIENTATION]
cont_orient = bullet.quat_to_deg(list(cont_orient))
action = [
cont_pos[0] - ee_pos[0], cont_pos[1] - ee_pos[1],
cont_pos[2] - ee_pos[2]
]
action = np.array(action) * 3.5 # to make grasp success < 20 timesteps
grip = trigger
for _ in range(2):
action = np.append(action, 0)
wrist_theta = cont_orient[2] - prev_vr_theta
action = np.append(action, wrist_theta)
action = np.append(action, grip)
action = np.append(action, 0)
# ===========================================================
# Add noise during actual data collection
noise = 0.1
noise_scalings = [noise] * 3 + [0.1 * noise] * 3 + [noise] * 2
action += np.random.normal(scale=noise_scalings)
# ===========================================================
action = np.clip(action, -1 + EPSILON, 1 - EPSILON)
prev_vr_theta = cont_orient[2]
return action
def get_new_command():
try:
global POS
global ORI
global GRIPPER
global BUTTON
events = p.getVREvents()
e = events[0]
POS = list(e[POSITION])
ORI = list(e[ORIENTATION])
if e[ANALOG] > GRIPPER:
GRIPPER = min(GRIPPER + 0.25, e[ANALOG])
else: # i.e, force it to slowly open/close
GRIPPER = max(GRIPPER - 0.25, e[ANALOG])
#GRIPPER = e[ANALOG]
#print(GRIPPER)
BUTTON = e[BUTTONS][2]
return 1
#print(p.getEulerFromQuaternion(ORI))
except:
return 0
def get_poses_and_clamps(self):
events = pybullet.getVREvents()
for event in events:
controller_id, position, orientation = event[:3]
buttons = event[6]
trigger_button = buttons[33]
stop_tracking_button = buttons[1]
trackpad_button = buttons[32]
if trackpad_button and not self.start_controlling:
print('Now the robot is being controlled')
self.start_controlling = True
if not self.start_controlling:
continue
if controller_id % 2 == 0 and not stop_tracking_button:
self.left_pose = position, orientation
self.left_clamp = trigger_button
elif controller_id % 2 == 1 and not stop_tracking_button:
self.right_pose = position, orientation
self.right_clamp = trigger_button
return self.left_pose, self.right_pose, self.left_clamp, self.right_clamp
widths = [3]*p.VR_MAX_CONTROLLERS
#use a few default colors
colors[0] = [0,0,0]
colors[1] = [0.5,0,0]
colors[2] = [0,0.5,0]
colors[3] = [0,0,0.5]
colors[4] = [0.5,0.5,0.]
colors[5] = [.5,.5,.5]
controllerId = -1
pt=[0,0,0]
print("waiting for VR controller trigger")
while (controllerId<0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
print("Using controllerId="+str(controllerId))
while True:
events = p.getVREvents(allAnalogAxes=1)
for e in (events):
if (e[CONTROLLER_ID]==controllerId ):
for a in range(10):
print("analog axis"+str(a)+"="+str(e[8][a]))
ser = getSerialOrNone(portname)
if (ser is None):
portname = "/dev/cu.usbmodem14"+str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is not None):
print("COnnected!")
portindex = portindex+1
p.saveWorld("setupTrackerWorld.py")
if (ser.isOpen()):
while True:
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
p.changeConstraint(hand_cid,e[POSITION],e[ORIENTATION], maxForce=50)
serialSteps = 0
while ser.inWaiting() > 0:
serialSteps=serialSteps+1
if (serialSteps>serialStepsUntilCheckVREvents):
ser.flushInput()
break
line = str(ser.readline())
words = line.split(",")
if (len(words)==6):
pink = convertSensor(words[1])
middle = convertSensor(words[2])
index = convertSensor(words[3])
widths = [3] * p.VR_MAX_CONTROLLERS
#use a few default colors
colors[0] = [0, 0, 0]
colors[1] = [0.5, 0, 0]
colors[2] = [0, 0.5, 0]
colors[3] = [0, 0, 0.5]
colors[4] = [0.5, 0.5, 0.]
colors[5] = [.5, .5, .5]
controllerId = -1
pt = [0, 0, 0]
print("waiting for VR controller trigger")
while (controllerId < 0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] == p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32] == p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
print("Using controllerId=" + str(controllerId))
while True:
events = p.getVREvents(allAnalogAxes=1)
for e in (events):
if (e[CONTROLLER_ID] == controllerId):
for a in range(10):
print("analog axis" + str(a) + "=" + str(e[8][a]))
p.connect(p.GUI)
#load the MuJoCo MJCF hand
minitaur = p.loadURDF("quadruped/minitaur.urdf")
robot_cid = -1
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
p.setRealTimeSimulation(1)
controllerId = -1
while True:
events = p.getVREvents()
for e in (events):
#print (e[BUTTONS])
if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED):
if (controllerId >= 0):
controllerId = -1
else:
controllerId = e[0]
if (e[0] == controllerId):
if (robot_cid >= 0):
p.changeConstraint(robot_cid, e[POSITION], e[ORIENTATION], maxForce=5000)
if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED):
if (robot_cid >= 0):
p.removeConstraint(robot_cid)
#q = [0,0,0,1]
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
CONTROLLER_ID = 0
POSITION = 1
ORIENTATION = 2
ANALOG = 3
BUTTONS = 6
uiControllerId = -1
print("waiting for VR UI controller trigger")
while (uiControllerId < 0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] == p.VR_BUTTON_IS_DOWN):
uiControllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32] == p.VR_BUTTON_IS_DOWN):
uiControllerId = e[CONTROLLER_ID]
print("Using uiControllerId=" + str(uiControllerId))
controllerId = -1
print("waiting for VR picking controller trigger")
while (controllerId < 0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] == p.VR_BUTTON_IS_DOWN):
widths = [3] * p.VR_MAX_CONTROLLERS
a = [0, 0, 0]
#use a few default colors
colors[0] = [0, 0, 0]
colors[1] = [0.5, 0, 0]
colors[2] = [0, 0.5, 0]
colors[3] = [0, 0, 0.5]
colors[4] = [0.5, 0.5, 0.]
colors[5] = [.5, .5, .5]
p.startStateLogging(p.STATE_LOGGING_VR_CONTROLLERS, "vr_hmd.bin", deviceTypeFilter=p.VR_DEVICE_HMD)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "generic_data.bin")
p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "contact_points.bin")
while True:
events = p.getVREvents(p.VR_DEVICE_HMD + p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
pos = e[POSITION]
mat = p.getMatrixFromQuaternion(e[ORIENTATION])
dir0 = [mat[0], mat[3], mat[6]]
dir1 = [mat[1], mat[4], mat[7]]
dir2 = [mat[2], mat[5], mat[8]]
lineLen = 0.1
dir = [-mat[2], -mat[5], -mat[8]]
to = [pos[0] + lineLen * dir[0], pos[1] + lineLen * dir[1], pos[2] + lineLen * dir[2]]
toX = [pos[0] + lineLen * dir0[0], pos[1] + lineLen * dir0[1], pos[2] + lineLen * dir0[2]]
toY = [pos[0] + lineLen * dir1[0], pos[1] + lineLen * dir1[1], pos[2] + lineLen * dir1[2]]
toZ = [pos[0] + lineLen * dir2[0], pos[1] + lineLen * dir2[1], pos[2] + lineLen * dir2[2]]
p.addUserDebugLine(pos, toX, [1, 0, 0], 1)
p.addUserDebugLine(pos, toY, [0, 1, 0], 1)
REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0]
JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1]
REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001]
MAX_FORCE = 500
def euc_dist(posA, posB):
dist = 0.
for i in range(len(posA)):
dist += (posA[i] - posB[i])**2
return dist
p.setRealTimeSimulation(1)
controllers = [e[0] for e in p.getVREvents()]
while True:
events = p.getVREvents()
for e in (events):
# Only use one controller
if e[0] == min(controllers):
break
sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])
# A simplistic version of gripper control
if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED:
# avg = 0.
KUKA_GRIPPER_CLOZ_POS = [
0.0, -0.047564246423083795, 0.6855956234759611, -0.7479294372303137, 0.05054599996976922, 0.0,
0.049838105678835724, 0.0
]
def euc_dist(posA, posB):
dist = 0.
for i in range(len(posA)):
dist += (posA[i] - posB[i])**2
return dist
p.setRealTimeSimulation(1)
controllers = [e[0] for e in p.getVREvents()]
for j in range(p.getNumJoints(kuka_gripper)):
print(p.getJointInfo(kuka_gripper, j))
while True:
events = p.getVREvents()
for e in (events):
# Only use one controller
###########################################
# This is important: make sure there's only one VR Controller!
if e[0] == controllers[0]:
break
sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])
def basic_vr_env(s):
last_button_2_push = time.time()
gravity = -10
cid = p.connect(p.SHARED_MEMORY)
print(cid)
print("Connected to shared memory")
if (cid < 0):
p.connect(p.GUI)
p.resetSimulation()
#p.setGravity(0,0,-0.01)
p.setVRCameraState([0, 0, -1.0], p.getQuaternionFromEuler([0, 0, 0]))
p.resetDebugVisualizerCamera(0.75, 45, -45, [0, 0, 0])
past_ori = np.array([None])
print('VR Setup Done')
s.bind((HOST, PORT))
s.listen()
print('Ready to accept connection')
conn, addr = s.accept()
xyz_offset = np.array([0, 0, 0])
with conn:
print('Connected by', addr)
while (1):
events = p.getVREvents()
try:
e = events[0]
ori = np.array(list(e[ORIENTATION]))
# check if flipped!
if past_ori.all() != None:
if (np.sign(ori) == -np.sign(past_ori)).all():
ori = -ori
#print('triggered!', ori)
past_ori = ori
if e[BUTTON_2[0]][BUTTON_2[1]] >= 1 and (
time.time() >= last_button_2_push + 1):
print("offsetting")
last_button_2_push = time.time()
xyz_offset = np.array(e[POSITION])
pos = np.array(list(e[POSITION])) - xyz_offset
action = np.array(list(pos) + list(ori) + [e[ANALOG]])
print(action)
#action = np.array(list(e[POSITION]) + list(e[ORIENTATION]) + [e[ANALOG]])
#print(action)
#print(time.time())
#if the button is pressed and its been a second since the last recording
# pass
# another button to work with if we want
data = conn.recv(1024)
if not data:
print('breaking')
break
action = pickle.dumps(action)
conn.sendall(action)
except Exception as e:
pass
print(e)
# print(traceback.format_exc())
p.disconnect()
print(portname)
ser = getSerialOrNone(portname)
if (ser is None):
portname = "/dev/cu.usbmodem14" + str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is not None):
print("COnnected!")
portindex = portindex + 1
p.saveWorld("setupTrackerWorld.py")
if (ser.isOpen()):
while True:
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
p.changeConstraint(hand_cid,
e[POSITION],
e[ORIENTATION],
maxForce=50)
serialSteps = 0
while ser.inWaiting() > 0:
serialSteps = serialSteps + 1
if (serialSteps > serialStepsUntilCheckVREvents):
ser.flushInput()
break
line = str(ser.readline())
words = line.split(",")
if (len(words) == 6):
def reset(self):
p.setRealTimeSimulation(enableRealTimeSimulation=0,
physicsClientId=self.id)
if self.vr and self.participant >= 0:
self.directory = os.path.join(
os.getcwd(), 'participant_%d' % self.participant,
'bed_bathing_vr_data_' + self.robot_type + '_' +
self.policy_name + ('_participant_%d' % self.participant) +
datetime.now().strftime('_%Y-%m-%d_%H-%M-%S'))
if not os.path.exists(self.directory):
os.makedirs(self.directory)
self.save_filename = os.path.join(self.directory,
'frame_%d.bullet')
self.action_list = []
if self.replay:
with open(os.path.join(self.replay_dir, 'setup.pkl'), 'rb') as f:
self.robot_type, self.gender, self.hipbone_to_mouth_height = pickle.load(
f)
with open(os.path.join(self.replay_dir, 'actions.pkl'), 'rb') as f:
self.action_list = pickle.load(f)
self.setup_timing()
self.task_success = 0
self.contact_points_on_arm = {}
if self.vr or self.replay:
if self.vr:
bed_to_hmd_height = self.hipbone_to_mouth_height + (
0.068 + 0.117 if self.gender == 'male' else 0.058 + 0.094)
p.setOriginCameraPositionAndOrientation(
deviceTypeFilter=p.VR_DEVICE_HMD,
pos_offset=[
0, 0.46 - bed_to_hmd_height * np.cos(np.pi / 3.),
-(0.7 - (0.117 if self.gender == 'male' else 0.094) +
bed_to_hmd_height * np.sin(np.pi / 3.))
],
orn_offset=[0, 0, 0, 1])
self.human, self.left_arm, self.right_arm, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='bed',
static_human_base=True,
human_impairment='random',
print_joints=False,
gender=self.gender,
hipbone_to_mouth_height=self.hipbone_to_mouth_height)
else:
if self.participant < 0:
self.gender = self.np_random.choice(['male', 'female'])
if self.new:
self.hipbone_to_mouth_height = self.np_random.uniform(
0.6 - 0.1, 0.6 +
0.1) if self.gender == 'male' else self.np_random.uniform(
0.54 - 0.1, 0.54 + 0.1)
self.human, self.left_arm, self.right_arm, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='bed',
static_human_base=True,
human_impairment='none',
print_joints=False,
gender=self.gender,
hipbone_to_mouth_height=self.hipbone_to_mouth_height)
else:
self.hipbone_to_mouth_height = 0.6 if self.gender == 'male' else 0.54
self.human, self.left_arm, self.right_arm, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='bed',
static_human_base=True,
human_impairment='none',
print_joints=False,
gender=self.gender,
hipbone_to_mouth_height=self.hipbone_to_mouth_height)
self.robot_lower_limits = self.robot_lower_limits[
self.robot_left_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_left_arm_joint_indices]
self.reset_robot_joints()
p.resetBasePositionAndOrientation(self.human, [0, 0, 0.7],
p.getQuaternionFromEuler(
[np.deg2rad(-30), 0, 0],
physicsClientId=self.id),
physicsClientId=self.id)
if self.vr or self.replay:
left_shoulder_pos, left_shoulder_orient = p.getLinkState(
self.human,
16,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
p.resetBasePositionAndOrientation(self.left_arm,
left_shoulder_pos,
left_shoulder_orient,
physicsClientId=self.id)
right_shoulder_pos, right_shoulder_orient = p.getLinkState(
self.human,
6,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
p.resetBasePositionAndOrientation(self.right_arm,
right_shoulder_pos,
right_shoulder_orient,
physicsClientId=self.id)
# Create the mattress
p.removeBody(self.bed, physicsClientId=self.id)
y_offset = -0.53
self.bed_parts = []
mattress_collision = p.createCollisionShape(
shapeType=p.GEOM_BOX,
halfExtents=[0.88 / 2.0, 1.25 / 2.0, 0.15 / 2.0],
collisionFramePosition=[0, 0, 0.15 / 2.0],
physicsClientId=self.id)
mattress_visual = p.createVisualShape(
shapeType=p.GEOM_BOX,
halfExtents=[0.88 / 2.0, 1.25 / 2.0, 0.15 / 2.0],
visualFramePosition=[0, 0, 0.15 / 2.0],
rgbaColor=[1, 1, 1, 1],
physicsClientId=self.id)
self.bed_parts.append(
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=mattress_collision,
baseVisualShapeIndex=mattress_visual,
basePosition=[0, y_offset, 0.4],
useMaximalCoordinates=False,
physicsClientId=self.id))
mattress_collision = p.createCollisionShape(
shapeType=p.GEOM_BOX,
halfExtents=[0.88 / 2.0, 0.7 / 2.0, 0.15 / 2.0],
collisionFramePosition=[0, 0.7 / 2.0, 0],
physicsClientId=self.id)
mattress_visual = p.createVisualShape(
shapeType=p.GEOM_BOX,
halfExtents=[0.88 / 2.0, 0.7 / 2.0, 0.15 / 2.0],
visualFramePosition=[0, 0.7 / 2.0, 0],
rgbaColor=[1, 1, 1, 1],
physicsClientId=self.id)
self.bed_parts.append(
p.createMultiBody(
baseMass=0.0,
baseCollisionShapeIndex=mattress_collision,
baseVisualShapeIndex=mattress_visual,
basePosition=[0, 1.25 / 2.0 + y_offset, 0.4 + 0.15 / 2.0],
baseOrientation=p.getQuaternionFromEuler(
[np.deg2rad(60), 0, 0], physicsClientId=self.id),
useMaximalCoordinates=False,
physicsClientId=self.id))
# Create the bed frame
visual_filename = os.path.join(self.world_creation.directory, 'bed',
'hospital_bed_frame_reduced.obj')
collision_filename = os.path.join(self.world_creation.directory, 'bed',
'hospital_bed_frame_vhacd.obj')
bed_collision = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=[1, 1.2, 1],
physicsClientId=self.id)
bed_visual = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=[1, 1.2, 1],
rgbaColor=[1, 1, 1, 1],
physicsClientId=self.id)
self.bed_parts.append(
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=bed_collision,
baseVisualShapeIndex=bed_visual,
basePosition=[0, y_offset + 0.45, 0.42],
baseOrientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, -np.pi / 2.0],
physicsClientId=self.id),
useMaximalCoordinates=False,
physicsClientId=self.id))
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# Disable collisions between the person's hips/legs and the mattress/frame
for bed_part in self.bed_parts:
for i in list(range(28, 42)) + [0, 1, 2, 3]:
p.setCollisionFilterPair(self.human,
bed_part,
i,
-1,
0,
physicsClientId=self.id)
# Continually resample random human pose until no contact with robot or environment is occurring
if self.vr or self.replay:
if self.new:
joints_positions = [
(7, np.deg2rad(20)), (8, np.deg2rad(-20)),
(10, np.deg2rad(-45)), (20, np.deg2rad(-45)),
(28, np.deg2rad(-60)), (35, np.deg2rad(-60))
]
else:
joint_angles = [
0.39717707, 0.27890519, -0.00883447, -0.67345593,
-0.00568484, 0.05987911, 0.00957937
]
joints_positions = [(i, joint_angles[i]) for i in range(7)]
joints_positions += [(13, np.deg2rad(-30)),
(28, np.deg2rad(-60)),
(35, np.deg2rad(-60))]
self.human_controllable_joint_indices = [
0, 1, 2, 25, 26, 27
] + list(range(7, 14)) + list(range(17, 24))
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices,
use_static_joints=True,
human_reactive_force=None)
human_joint_states = p.getJointStates(self.human,
jointIndices=list(
range(4, 14)),
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_HMD +
p.VR_DEVICE_CONTROLLER,
physicsClientId=self.id)
if len(events) == 3:
self.arm_sim(events[1], "right")
self.arm_sim(events[1], "left")
left_arm_joint_states = p.getJointStates(self.human,
jointIndices=list(
range(17, 24)),
physicsClientId=self.id)
left_arm_joint_positions = np.array(
[x[0] for x in left_arm_joint_states])
for i in range(7):
p.resetJointState(self.left_arm,
jointIndex=i,
targetValue=left_arm_joint_positions[i],
targetVelocity=0,
physicsClientId=self.id)
right_arm_joint_states = p.getJointStates(self.human,
jointIndices=list(
range(7, 14)),
physicsClientId=self.id)
right_arm_joint_positions = np.array(
[x[0] for x in right_arm_joint_states])
for i in range(7):
p.resetJointState(self.right_arm,
jointIndex=i,
targetValue=right_arm_joint_positions[i],
targetVelocity=0,
physicsClientId=self.id)
else:
if self.new:
self.human_controllable_joint_indices = list(range(4, 14))
human_positioned = False
right_arm_links = [9, 11, 13]
left_arm_links = [19, 21, 23]
r_arm_dists = []
right_arm_shoulder_links = [6, 9, 11, 13]
left_arm_shoulder_links = [16, 19, 21, 23]
r_arm_bed_dists = []
l_arm_bed_dists = []
while not human_positioned or (
np.min(r_arm_dists + r_arm_bed_dists +
l_arm_bed_dists + [1]) < 0.01):
human_positioned = True
joints_positions = [(7, np.deg2rad(20)),
(8, np.deg2rad(-20)),
(10, np.deg2rad(-45)),
(20, np.deg2rad(-45)),
(28, np.deg2rad(-60)),
(35, np.deg2rad(-60))]
joints_positions += [
(i, 0)
for i in list(range(7, 14)) + list(range(17, 24))
]
joints_positions += [
(i,
self.np_random.uniform(np.deg2rad(-10),
np.deg2rad(10)))
for i in [0, 1, 2]
]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices if
(self.human_control
or self.world_creation.human_impairment == 'tremor')
else [],
use_static_joints=True,
human_reactive_force=None,
non_static_joints=(list(range(4, 24))
if self.human_control else []))
joints_positions = [
(i,
self.np_random.uniform(np.deg2rad(-10),
np.deg2rad(10)))
for i in list(range(7, 14))
]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices if
(self.human_control
or self.world_creation.human_impairment == 'tremor')
else [],
use_static_joints=False,
human_reactive_force=None,
add_joint_positions=True)
r_arm_dists = [
c[8] for link in right_arm_links
for c in p.getClosestPoints(bodyA=self.human,
bodyB=self.human,
linkIndexA=link,
distance=0.05,
physicsClientId=self.id)
if c[4] not in (right_arm_links + [3, 6])
]
r_arm_bed_dists = [
c[8] for link in right_arm_shoulder_links
for bed_part in self.bed_parts
for c in p.getClosestPoints(bodyA=self.human,
bodyB=bed_part,
linkIndexA=link,
distance=0.05,
physicsClientId=self.id)
]
l_arm_bed_dists = [
c[8] for link in left_arm_shoulder_links
for bed_part in self.bed_parts
for c in p.getClosestPoints(bodyA=self.human,
bodyB=bed_part,
linkIndexA=link,
distance=0.05,
physicsClientId=self.id)
]
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
else:
for bed_parts in self.bed_parts:
p.changeDynamics(bed_parts,
-1,
lateralFriction=5,
spinningFriction=5,
rollingFriction=5,
physicsClientId=self.id)
joints_positions = [(7, np.deg2rad(50)), (8, np.deg2rad(-50)),
(17, np.deg2rad(-30)),
(28, np.deg2rad(-60)),
(35, np.deg2rad(-60))]
self.world_creation.setup_human_joints(
self.human,
joints_positions, [],
use_static_joints=True,
human_reactive_force=None,
non_static_joints=(list(range(4, 14))))
p.setGravity(0, 0, -1, physicsClientId=self.id)
# Let the arm settle on the bed
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
self.human_controllable_joint_indices = list(range(
4, 14)) if self.human_control else []
self.world_creation.setup_human_joints(
self.human, [],
self.human_controllable_joint_indices,
use_static_joints=True,
human_reactive_force=None,
human_reactive_gain=0.01)
self.target_human_joint_positions = []
if self.human_control:
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
p.changeDynamics(self.human,
-1,
mass=0,
physicsClientId=self.id)
p.resetBaseVelocity(self.human,
linearVelocity=[0, 0, 0],
angularVelocity=[0, 0, 0],
physicsClientId=self.id)
self.human_lower_limits = self.human_lower_limits[
self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[
self.human_controllable_joint_indices]
shoulder_pos, shoulder_orient = p.getLinkState(
self.human,
9,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human,
11,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human,
13,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
if self.vr or self.replay:
human_joint_indices = list(range(4, 14))
else:
human_joint_indices = self.human_controllable_joint_indices
target_pos = np.array([-0.5, -0.1, 1])
if self.robot_type == 'pr2':
target_orient = np.array(
p.getQuaternionFromEuler(np.array([0, 0, 0]),
physicsClientId=self.id))
_, _, self.target_robot_joint_positions = self.position_robot_toc(
self.robot,
76, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(29, 29 + 7),
pos_offset=np.array([0, 0, 0]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=human_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.target_robot_joint_positions = self.target_robot_joint_positions[
0]
self.world_creation.set_gripper_open_position(self.robot,
position=0.2,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[1] * 3,
pos_offset=[0, 0, 0],
orient_offset=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
maximal=False)
elif self.robot_type == 'jaco':
target_orient = p.getQuaternionFromEuler(np.array(
[0, np.pi / 2.0, 0]),
physicsClientId=self.id)
base_position, _, self.target_robot_joint_positions = self.position_robot_toc(
self.robot,
8, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
pos_offset=np.array([0.1, 0.55, 0.6]),
max_ik_iterations=200,
step_sim=True,
random_position=0.1,
check_env_collisions=False,
human_joint_indices=human_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.target_robot_joint_positions = self.target_robot_joint_positions[
0]
self.world_creation.set_gripper_open_position(self.robot,
position=1.1,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[1] * 3,
pos_offset=[-0.01, 0, 0.03],
orient_offset=p.getQuaternionFromEuler(
[0, -np.pi / 2.0, 0], physicsClientId=self.id),
maximal=False)
# Load a nightstand in the environment for the jaco arm
self.nightstand_scale = 0.275
visual_filename = os.path.join(self.world_creation.directory,
'nightstand', 'nightstand.obj')
collision_filename = os.path.join(self.world_creation.directory,
'nightstand', 'nightstand.obj')
nightstand_visual = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=[self.nightstand_scale] * 3,
rgbaColor=[0.5, 0.5, 0.5, 1.0],
physicsClientId=self.id)
nightstand_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=[self.nightstand_scale] * 3,
physicsClientId=self.id)
nightstand_pos = np.array([-0.85, 0.12, 0]) + base_position
nightstand_orient = p.getQuaternionFromEuler(
np.array([np.pi / 2.0, 0, 0]), physicsClientId=self.id)
self.nightstand = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=nightstand_collision,
baseVisualShapeIndex=nightstand_visual,
basePosition=nightstand_pos,
baseOrientation=nightstand_orient,
baseInertialFramePosition=[0, 0, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
self.generate_targets()
p.setPhysicsEngineParameter(numSubSteps=0,
numSolverIterations=50,
physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.human, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.tool, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
if self.replay:
print(os.path.join(self.replay_dir, 'frame_0.bullet'))
p.restoreState(
fileName=os.path.join(self.replay_dir, 'frame_0.bullet'))
obs = self._get_obs([0], [0, 0])
if self.vr and self.participant >= 0:
p.saveBullet(self.save_filename % 0)
with open(os.path.join(self.directory, 'setup.pkl'), 'wb') as f:
pickle.dump([
self.robot_type, self.gender, self.hipbone_to_mouth_height
], f)
return obs
def events():
return [VREvent(e) for e in pb.getVREvents()]
def take_vr_step(self):
events = p.getVREvents(deviceTypeFilter=p.VR_DEVICE_HMD+p.VR_DEVICE_CONTROLLER, physicsClientId=self.id)
if len(events) == 3:
self.head_waist_sim(events)
