def main():
"""
main function
"""
setup_world()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
# urdf_fn = '/Users/krishneelchaudhary/Downloads/bullet3/data/kuka_lwr/kuka.urdf'
urdf_fn = '/Users/krishneelchaudhary/Downloads/bullet3/examples/pybullet/gym/pybullet_data/kuka_iiwa/model_for_sdf.urdf'
base_pos = [0, 0, 0]
base_ori = p.getQuaternionFromEuler([0, 0, math.pi])
kuka_id = p.loadURDF(urdf_fn,
base_pos,
base_ori,
flags=p.URDF_USE_SELF_COLLISION)
for _ in range(10000):
p.saveBullet('snap', CLIENT)
# position, quaternion = p.getBasePositionAndOrientation(kuka_id)
# angles = p.getEulerFromQuaternion(quaternion)
# p.stepSimulation()
time.sleep(1 / 240.0)
p.disconnect()
def save_state(self, filepath):
"""Save simulation state to .bullet file.
Args:
filepath (str): filepath to store .bullet file
"""
pybullet.saveBullet(filepath)
def saveEnvToFile(self, path):
bullet_file = os.path.join(path, 'env.bullet')
pickle_file = os.path.join(path, 'env.pickle')
pb.saveBullet(bullet_file)
state = self.getStateDict()
with open(pickle_file, 'wb') as f:
pickle.dump(state, f)
def save_state_bullet(self,fileName):
""" Save the environment state to the .bullet file.
Args : None
Returns: state_id (int) in memory identifier of state in pybullet.
"""
pb.saveBullet(bulletFileName=fileName, physicsClientId=self.world.physics_id)
return fileName
def simulator(self):
"""
This function is used to do some test for simulator
"""
# connect gpu
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
# set gravity
p.setGravity(0, 0, 0)
# model initial location
StartPos = self.base_pos
# model initial orientation in Euler
StartOrientation = p.getQuaternionFromEuler(self.base_ori)
# load model file and set the initial position and fixed base link
boxId = p.loadURDF("barrett_hand_target.urdf", StartPos, StartOrientation, useFixedBase=True)
# load object model
# object = p.loadURDF("object.urdf", useFixedBase=True)
# set gripper be the loaded model
gripper = boxId
# set camera parameters
p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=45, cameraPitch=0, cameraTargetPosition=[0.5, 0.5, 0.2])
LinkId = []
xxx = 0
while xxx != 10:
xxx += 1
print(xxx)
p.resetSimulation()
StartOrientation = p.getQuaternionFromEuler(self.base_ori)
boxId = p.loadURDF("barrett_hand_target.urdf", self.base_pos, StartOrientation, useFixedBase=True)
gripper = boxId
self.new_random_state()
for i in range(0, p.getNumJoints(gripper)):
p.setJointMotorControl2(gripper, i, p.POSITION_CONTROL, targetPosition=0, force=0)
# obtain the limit rotation angle range of the joint
lower_limit = p.getJointInfo(gripper, i)[8]
upper_limit = p.getJointInfo(gripper, i)[9]
# obtain the joint name
linkName = p.getJointInfo(gripper, i)[12].decode("ascii")
# set the gui control board
LinkId.append(p.addUserDebugParameter(linkName, lower_limit, upper_limit, 0))
moveable_joint_ID = [0, 1, 2, 5, 6, 7, 10, 11]
# move joints following command
for i in range(len(moveable_joint_ID)):
linkPos = self.angle_state[i]
p.setJointMotorControl2(gripper, moveable_joint_ID[i], p.POSITION_CONTROL, targetPosition=linkPos)
# start do simulation
p.stepSimulation()
time.sleep(1. / 240.)
p.saveBullet('Test.bullet')
p.disconnect()
def simulator(Initial_pos=[0, 0, 0], Initial_angle=[0, 0, 0]):
"""
This function is used to do simulation for the gripper.
:param Initial_pos: The initial position of the barrett hand.
It should be a list with three values which means at x, y, z directions.
:param Initial_angle: The initial orientation of the barrett hand.
It should be a list with three values which means the rotation angle at x, y, z directions.
The unit of the rotation angle is the radian system.
:return: The state of the gripper.
"""
# connect physical server
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
# set gravity
p.setGravity(0, 0, 0)
# create an empty link list to store link id
LinkId = []
# model initial location
StartPos = Initial_pos
# model initial orientation in Euler
StartOrientation = p.getQuaternionFromEuler(Initial_angle)
# load model file and set the initial position and fixed base link
boxId = p.loadURDF("barrett_hand_target.urdf", StartPos, StartOrientation, useFixedBase=True)
# load object model
object = p.loadURDF("object.urdf", useFixedBase=True)
# set gripper be the loaded model
gripper = boxId
# set camera parameters
p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=45, cameraPitch=0, cameraTargetPosition=[0.5, 0.5, 0.2])
# for loop to obtain the joint information and set parameters
for i in range(0, p.getNumJoints(gripper)):
p.setJointMotorControl2(gripper, i, p.POSITION_CONTROL, targetPosition=0, force=0)
# obtain the limit rotation angle range of the joint
lower_limit = p.getJointInfo(gripper, i)[8]
upper_limit = p.getJointInfo(gripper, i)[9]
# obtain the joint name
linkName = p.getJointInfo(gripper, i)[12].decode("ascii")
# set the gui control board
LinkId.append(p.addUserDebugParameter(linkName, lower_limit, upper_limit, 0))
while p.isConnected():
# start do simulation
p.stepSimulation()
time.sleep(1. / 240.)
# move joints following command
for i in range(0, p.getNumJoints(gripper)):
linkPos = p.readUserDebugParameter((LinkId[i]))
p.setJointMotorControl2(gripper, i, p.POSITION_CONTROL, targetPosition=linkPos)
p.saveBullet('Test.bullet')
p.disconnect()
def saveStatus(self, save_state=False, on_disk=False, filename=None):
"""Save the current status of the simulation.
Either stores
- an initial configuration (save_state = False ,on_disk = False) with a sufficient filename
- current state in memory (save_state = True, on_disk = False)
- current state on disk (save_state = True, on_disk = True) with a sufficient filename
Is a wrapper for pybullet.saveWorld, saveState, saveBullet
Returns:
state_id, filename
"""
if save_state and on_disk:
if not filename:
print("Supply a valid filename!")
return
pybullet.saveBullet(filename)
return filename
if save_state and not on_disk:
if not hasattr(self, "stored_states"):
self.stored_states = []
self.stored_states.append(pybullet.saveState())
return self.stored_states[-1]
if not save_state and not on_disk:
if not filename:
print("Supply a valid filename!")
return
if not hasattr(self, "stored_states"):
self.stored_states = []
self.stored_states.append(
pybullet.saveWorld(fileName=filename, clientServerId=self.client_id)
)
return self.stored_states[-1], filename
def testSaveRestoreState(self):
numSteps = 500
numSteps2 = 30
verbose = 0
self.setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
self.dumpStateToFile(file)
file.close()
#################################
self.setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
stateId = p.saveState()
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
self.dumpStateToFile(file)
file.close()
p.restoreState(stateId)
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
self.dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
def saveFile(self, filename):
p.saveBullet(filename)
def take_step(self, action, robot_arm='left', gains=0.05, forces=1, human_gains=0.1, human_forces=1, step_sim=True):
action = np.clip(action, a_min=self.action_space.low, a_max=self.action_space.high)
if self.last_sim_time is None:
self.last_sim_time = time.time()
action *= 0.05
action_robot = action
indices = self.robot_left_arm_joint_indices if robot_arm == 'left' else self.robot_right_arm_joint_indices if robot_arm == 'right' else self.robot_both_arm_joint_indices
if self.vr:
if self.human_control:
action_robot = action[:self.action_robot_len]
robot_joint_states = p.getJointStates(self.robot, jointIndices=indices, physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
for _ in range(self.frame_skip):
action_robot[robot_joint_positions + action_robot < self.robot_lower_limits] = 0
action_robot[robot_joint_positions + action_robot > self.robot_upper_limits] = 0
robot_joint_positions += action_robot
p.setJointMotorControlArray(self.robot, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=robot_joint_positions, positionGains=np.array([gains]*self.action_robot_len), forces=[forces]*self.action_robot_len, physicsClientId=self.id)
if step_sim:
for frame in range(self.frame_skip):
self.take_vr_step()
p.stepSimulation(physicsClientId=self.id)
self.update_targets()
if self.vr and self.participant >= 0:
p.saveBullet(self.save_filename % (self.iteration*self.frame_skip + frame + 1), physicsClientId=self.id)
if self.gui:
# Slow down time so that the simulation matches real time
self.slow_time()
self.record_video_frame()
else:
if self.human_control or (self.world_creation.human_impairment == 'tremor' and self.human_controllable_joint_indices):
human_len = len(self.human_controllable_joint_indices)
if self.human_control:
action_robot = action[:self.action_robot_len]
action_human = action[self.action_robot_len:]
else:
action_human = np.zeros(human_len)
if len(action_human) != human_len:
print('Received human actions of length %d does not match expected action length of %d' % (len(action_human), human_len))
exit()
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
robot_joint_states = p.getJointStates(self.robot, jointIndices=indices, physicsClientId=self.id)
robot_joint_positions = np.array([x[0] for x in robot_joint_states])
for _ in range(self.frame_skip):
action_robot[robot_joint_positions + action_robot < self.robot_lower_limits] = 0
action_robot[robot_joint_positions + action_robot > self.robot_upper_limits] = 0
robot_joint_positions += action_robot
if self.human_control or (self.world_creation.human_impairment == 'tremor' and self.human_controllable_joint_indices):
action_human[human_joint_positions + action_human < self.human_lower_limits] = 0
action_human[human_joint_positions + action_human > self.human_upper_limits] = 0
if self.world_creation.human_impairment == 'tremor':
human_joint_positions = self.target_human_joint_positions + self.world_creation.human_tremors * (1 if self.iteration % 2 == 0 else -1)
self.target_human_joint_positions += action_human
human_joint_positions += action_human
p.setJointMotorControlArray(self.robot, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=robot_joint_positions, positionGains=np.array([gains]*self.action_robot_len), forces=[forces]*self.action_robot_len, physicsClientId=self.id)
if self.human_control or (self.world_creation.human_impairment == 'tremor' and self.human_controllable_joint_indices):
p.setJointMotorControlArray(self.human, jointIndices=self.human_controllable_joint_indices, controlMode=p.POSITION_CONTROL, targetPositions=human_joint_positions, positionGains=np.array([human_gains]*human_len), forces=[human_forces*self.world_creation.human_strength]*human_len, physicsClientId=self.id)
if step_sim:
# Update robot position
for _ in range(self.frame_skip):
p.stepSimulation(physicsClientId=self.id)
if self.human_control:
self.enforce_realistic_human_joint_limits()
self.enforce_hard_human_joint_limits()
self.update_targets()
if self.gui:
# Slow down time so that the simulation matches real time
self.slow_time()
self.record_video_frame()
self.iteration += 1
def simulator(Initial_pos=[0, 0, 0], Initial_angle=[0, 0, 0]):
"""
This function is used to do simulation
:param Initial_pos: The initial position of the base link
:param Initial_angle: The initial orientation of the base link
"""
# initial the robot class
robot = sys_state.system_state()
# set the starting values
T = 1e5
alpha = 0.99
old_Q = -20
iteration = 0
max_iteration = 20000
Q_list = []
# connect physical server
physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
# set gravity
p.setGravity(0, 0, 0)
# load model file and set the initial position and fixed base link
gripper = p.loadURDF("barrett_hand_target.urdf", useFixedBase=True)
# load object model
object = p.loadURDF("object.urdf", useFixedBase=True)
# set camera parameters
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=45,
cameraPitch=0,
cameraTargetPosition=[0.5, 0.5, 0.2])
while iteration != max_iteration:
print("Iteration Number is ", iteration)
# start do simulation
p.stepSimulation()
time.sleep(1. / 240.)
# move joints to the new state
moveable_joint_ID = [0, 1, 2, 5, 6, 7, 10, 11]
for i in range(len(moveable_joint_ID)):
linkPos = robot.angle_state[i]
p.setJointMotorControl2(gripper,
moveable_joint_ID[i],
p.POSITION_CONTROL,
targetPosition=linkPos)
# create a dictionary to store values
vector = {
'f1_dist': {
'gripper': None,
'distance': None,
'normal': None
},
'f1_med': {
'gripper': None,
'distance': None,
'normal': None
},
'f2_dist': {
'gripper': None,
'distance': None,
'normal': None
},
'f2_med': {
'gripper': None,
'distance': None,
'normal': None
},
'f3_dist': {
'gripper': None,
'distance': None,
'normal': None
},
'f3_med': {
'gripper': None,
'distance': None,
'normal': None
},
'base': {
'gripper': None,
'distance': None,
'normal': None
}
}
# store the contact point location, the distance vector and the normal vector
contact_joint_linkID = [3, 4, 8, 9, 12, 13, 14]
key = list(vector.keys())
for n in range(len(contact_joint_linkID)):
# the contact point location
gripper_c = np.array(
p.getClosestPoints(gripper,
object,
5.0,
linkIndexA=contact_joint_linkID[n])[0][5])
vector[key[n]]['gripper'] = list(gripper_c)
# the distance vector
object_c = np.array(
p.getClosestPoints(gripper,
object,
5.0,
linkIndexA=contact_joint_linkID[n])[0][6])
vector[key[n]]['distance'] = object_c - gripper_c
# the normal vector
vector[key[n]]['normal'] = \
-np.array(p.getClosestPoints(gripper, object, 5.0, linkIndexA=contact_joint_linkID[n])[0][7])
# collision detection
distance = []
for item in vector.keys():
distance.append(np.linalg.norm(vector[item]['distance']))
# if the collision does not happen
if (np.array(distance)).all() > 0.025:
# calculate the quality score of the new state
new_Q = quality_function(vector_dict=vector,
normal=robot.contact_pts_and_n_vector())
# store the quality value
Q_list.append(old_Q)
# calculate the difference between the quality scores of the two states
delta_Q = old_Q - new_Q
# judge whether jump to new state
if jump(delta_E=delta_Q, Tem=T):
old_Q = new_Q
# save the current state
p.saveBullet('result_state/Best.bullet')
T = alpha * T
# update the iteration number
iteration += 1
# generate the new random state
robot.new_random_state()
# set the base link to the new state
p.resetBasePositionAndOrientation(
gripper, robot.base_pos, p.getQuaternionFromEuler(robot.base_ori))
p.disconnect()
# plot the quality versus iteration image
plt.plot(Q_list, 'k-')
plt.xlabel('Iteration Number')
plt.ylabel('Quality')
plt.grid()
plt.savefig('result_plot/Quality plot for {0} iterations.png'.format(
max_iteration))
plt.show()
def __init__(self, *args):
super(PulleySeesaw, self).__init__(*args)
self.experiment_dict['reachable_max_height'] = 1.2
self.restore_state = True
self.training = not self.experiment_dict['render']
connect(use_gui=self.experiment_dict['render'])
if (self.detailed_gmp):
self.robot = p.loadURDF(
DRAKE_IIWA_URDF, useFixedBase=True,
globalScaling=1.2) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
else:
self.robot = None
self.KNOT = 0.1
self.NOKNOT = 0
self.break_on_timeout = True
self.config_size = 20
self.ARM_LEN = 1.6
self.predict_mask = list(range(self.config_size - 5))
x_scene_offset = 0.9
num_small_blue = 5
self.perspectives = [(0, -90)]
self.pulley = p.loadSDF("models/pulley/newsdf.sdf", globalScaling=2.5)
for pid in self.pulley:
set_pose(
pid,
Pose(Point(x=x_scene_offset - 0.19, y=-0.15, z=1.5),
Euler(roll=math.pi / 2.0, pitch=0, yaw=1.9)))
self.floor = p.loadURDF('models/short_floor.urdf', useFixedBase=True)
self.seesaw = p.loadURDF("models/seesaw.urdf", useFixedBase=True)
self.seesaw_joint = 1
set_pose(self.seesaw, Pose(Point(x=x_scene_offset + 0.8, y=0, z=0)))
self.block_pos = [x_scene_offset - 1, -0.8, 0.05]
self.black_block = p.loadURDF("models/box_heavy.urdf",
useFixedBase=True)
set_pose(
self.black_block,
Pose(
Point(x=self.block_pos[0],
y=self.block_pos[1],
z=self.block_pos[2])))
self.objects = []
small_boxes = [
"small_blue_heavy", "small_purple_heavy", "small_green_heavy",
"small_red_heavy", "small_yellow_heavy"
]
for i in range(num_small_blue):
self.objects.append(
p.loadURDF("models/" + str(small_boxes[i]) + ".urdf",
useFixedBase=False))
self.red_block = p.loadURDF("models/box_red.urdf", useFixedBase=False)
set_pose(self.red_block, Pose(Point(x=x_scene_offset + 1.7, y=0,
z=0.5)))
self.useMaximalCoordinates = True
sphereRadius = 0.01
self.mass = 1
self.basePosition = [x_scene_offset - 0.2, -1.7, 1.6]
self.baseOrientation = [0, 0, 0, 1]
if (self.training):
p.setGravity(0, 0, -10)
self.cup = p.loadURDF("models/cup/cup_small.urdf",
useFixedBase=True)
self.sphereUid = self.cup
set_pose(self.cup, Pose(Point(x=x_scene_offset - 0.2, y=0.1, z=1)))
self.knot = self.KNOT
else:
self.cupCollideId = p.createCollisionShape(
p.GEOM_MESH,
fileName="models/cup/Cup/cup_vhacd.obj",
meshScale=[6, 6, 1.5],
collisionFrameOrientation=p.getQuaternionFromEuler(
[math.pi / 2.0, 0, 0]),
collisionFramePosition=[0.07, 0.3, 0])
self.cupShapeId = p.createVisualShape(
p.GEOM_MESH,
fileName="models/cup/Cup/textured-0008192.obj",
meshScale=[6, 6, 1.5],
rgbaColor=[1, 0.886, 0.552, 1],
visualFrameOrientation=p.getQuaternionFromEuler(
[math.pi / 2.0, 0, 0]),
visualFramePosition=[0.07, 0.3, 0])
self.colBoxId = p.createCollisionShape(
p.GEOM_CYLINDER,
radius=sphereRadius,
height=0.03,
halfExtents=[sphereRadius, sphereRadius, sphereRadius],
collisionFrameOrientation=p.getQuaternionFromEuler(
[math.pi / 2.0, 0, 0]))
self.visualcolBoxId = p.createVisualShape(
p.GEOM_CYLINDER,
radius=sphereRadius,
length=0.03,
halfExtents=[sphereRadius, sphereRadius, sphereRadius],
rgbaColor=[1, 0.886, 0.552, 1],
visualFrameOrientation=p.getQuaternionFromEuler(
[math.pi / 2.0, 0, 0]))
# visualShapeId = -1
self.link_Masses = []
self.linkCollisionShapeIndices = []
self.linkVisualShapeIndices = []
self.linkPositions = []
self.linkOrientations = []
self.linkInertialFramePositions = []
self.linkInertialFrameOrientations = []
self.indices = []
self.jointTypes = []
self.axis = []
numel = 70
for i in range(numel):
self.link_Masses.append(0.3)
self.linkCollisionShapeIndices.append(self.colBoxId)
self.linkVisualShapeIndices.append(self.visualcolBoxId)
self.linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0])
self.linkOrientations.append([0, 0, 0, 1])
self.linkInertialFramePositions.append([0, 0, 0])
self.linkInertialFrameOrientations.append([0, 0, 0, 1])
self.indices.append(i)
self.jointTypes.append(p.JOINT_FIXED)
self.axis.append([0, 0, 1])
self.link_Masses.append(30)
self.linkCollisionShapeIndices.append(self.cupCollideId)
self.linkVisualShapeIndices.append(self.cupShapeId)
self.linkPositions.append([0, 0, 0])
self.linkOrientations.append([0, 0, 0, 1])
self.linkInertialFramePositions.append([0, 0, 0])
self.linkInertialFrameOrientations.append([0, 0, 0, 1])
self.indices.append(numel)
self.jointTypes.append(p.JOINT_FIXED)
self.axis.append([0, 0, 1])
self.sphereUid = p.createMultiBody(
self.mass,
-1,
-1,
self.basePosition,
self.baseOrientation,
linkMasses=self.link_Masses,
linkCollisionShapeIndices=self.linkCollisionShapeIndices,
linkVisualShapeIndices=self.linkVisualShapeIndices,
linkPositions=self.linkPositions,
linkOrientations=self.linkOrientations,
linkInertialFramePositions=self.linkInertialFramePositions,
linkInertialFrameOrientations=self.
linkInertialFrameOrientations,
linkParentIndices=self.indices,
linkJointTypes=self.jointTypes,
linkJointAxis=self.axis,
useMaximalCoordinates=self.useMaximalCoordinates)
p.setRealTimeSimulation(0)
self.anistropicFriction = [0.00, 0.00, 0.00]
p.changeDynamics(self.sphereUid,
-1,
lateralFriction=0,
anisotropicFriction=self.anistropicFriction)
if (self.restore_state):
self.keystone = p.loadURDF("models/tiny_green.urdf")
p.setGravity(0, 0, -10)
if (self.restore_state):
saved_world = p.restoreState(
fileName="./temp/pulley_start_state.bullet")
else:
for j in range(100):
p.stepSimulation(physicsClientId=0)
self.knot = p.createConstraint(self.black_block,
-1,
self.sphereUid,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=self.block_pos,
childFramePosition=self.block_pos)
if (not self.restore_state):
for j in range(3000):
p.stepSimulation(physicsClientId=0)
if (not self.restore_state):
self.keystone = p.loadURDF("models/tiny_green.urdf")
set_pose(self.keystone, Pose(Point(x=0.7, y=0, z=0.9)))
for j in range(1000):
p.stepSimulation(physicsClientId=0)
saved_world = p.saveState()
p.saveBullet("./temp/pulley_start_state.bullet")
self.static_objects = [
self.red_block, self.black_block, self.sphereUid
]
self.macroaction = MacroAction(
[
PickPlace(objects=self.objects,
robot=self.robot,
fixed=self.fixed_objects,
gmp=self.detailed_gmp),
# AddLink(objects = self.objects, robot=self.robot, fixed=self.fixed_objects, gmp=self.detailed_gmp),
],
experiment_dict=self.experiment_dict)
self.action_space_size = self.macroaction.action_space_size
self.config_size = 6 * 6 + len(
self.macroaction.link_status) # (4 for links)
self.action_space = spaces.Box(low=-1,
high=1,
shape=(self.action_space_size, ))
self.actor_critic = opt_cuda(
Policy([self.config_size],
self.action_space,
base_kwargs={'recurrent': False}))
self.predict_mask = [0, 1, 2] + [6, 7, 8] + [12, 13, 14] + [18, 19, 20]
self.config_state_attrs()
def setup_workspace(self,
rectangle_loc=[[0.562, 0.003, 0.016], [1, 0., 0., 0]],
load_previous=False,
circle_loc=[[0.425, 0.101, 0.01], [1, 0., 0., 0]],
square_loc=None,
obstacle_loc=[[0.53172045, -0.03062703, 0.07507126],
[1, -0., 0., 0]],
board_loc=[[0.479, 0.0453, 0.013],
[0.707, 0.707, 0., 0.]],
hole_goal=[[0.55, 0.08, 0.0], [1, 0, 0, 0]]):
#RigidTransform(rotation=np.array([[-5.78152806e-02, -9.98327119e-01, 4.84639353e-07],
# [-9.98327425e-01, 5.78157598e-02, 3.97392158e-08],
# [ 4.07518811e-07, -6.59092487e-08, -9.99999635e-01]]), translation=np.array([ 0.53810962, 0.08998347, -0.00768057]), from_frame='peg_center', to_frame='world')
#hole_pose = (np.array([ 0.53810962, 0.08998347, -0.00768057]), np.array([-2.89198577e-02, -9.19833769e-08, 1.37694750e-07, 9.99581685e-01]))
#self.floor = p.loadURDF("../../models/short_floor.urdf")
#make board
width = 0.4
length = 0.3
fake_board_thickness = 0.05
height = 0.01
block_height = 0.015
self.hole_depth = block_height
self.block_height = block_height
self.board = ut.create_box(width,
length,
height + fake_board_thickness,
color=(1, 0.7, 0, 1))
self.frontcamera_block = ut.create_box(0.4,
0.2,
0.4,
color=(.2, 0.7, 0, 0.2))
self.topcamera_block = ut.create_box(0.3,
0.08,
0.15,
color=(1, 0.7, 0, 0.2))
self.controlpc_block = ut.create_box(0.4,
0.4,
0.42,
color=(1, 0.7, 0, 0.2))
board_loc[0][-1] -= 0.5 * fake_board_thickness
ut.set_pose(self.board, board_loc)
ut.set_point(self.frontcamera_block, (0.6 + .15, 0, 0.1))
ut.set_point(self.topcamera_block, (0.5, 0, 0.95))
ut.set_point(self.controlpc_block, (-0.1, 0.45, 0.2))
#make circle
#make rectangle
self.hole = ut.create_box(0.092, 0.069, 0.001, color=(0.1, 0, 0, 1))
board_z = 0.013 + 0.005
#The perception z axis estimates are bad so let's use prior information to give it the right pose
for loc in [rectangle_loc, circle_loc, square_loc]:
if loc is not None:
loc[0][-1] = board_z + 0.5 * block_height
hole_goal[0][-1] = board_z + 0.5 * 0.001
if load_previous:
rectangle_loc = np.load("saves/rectangle_loc.npy",
allow_pickle=True)
circle_loc = np.load("saves/circle_loc.npy", allow_pickle=True)
obstacle_loc = np.load("saves/obstacle_loc.npy", allow_pickle=True)
square_loc = np.load("saves/square_loc.npy", allow_pickle=True)
hole_goal = np.load("saves/hole_loc.npy", allow_pickle=True)
for loc in [
rectangle_loc, circle_loc, obstacle_loc, hole_goal,
square_loc
]:
if loc is None or loc[0].any() is None:
loc = None
else:
np.save("saves/rectangle_loc.npy", rectangle_loc)
np.save("saves/circle_loc.npy", circle_loc)
np.save("saves/obstacle_loc.npy", obstacle_loc)
np.save("saves/hole_loc.npy", hole_goal)
np.save("saves/square_loc.npy", square_loc)
if obstacle_loc is not None:
self.obstacle = ut.create_box(0.08,
0.04,
0.1,
color=(0.5, 0.5, 0.5, 1))
obstacle_loc[0][-1] = board_z + 0.5 * 0.1
ut.set_pose(self.obstacle, obstacle_loc)
else:
self.obstacle = None
if circle_loc is not None:
radius = 0.078 / 2
self.circle = ut.create_cylinder(radius,
block_height,
color=(1, 1, 0, 1))
ut.set_pose(self.circle, circle_loc)
else:
self.circle = None
if rectangle_loc is not None:
self.rectangle = ut.create_box(0.092,
0.069,
block_height,
color=(0.3, 0, 0.1, 1))
ut.set_pose(self.rectangle, rectangle_loc)
else:
self.rectangle = None
if square_loc is not None:
self.square = ut.create_box(0.072,
0.072,
block_height,
color=(0.8, 0, 0.1, 1))
ut.set_pose(self.square, square_loc)
self.hole_goal = hole_goal
self.shape_name_to_shape = {}
self.shape_name_to_shape[Obstacle.__name__] = self.obstacle
ut.set_pose(self.hole, hole_goal)
self.shape_name_to_shape[Circle.__name__] = self.circle
self.shape_name_to_shape[Rectangle.__name__] = self.rectangle
self.shape_name_to_shape[Square.__name__] = self.square
input("workspace okay?")
p.saveBullet("curr_state.bt")
def testSaveRestoreState(self):
numSteps = 500
numSteps2 = 30
verbose = 0
self.setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
self.dumpStateToFile(file)
file.close()
#################################
self.setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
stateId = p.saveState()
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
self.dumpStateToFile(file)
file.close()
p.restoreState(stateId)
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
self.dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
def save(self):
p.saveBullet("state.bullet")
return ["state.bullet", self.t, self.obs]
fromfile='saveFile.txt',
tofile='restoreFile.txt',
)
numDifferences = 0
for line in diff:
numDifferences = numDifferences+1
sys.stdout.write(line)
if (numDifferences>0):
print("Error:", numDifferences, " lines are different between files.")
else:
print("OK, files are identical")
setupWorld()
for i in range (numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range (numSteps2):
p.stepSimulation()
file = open("saveFile.txt","w")
dumpStateToFile(file)
tofile='restoreFile.txt',
)
numDifferences = 0
for line in diff:
numDifferences = numDifferences + 1
sys.stdout.write(line)
if (numDifferences > 0):
print("Error:", numDifferences, " lines are different between files.")
else:
print("OK, files are identical")
setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
dumpStateToFile(file)
file.close()
def save_bullet(filename):
p.saveBullet(filename, physicsClientId=CLIENT)
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 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,
'feeding_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
if self.vr or self.replay:
if self.vr:
seat_to_hmd_height = self.hipbone_to_mouth_height + (
0.068 +
0.1335 * self.config('radius_scale', 'human_female')
if self.gender == 'male' else 0.058 +
0.127 * self.config('radius_scale', 'human_female'))
p.setOriginCameraPositionAndOrientation(
deviceTypeFilter=p.VR_DEVICE_HMD,
pos_offset=[0, -0.06, -(0.47 + seat_to_hmd_height)],
orn_offset=[0, 0, 0, 1])
self.human, self.left_arm, self.right_arm, self.wheelchair, 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='wheelchair',
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.wheelchair, 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='wheelchair',
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.wheelchair, 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='wheelchair',
static_human_base=True,
human_impairment='random',
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_right_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_right_arm_joint_indices]
self.reset_robot_joints()
# Place a bowl of food on a table
self.table = p.loadURDF(os.path.join(self.world_creation.directory,
'table', 'table_tall.urdf'),
basePosition=[0.35, -0.9, 0],
baseOrientation=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
physicsClientId=self.id)
self.bowl_scale = 0.75
bowl_pos = np.array([-0.15, -0.55, 0.75]) + np.array([
self.np_random.uniform(-0.05, 0.05),
self.np_random.uniform(-0.05, 0.05), 0
])
self.bowl = p.loadURDF(os.path.join(self.world_creation.directory,
'dinnerware', 'bowl.urdf'),
basePosition=bowl_pos,
baseOrientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0],
physicsClientId=self.id),
useMaximalCoordinates=False,
physicsClientId=self.id)
if self.robot_type == 'jaco':
p.resetBasePositionAndOrientation(
self.robot, [-0.35, -0.3, 0.36],
[0.0, 0.0, -0.7071067811865475, 0.7071067811865476],
physicsClientId=self.id)
if self.new:
target_pos = np.array(bowl_pos) + np.array([
0, -0.1, 0.4
]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler(
np.array([np.pi / 2.0, 0, np.pi / 2.0]),
physicsClientId=self.id)
# , check_env_collisions=True!!!!!!!!!!!
_, self.target_robot_joint_positions = self.util.ik_random_restarts(
self.robot,
8,
target_pos,
target_orient,
self.world_creation,
self.robot_right_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
max_iterations=1000,
max_ik_random_restarts=40,
random_restart_threshold=0.01,
step_sim=True,
check_env_collisions=True
if self.vr or self.replay else False or self.replay)
self.world_creation.set_gripper_open_position(
self.robot, position=1.33, left=False, set_instantly=True)
self.spoon = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.08] * 3,
pos_offset=[0.1, -0.0225, 0.03],
orient_offset=p.getQuaternionFromEuler(
[-0.1, -np.pi / 2.0, 0], physicsClientId=self.id),
left=False,
maximal=False)
if self.vr or self.replay:
self.human_controllable_joint_indices = [
0, 1, 2, 25, 26, 27
] + list(range(7, 14)) + list(range(17, 24))
joints_positions = [(10, np.deg2rad(-90)), (20, np.deg2rad(-90)),
(28, np.deg2rad(-90)), (31, np.deg2rad(80)),
(35, np.deg2rad(-90)), (38, np.deg2rad(80))]
joints_positions += [
(25, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(26, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(27, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30)))
]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices,
use_static_joints=True,
human_reactive_force=None)
p.resetBasePositionAndOrientation(self.human, [
0, 0.03,
0.89 - 0.23725 if self.gender == 'male' else 0.86 - 0.225
], [0, 0, 0, 1],
physicsClientId=self.id)
human_joint_states = p.getJointStates(
self.human,
jointIndices=[24, 25, 26, 27],
physicsClientId=self.id)
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)
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_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)
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:
self.human_controllable_joint_indices = [24, 25, 26, 27]
if self.new:
p.resetBasePositionAndOrientation(self.human, [
0, 0.03,
0.89 - 0.23725 if self.gender == 'male' else 0.86 - 0.225
], [0, 0, 0, 1],
physicsClientId=self.id)
human_positioned = False
right_arm_shoulder_links = [6, 9, 11, 13]
left_arm_shoulder_links = [16, 19, 21, 23]
r_arm_wheelchair_dists = []
l_arm_wheelchair_dists = []
while not human_positioned or (np.min(human_robot_dists + [1])
< 0.01):
human_positioned = True
joints_positions = [(10, np.deg2rad(-90)),
(20, np.deg2rad(-90)),
(28, np.deg2rad(-90)),
(31, np.deg2rad(80)),
(35, np.deg2rad(-90)),
(38, np.deg2rad(80))]
joints_positions += [
(i, 0)
for i in list(range(7, 14)) + list(range(17, 24))
]
joints_positions += [
(i,
self.np_random.uniform(np.deg2rad(-30),
np.deg2rad(30)))
for i in [25, 26, 27]
]
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, [],
use_static_joints=True,
human_reactive_force=None)
if self.robot_type == 'jaco':
human_robot_dists = [
c[8] for c in p.getClosestPoints(
bodyA=self.human,
bodyB=self.robot,
distance=0.05,
physicsClientId=self.id)
]
else:
human_robot_dists = []
else:
joints_positions = [
(10, np.deg2rad(-90)), (20, np.deg2rad(-90)),
(28, np.deg2rad(-90)), (31, np.deg2rad(80)),
(35, np.deg2rad(-90)), (38, np.deg2rad(80))
]
joints_positions += [
(i, self.np_random.uniform(np.deg2rad(-30),
np.deg2rad(30)))
for i in [25, 26, 27]
]
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)
p.resetBasePositionAndOrientation(self.human, [
0, 0.03,
0.89 - 0.23725 if self.gender == 'male' else 0.86 - 0.225
], [0, 0, 0, 1],
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])
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]
# Set target on mouth
self.mouth_pos = [0, -0.11, 0.03
] if self.gender == 'male' else [0, -0.1, 0.03]
head_pos, head_orient = p.getLinkState(self.human,
27,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
target_pos, target_orient = p.multiplyTransforms(
head_pos,
head_orient,
self.mouth_pos, [0, 0, 0, 1],
physicsClientId=self.id)
self.target_pos = np.array(target_pos)
sphere_collision = -1
if self.vr:
sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.01,
rgbaColor=[0, 1, 0, 0],
physicsClientId=self.id)
else:
sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.01,
rgbaColor=[0, 1, 0, 1],
physicsClientId=self.id)
self.target = p.createMultiBody(
baseMass=0.0,
baseCollisionShapeIndex=sphere_collision,
baseVisualShapeIndex=sphere_visual,
basePosition=self.target_pos,
useMaximalCoordinates=False,
physicsClientId=self.id)
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
human_joint_indices = [24, 25, 26, 27]
if self.robot_type == 'pr2':
target_pos = np.array(bowl_pos) + np.array(
[0, -0.1, 0.4]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler([np.pi / 2.0, 0, 0],
physicsClientId=self.id)
_, _, self.target_robot_joint_positions = self.position_robot_toc(
self.robot,
54, [(target_pos, target_orient), (self.target_pos, None)],
[(self.target_pos, target_orient)],
self.robot_right_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(15, 15 + 7),
pos_offset=np.array([0.1, 0.2, 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.03,
left=False,
set_instantly=True)
self.spoon = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.08] * 3,
pos_offset=[0, -0.03, -0.11],
orient_offset=p.getQuaternionFromEuler(
[-0.2, 0, 0], physicsClientId=self.id),
left=False,
maximal=False)
elif self.robot_type == 'jaco' and not self.new:
# CHECK ENV_COLLISION
target_pos = np.array(bowl_pos) + np.array(
[0, -0.1, 0.4]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler(np.array(
[np.pi / 2.0, 0, np.pi / 2.0]),
physicsClientId=self.id)
_, self.target_robot_joint_positions = self.util.ik_random_restarts(
self.robot,
8,
target_pos,
target_orient,
self.world_creation,
self.robot_right_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
max_iterations=1000,
max_ik_random_restarts=40,
random_restart_threshold=0.01,
step_sim=True,
check_env_collisions=True if self.vr or self.replay else False)
self.world_creation.set_gripper_open_position(self.robot,
position=1.33,
left=False,
set_instantly=True)
self.spoon = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.08] * 3,
pos_offset=[0.1, -0.0225, 0.03],
orient_offset=p.getQuaternionFromEuler(
[-0.1, -np.pi / 2.0, 0], physicsClientId=self.id),
left=False,
maximal=False)
p.resetBasePositionAndOrientation(self.bowl,
bowl_pos,
p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0],
physicsClientId=self.id),
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.spoon, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=2,
numSolverIterations=10,
physicsClientId=self.id)
# Generate food
spoon_pos, spoon_orient = p.getBasePositionAndOrientation(
self.spoon, physicsClientId=self.id)
spoon_pos = np.array(spoon_pos)
food_radius = 0.005
food_collision = p.createCollisionShape(p.GEOM_SPHERE,
radius=food_radius,
physicsClientId=self.id)
food_visual = -1
if self.vr:
food_mass = 0.0
else:
food_mass = 0.001
food_count = 2 * 2 * 2
batch_positions = []
for i in range(2):
for j in range(2):
for k in range(2):
batch_positions.append(
np.array([
i * 2 * food_radius - 0.005, j * 2 *
food_radius, k * 2 * food_radius + 0.02
]) + spoon_pos)
last_food_id = p.createMultiBody(
baseMass=food_mass,
baseCollisionShapeIndex=food_collision,
baseVisualShapeIndex=food_visual,
basePosition=[0, 0, 0],
useMaximalCoordinates=False,
batchPositions=batch_positions,
physicsClientId=self.id)
self.foods = list(
range(last_food_id - food_count + 1, last_food_id + 1))
self.foods_hit_person = []
self.total_food_count = len(self.foods)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
if self.vr:
self.update_objects()
else:
# Drop food in the spoon
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
if self.replay:
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 save_state(self,
path: Union[pathlib.Path, str],
filename: str = "scene.bullet"):
path = pathlib.Path(path)
path.mkdir(parents=True, exist_ok=True)
pb.saveBullet(str(path / filename))
env = gym.make("pandaPlay-v0")
env.render(mode='human')
o = env.reset()
start_time = time.time()
while (1):
# Get the action
s.sendall(b'R')
data = s.recv(1024)
command = pickle.loads(data)
action = vr_command_to_action(env, o['observation'], command)
# states for determinsitc reset
if not debugging:
p.saveBullet(example_path + '/env_states/' + str(counter) +
".bullet")
o2, r, d, _ = env.step(action)
# main buffs - time will be variable but it just gives as an indication of actual frame rate
# true time is simulation time, we are determinsitically stepping.
acts.append(action), obs.append(o['observation']), goals.append(o['desired_goal']), ags.append(
o2['achieved_goal']), \
cagb.append(o2['controllable_achieved_goal']), fpsb.append(o2['full_positional_state']), times.append(time.time() -start_time)
#
state = env.panda.calc_actor_state()
print(state['orn'])
#print([ '%.2f' % elem for elem in state['joints']])
joints.append(state['joints'])
