31, [-0.5, 0, 0.5], [0.5, 0, 0.5], [-0.5, 0, 1.5],
[0.5, 0, 1.5],
mass=1,
scale=1,
orientation=p.getQuaternionFromEuler([0, 0, 0]),
bodyAnchorIds=[
cloth_attachment1, cloth_attachment2,
cloth_attachment3, cloth_attachment4
],
anchors=[0, 30, 960, 930],
collisionMargin=0.02)
p.clothParams(cloth,
kLST=0.1,
kAST=1.0,
kVST=1.0,
kDP=0.01,
kDF=0.5,
kCHR=1.0,
kKHR=1.0,
kAHR=1.0,
piterations=20)
elif sim == 2:
cloth = p.loadClothPatch(
31,
31, [-0.5, 0, 0.5], [0.5, 0, 0.5], [-0.5, 0, 1.5], [0.5, 0, 1.5],
mass=1,
scale=1,
orientation=p.getQuaternionFromEuler([0, np.pi / 4, 0]),
bodyAnchorIds=[
cloth_attachment1, cloth_attachment2, cloth_attachment3,
cloth_attachment4
],
def reset(self):
self.setup_timing()
self.task_success = 0
self.forearm_in_sleeve = False
self.upperarm_in_sleeve = False
self.human, 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='random')
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()
if self.robot_type == 'jaco':
wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(
self.wheelchair, physicsClientId=self.id)
p.resetBasePositionAndOrientation(
self.robot,
np.array(wheelchair_pos) + np.array([0.35, -0.3, 0.3]),
p.getQuaternionFromEuler([0, 0, 0], physicsClientId=self.id),
physicsClientId=self.id)
joints_positions = [(6, np.deg2rad(-90)), (13, np.deg2rad(-80)),
(16, np.deg2rad(-90)), (28, np.deg2rad(-90)),
(31, np.deg2rad(80)), (35, np.deg2rad(-90)),
(38, np.deg2rad(80))]
self.human_controllable_joint_indices = [
10, 11, 12, 13, 14, 15, 16, 17, 18, 19
]
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 if self.gender == 'male' else 0.86],
[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]
shoulder_pos, shoulder_orient = p.getLinkState(
self.human,
15,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human,
17,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human,
19,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
target_pos = np.array([-0.85, -0.4, 0
]) + np.array([1.85, 0.6, 0]) + np.array([
-0.55, -0.5, 1.2
]) + self.np_random.uniform(-0.05, 0.05, size=3)
offset = np.array([0, 0, 0.1])
if self.robot_type == 'pr2':
target_orient = p.getQuaternionFromEuler([0, 0, np.pi],
physicsClientId=self.id)
target_orient_shoulder = p.getQuaternionFromEuler(
[0, 0, np.pi * 3 / 2.0], physicsClientId=self.id)
self.position_robot_toc(
self.robot,
76, [(target_pos, target_orient)],
[(shoulder_pos + offset, target_orient_shoulder),
(elbow_pos + offset, target_orient),
(wrist_pos + offset, target_orient)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(29, 29 + 7),
pos_offset=np.array([1.7, 0.7, 0]),
base_euler_orient=[0, 0, np.pi],
right_side=False,
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
elif self.robot_type == 'jaco':
target_orient = p.getQuaternionFromEuler(np.array(
[0, -np.pi / 2.0, 0]),
physicsClientId=self.id)
target_joint_positions = self.util.ik_random_restarts(
self.robot,
8,
target_pos,
target_orient,
self.world_creation,
self.robot_left_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.03,
step_sim=True)
self.world_creation.set_gripper_open_position(self.robot,
position=1.33,
left=False,
set_instantly=True)
else:
target_orient = p.getQuaternionFromEuler([0, -np.pi / 2.0, 0],
physicsClientId=self.id)
target_orient_shoulder = p.getQuaternionFromEuler(
[np.pi / 2.0, -np.pi / 2.0, 0], physicsClientId=self.id)
if self.robot_type == 'baxter':
self.position_robot_toc(
self.robot,
48, [(target_pos, target_orient)],
[(shoulder_pos + offset, target_orient_shoulder),
(elbow_pos + offset, target_orient),
(wrist_pos + offset, target_orient)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(10, 17),
pos_offset=np.array([1.7, 0.7, 0.975]),
base_euler_orient=[0, 0, np.pi],
right_side=False,
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
else:
self.position_robot_toc(
self.robot,
19, [(target_pos, target_orient)],
[(shoulder_pos + offset, target_orient_shoulder),
(elbow_pos + offset, target_orient),
(wrist_pos + offset, target_orient)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 2, 3, 4, 5, 6, 7],
pos_offset=np.array([1.8, 0.7, 0.975]),
base_euler_orient=[0, 0, np.pi],
right_side=False,
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
if self.human_control or self.world_creation.human_impairment == 'tremor':
human_len = len(self.human_controllable_joint_indices)
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])
p.setJointMotorControlArray(
self.human,
jointIndices=self.human_controllable_joint_indices,
controlMode=p.POSITION_CONTROL,
targetPositions=human_joint_positions,
positionGains=np.array([0.005] * human_len),
forces=[1] * human_len,
physicsClientId=self.id)
state = p.getLinkState(
self.robot,
76 if self.robot_type == 'pr2' else 19 if self.robot_type
== 'sawyer' else 48 if self.robot_type == 'baxter' else 8,
computeForwardKinematics=True,
physicsClientId=self.id)
self.start_ee_pos = np.array(state[0])
self.start_ee_orient = np.array(state[1]) # Quaternions
self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387])
self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos
# Set up gripper - cloth constraint
gripper_visual = p.createVisualShape(p.GEOM_SPHERE,
radius=0.01,
rgbaColor=[0, 0, 0, 0],
physicsClientId=self.id)
gripper_collision = p.createCollisionShape(p.GEOM_SPHERE,
radius=0.0001,
physicsClientId=self.id)
self.cloth_attachment = p.createMultiBody(
baseMass=0,
baseVisualShapeIndex=gripper_visual,
baseCollisionShapeIndex=gripper_collision,
basePosition=self.start_ee_pos,
useMaximalCoordinates=1,
physicsClientId=self.id)
# Load cloth
self.cloth = p.loadCloth(
os.path.join(self.world_creation.directory, 'clothing',
'hospitalgown_reduced.obj'),
scale=1.4,
mass=0.23,
position=np.array([0.02, -0.38, 0.83]) + self.cloth_offset / 1.4,
orientation=p.getQuaternionFromEuler([0, 0, np.pi],
physicsClientId=self.id),
bodyAnchorId=self.cloth_attachment,
anchors=[
2087, 3879, 3681, 3682, 2086, 2041, 987, 2042, 2088, 1647, 2332
],
collisionMargin=0.04,
rgbaColor=np.array([139. / 256., 195. / 256., 74. / 256., 0.6]),
rgbaLineColor=np.array([197. / 256., 225. / 256., 165. / 256., 1]),
physicsClientId=self.id)
p.clothParams(self.cloth,
kLST=0.05,
kAST=1.0,
kVST=1.0,
kDP=0.001,
kDG=10,
kDF=0.25,
kCHR=1.0,
kKHR=1.0,
kAHR=0.5,
piterations=5,
physicsClientId=self.id)
self.triangle1_point_indices = [621, 37, 1008]
self.triangle2_point_indices = [130, 3908, 2358]
# double m_kLST; // Material: Linear stiffness coefficient [0,1]
# double m_kAST; // Material: Area/Angular stiffness coefficient [0,1]
# double m_kVST; // Material: Volume stiffness coefficient [0,1]
# double m_kVCF; // Velocities correction factor (Baumgarte)
# double m_kDP; // Damping coefficient [0,1]
# double m_kDG; // Drag coefficient [0,+inf]
# double m_kLF; // Lift coefficient [0,+inf]
# double m_kPR; // Pressure coefficient [-inf,+inf]
# double m_kVC; // Volume conversation coefficient [0,+inf]
# double m_kDF; // Dynamic friction coefficient [0,1]
# double m_kMT; // Pose matching coefficient [0,1]
# double m_kCHR; // Rigid contacts hardness [0,1]
# double m_kKHR; // Kinetic contacts hardness [0,1]
# double m_kSHR; // Soft contacts hardness [0,1]
# double m_kAHR; // Anchors hardness [0,1]
# int m_viterations; // Velocities solver iterations
# int m_piterations; // Positions solver iterations
# int m_diterations; // Drift solver iterations
p.setGravity(
0, 0, -9.81 / 2,
physicsClientId=self.id) # Let the cloth settle more gently
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
p.setGravity(0, 0, -1, body=self.human, physicsClientId=self.id)
p.setGravity(0,
0,
0,
body=self.cloth_attachment,
physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=4, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# Wait for the cloth to settle
for _ in range(200):
# Force the cloth attachment to stay at the end effector
p.resetBasePositionAndOrientation(self.cloth_attachment,
self.start_ee_pos, [0, 0, 0, 1],
physicsClientId=self.id)
p.stepSimulation(physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
return self._get_obs([0], [0, 0])
mass=1,
position=[0, 0, 2],
orientation=p.getQuaternionFromEuler([0, 0, 0]),
bodyAnchorId=cloth_attachment,
anchors=[2129, 2243, 2244, 952, 953],
collisionMargin=0.01,
kLST=0.1,
kAST=0.0,
kVST=0.0,
physicsClientId=0)
# cloth = p.loadCloth('hospitalgown_adaptivereduce.obj', scale=1, mass=1, position=[0, 0, 2], orientation=p.getQuaternionFromEuler([0, 0, 0]), bodyAnchorId=cloth_attachment, anchors=[18, 19, 199, 952, 953, 954, 1416, 1569, 1570, 1571, 1789, 1791, 1879, 1882, 1922, 2062, 2063, 2129, 2238, 2243, 2244, 2292], collisionMargin=0.01, kLST=0.1, kAST=0.0, kVST=0.0, physicsClientId=0)
# Points near rim of sleeve
# 14 points along sleeve edge: 715, 717, 756, 979, 981, 1276, 1615, 1828, 2057, 2173, 2174, 2191, 2277, 2289
# cloth = p.loadCloth('hospitalgown_adaptivereduce.obj', scale=1, mass=1, position=[0, 0, 2], orientation=p.getQuaternionFromEuler([0, 0, 0]), bodyAnchorId=cloth_attachment, anchors=[40, 41, 500, 501, 502, 554, 555, 715, 716, 717, 756, 979, 980, 981, 1276, 1613, 1614, 1615, 1827, 1828, 1962, 2057, 2106, 2114, 2173, 2174, 2191, 2203, 2277, 2289], collisionMargin=0.01, kLST=0.1, kAST=0.0, kVST=0.0, physicsClientId=0)
# cloth = p.loadCloth('hospitalgown_adaptivereduce.obj', scale=1, mass=1, position=[0, 0, 2], orientation=p.getQuaternionFromEuler([0, 0, 0]), bodyAnchorId=cloth_attachment, anchors=list(range(1580, 1600)), collisionMargin=0.01, kLST=0.1, kAST=0.0, kVST=0.0, physicsClientId=0)
p.clothParams(cloth, kDP=0.01, kDF=0.5, kAHR=1, piterations=20)
# joint_names = []
# for j in list(range(p.getNumJoints(cloth))) + [-1]:
# print(p.getJointInfo(cloth, j))
# joint_names.append((j, p.getJointInfo(cloth, j)[1]))
# print(joint_names)
# print(p.getJointInfo(cloth, 0))
# print(p.getLinkState(cloth, 0))
# print(p.getDynamicsInfo(cloth, -1))
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.5,
cameraYaw=0,
cameraPitch=-45,
def reset(self):
self.setup_timing()
self.task_success = 0
self.forearm_in_sleeve = False
self.upperarm_in_sleeve = False
self.meanft = np.zeros(6)
self.wheelchair = self.world_creation.create_new_world(furniture_type='wheelchair', static_human_base=True, human_impairment='random', gender='random')
if self.robot.wheelchair_mounted:
wheelchair_pos, wheelchair_orient = self.wheelchair.get_base_pos_orient()
self.robot.set_base_pos_orient(wheelchair_pos + np.array(self.robot.toc_base_pos_offset[self.task]), [0, 0, -np.pi/2.0], euler=True)
joints_positions = [(self.human.j_right_elbow, -90), (self.human.j_left_shoulder_x, -80), (self.human.j_left_elbow, -90), (self.human.j_right_hip_x, -90), (self.human.j_right_knee, 80), (self.human.j_left_hip_x, -90), (self.human.j_left_knee, 80)]
self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=None if self.human_control else 1, reactive_gain=0.01)
shoulder_pos = self.human.get_pos_orient(self.human.left_shoulder)[0]
elbow_pos = self.human.get_pos_orient(self.human.left_elbow)[0]
wrist_pos = self.human.get_pos_orient(self.human.left_wrist)[0]
target_ee_pos = np.array([0.45, -0.3, 1.2]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_ee_orient = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task][0]), physicsClientId=self.id))
offset = np.array([0, 0, 0.1])
if self.robot.wheelchair_mounted:
# Use IK to find starting joint angles for mounted robots
self.robot.ik_random_restarts(right=False, target_pos=target_ee_pos, target_orient=target_ee_orient, max_iterations=1000, max_ik_random_restarts=40, success_threshold=0.03, step_sim=True, check_env_collisions=False)
else:
# Use TOC with JLWKI to find an optimal base position for the robot near the person
target_ee_orient_shoulder = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task][1]), physicsClientId=self.id))
self.robot.position_robot_toc(self.task, 'left', [(target_ee_pos, target_ee_orient)], [(shoulder_pos+offset, target_ee_orient_shoulder), (elbow_pos+offset, target_ee_orient), (wrist_pos+offset, target_ee_orient)], base_euler_orient=[0, 0, np.pi], step_sim=True, check_env_collisions=False, right_side=False)
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.left_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True)
if self.human_control or self.human.impairment == 'tremor':
# Ensure the human arm remains stable while loading the cloth
self.human.control(self.human.controllable_joint_indices, self.human.get_joint_angles(self.human.controllable_joint_indices), 0.05, 1)
self.start_ee_pos, self.start_ee_orient = self.robot.get_pos_orient(self.robot.left_end_effector)
self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387])
self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos
# Setup gripper - cloth constraint
# NOTE: Mass of cloth_attachment determines the stiffness of the anchors!
sphere_collision_1, sphere_visual_1 = self.world_creation.create_sphere(radius=0.0001, mass=0.1, pos=[0, 0, 0], visual=True, collision=False, rgba=[0, 0, 0, 0], return_collision_visual=True)
sphere_collision_2, sphere_visual_2 = self.world_creation.create_sphere(radius=0.0001, mass=0.1, pos=[0, 0, 0], visual=True, collision=False, rgba=[0, 0, 0, 0], return_collision_visual=True)
end_effector_attachment = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=sphere_collision_1, baseVisualShapeIndex=sphere_visual_1, basePosition=self.start_ee_pos, baseOrientation=[0, 0, 0, 1], linkMasses=[0.1], linkCollisionShapeIndices=[sphere_collision_2], linkVisualShapeIndices=[sphere_visual_2], linkPositions=[[0, 0, 0]], linkOrientations=[[0, 0, 0, 1]], linkInertialFramePositions=[[0, 0, 0]], linkInertialFrameOrientations=[[0, 0, 0, 1]], linkParentIndices=[0], linkJointTypes=[p.JOINT_REVOLUTE], linkJointAxis=[[1, 1, 1]], linkLowerLimits=[0], linkUpperLimits=[0], useMaximalCoordinates=False, physicsClientId=self.id)
# end_effector_attachment = p.loadURDF(os.path.join(self.world_creation.directory, 'clothing', 'ft_sensor.urdf'), physicsClientId=self.id)
self.end_effector_attachment = Agent()
self.end_effector_attachment.init(end_effector_attachment, self.id, self.np_random, indices=-1)
self.cloth_attachment = self.world_creation.create_sphere(radius=0.0001, mass=1, pos=self.start_ee_pos, visual=True, collision=False, rgba=[0, 0, 0, 0], maximal_coordinates=True)
# self.robot.create_constraint(self.robot.left_end_effector, self.end_effector_attachment, self.end_effector_attachment.base, parent_orient=[0, 0, -np.pi/2.0])
self.end_effector_attachment.create_constraint(0, self.cloth_attachment, self.cloth_attachment.base)
self.end_effector_attachment.enable_force_torque_sensor(0)
# self.cloth_attachment = self.world_creation.create_sphere(radius=0.0001, mass=1, pos=self.start_ee_pos, visual=True, collision=True, rgba=[0, 0, 0, 0], maximal_coordinates=True)
# # self.robot.create_constraint(self.robot.left_end_effector, self.cloth_attachment, self.cloth_attachment.base, parent_pos=[0.1, 0, 0], parent_orient=[0, 0, -np.pi/2.0])
# self.robot.create_constraint(self.robot.left_end_effector, self.cloth_attachment, self.cloth_attachment.base)
# self.robot.enable_force_torque_sensor(self.robot.left_end_effector)
# Load cloth
# self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.2, mass=0.16, position=np.array([0.4, -0.42, 0.99]) + self.cloth_offset/1.2, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment.body, anchors=[3831, 3558, 1877, 3708, 1974, 2707, 3201, 637, 2405, 3279, 2708, 2592, 2407, 445, 509, 577], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id)
# p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.05, kDG=10, kDF=0.39, kCHR=2.38, kKHR=2.0, kAHR=2.0, piterations=2, physicsClientId=self.id)
# # Points along the opening of sleeve
# self.triangle1_point_indices = [1814, 1869, 1121]
# self.triangle2_point_indices = [2115, 540, 365]
self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.4, mass=0.16, position=np.array([0.02, -0.38, 0.83]) + self.cloth_offset/1.4, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment.body, anchors=[2087, 3879, 3681, 3682, 2086, 2041, 987, 2042, 2088, 1647, 2332, 719, 1569, 1528, 721], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id)
# p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.005, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id)
p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.01, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id)
# Points along the opening of sleeve
self.triangle1_point_indices = [1180, 2819, 30]
self.triangle2_point_indices = [1322, 13, 696]
# Points along the end of sleeve
# self.triangle1_point_indices = [621, 37, 1008]
# self.triangle2_point_indices = [130, 3908, 2358]
# double m_kLST; // Material: Linear stiffness coefficient [0,1]
# double m_kAST; // Material: Area/Angular stiffness coefficient [0,1]
# double m_kVST; // Material: Volume stiffness coefficient [0,1]
# double m_kVCF; // Velocities correction factor (Baumgarte)
# double m_kDP; // Damping coefficient [0,1]
# double m_kDG; // Drag coefficient [0,+inf]
# double m_kLF; // Lift coefficient [0,+inf]
# double m_kPR; // Pressure coefficient [-inf,+inf]
# double m_kVC; // Volume conversation coefficient [0,+inf]
# double m_kDF; // Dynamic friction coefficient [0,1]
# double m_kMT; // Pose matching coefficient [0,1]
# double m_kCHR; // Rigid contacts hardness [0,1]
# double m_kKHR; // Kinetic contacts hardness [0,1]
# double m_kSHR; // Soft contacts hardness [0,1]
# double m_kAHR; // Anchors hardness [0,1]
# int m_viterations; // Velocities solver iterations
# int m_piterations; // Positions solver iterations
# int m_diterations; // Drift solver iterations
p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently
p.setGravity(0, 0, 0, body=self.robot.body, physicsClientId=self.id)
p.setGravity(0, 0, -1, body=self.human.body, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.cloth_attachment.body, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.end_effector_attachment.body, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=8, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id)
# Wait for the cloth to settle
for _ in range(50):
# Force the end effector attachment to stay at the end effector
self.end_effector_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
# self.cloth_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
p.stepSimulation(physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
return self._get_obs([0]*6, [0, 0])
def reset(self):
super(DressingEnv, self).reset()
self.build_assistive_env('wheelchair_left')
self.cloth_forces = np.zeros((1, 1))
if self.robot.wheelchair_mounted:
wheelchair_pos, wheelchair_orient = self.furniture.get_base_pos_orient()
self.robot.set_base_pos_orient(wheelchair_pos + np.array(self.robot.toc_base_pos_offset[self.task]), [0, 0, np.pi/2.0])
# Update robot and human motor gains
self.robot.motor_gains = self.human.motor_gains = 0.01
joints_positions = [(self.human.j_right_elbow, -90), (self.human.j_left_shoulder_x, -45), (self.human.j_left_elbow, -90), (self.human.j_right_hip_x, -90), (self.human.j_right_knee, 80), (self.human.j_left_hip_x, -90), (self.human.j_left_knee, 80)]
self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=None if self.human.controllable else 1, reactive_gain=0.01)
shoulder_pos = self.human.get_pos_orient(self.human.left_shoulder)[0]
elbow_pos = self.human.get_pos_orient(self.human.left_elbow)[0]
wrist_pos = self.human.get_pos_orient(self.human.left_wrist)[0]
target_ee_pos = np.array([0.45, -0.3, 1]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_ee_orient = self.get_quaternion(self.robot.toc_ee_orient_rpy[self.task][0])
target_ee_orient_shoulder = self.get_quaternion(self.robot.toc_ee_orient_rpy[self.task][-1])
offset = np.array([0, 0, 0.1])
self.init_robot_pose(target_ee_pos, target_ee_orient, [(target_ee_pos, target_ee_orient)], [(shoulder_pos+offset, target_ee_orient_shoulder), (elbow_pos+offset, target_ee_orient), (wrist_pos+offset, target_ee_orient)], arm='left', tools=[], collision_objects=[self.human, self.furniture], right_side=False)
if self.robot.mobile:
# Change robot gains since we use numSubSteps=8
self.robot.gains = list(np.array(self.robot.gains) / 8.0)
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.left_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True)
if self.human.controllable or self.human.impairment == 'tremor':
# Ensure the human arm remains stable while loading the cloth
self.human.control(self.human.controllable_joint_indices, self.human.get_joint_angles(self.human.controllable_joint_indices), 0.05, 1)
self.start_ee_pos, self.start_ee_orient = self.robot.get_pos_orient(self.robot.left_end_effector)
self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387])
self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos
self.cloth_attachment = self.create_sphere(radius=0.0001, mass=0, pos=self.start_ee_pos, visual=True, collision=False, rgba=[0, 0, 0, 0], maximal_coordinates=True)
# Load cloth
self.cloth = p.loadCloth(os.path.join(self.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.4, mass=0.16, position=np.array([0.02, -0.38, 0.84]) + self.cloth_offset/1.4, orientation=self.get_quaternion([0, 0, np.pi]), bodyAnchorId=self.cloth_attachment.body, anchors=[2086, 2087, 2088, 2041], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id)
p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.01, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id)
# Points along the opening of the left arm sleeve
self.triangle1_point_indices = [1180, 2819, 30]
self.triangle2_point_indices = [1322, 13, 696]
# double m_kLST; // Material: Linear stiffness coefficient [0,1]
# double m_kAST; // Material: Area/Angular stiffness coefficient [0,1]
# double m_kVST; // Material: Volume stiffness coefficient [0,1]
# double m_kVCF; // Velocities correction factor (Baumgarte)
# double m_kDP; // Damping coefficient [0,1]
# double m_kDG; // Drag coefficient [0,+inf]
# double m_kLF; // Lift coefficient [0,+inf]
# double m_kPR; // Pressure coefficient [-inf,+inf]
# double m_kVC; // Volume conversation coefficient [0,+inf]
# double m_kDF; // Dynamic friction coefficient [0,1]
# double m_kMT; // Pose matching coefficient [0,1]
# double m_kCHR; // Rigid contacts hardness [0,1]
# double m_kKHR; // Kinetic contacts hardness [0,1]
# double m_kSHR; // Soft contacts hardness [0,1]
# double m_kAHR; // Anchors hardness [0,1]
# int m_viterations; // Velocities solver iterations
# int m_piterations; // Positions solver iterations
# int m_diterations; // Drift solver iterations
p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently
if not self.robot.mobile:
self.robot.set_gravity(0, 0, 0)
self.human.set_gravity(0, 0, -1)
self.cloth_attachment.set_gravity(0, 0, 0)
p.setPhysicsEngineParameter(numSubSteps=8, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id)
# Wait for the cloth to settle
for _ in range(50):
# Force the end effector attachment to stay at the end effector
self.cloth_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
p.stepSimulation(physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
self.init_env_variables()
return self._get_obs()
