def collect_observations(self, human):
#print("ordered_joint_indices")
#print(ordered_joint_indices)
jointStates = p.getJointStates(human,self.ordered_joint_indices)
j = np.array([self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints]).flatten()
#print("j")
#print(j)
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)
#print("body_xyz")
#print(body_xyz, qx,qy,qz,qw)
z = body_xyz[2]
self.distance = body_xyz[0]
if self.initial_z==None:
self.initial_z = z
(vx, vy, vz), _ = p.getBaseVelocity(human)
more = np.array([z-self.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw])
rcont = p.getContactPoints(human, -1, self.right_foot, -1)
#print("rcont")
#print(rcont)
lcont = p.getContactPoints(human, -1, self.left_foot, -1)
#print("lcont")
#print(lcont)
feet_contact = np.array([len(rcont)>0, len(lcont)>0])
return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def closeGripper(gripperID, targetID):
contact = p.getContactPoints(gripperID, targetID)
fourPos = p.getJointState(gripperID, 4)[0]
sixPos = p.getJointState(gripperID, 6)[0]
targetVelocity = 0.02
while not contact:
p.setJointMotorControl2(gripperID,
4,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity)
p.setJointMotorControl2(gripperID,
6,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity)
stepSim(50)
contact = p.getContactPoints(gripperID, targetID)
p.setJointMotorControl2(gripperID, 4, p.VELOCITY_CONTROL, targetVelocity=0)
p.setJointMotorControl2(gripperID, 6, p.VELOCITY_CONTROL, targetVelocity=0)
fourPos = p.getJointState(gripperID, 4)[0]
sixPos = p.getJointState(gripperID, 6)[0]
pos = max(fourPos, sixPos)
p.setJointMotorControl2(gripperID,
4,
p.POSITION_CONTROL,
targetPosition=pos + 0.0125,
force=100)
p.setJointMotorControl2(gripperID,
6,
p.POSITION_CONTROL,
targetPosition=pos + 0.0125,
force=100)
def _getReward(self):
target = self.target
objectPosition = self.objectPosition
tip = self.robot.joints[self.fingerTipName].getWorldPosition()
tip_target = [0, 0, 0]
norm = np.sqrt((np.power(target[0] - objectPosition[0], 2)) +
(np.power(target[1] - objectPosition[1], 2)))
tip_target[0] = objectPosition[0] - (
(target[0] - objectPosition[0]) / norm) * (self.r)
tip_target[1] = objectPosition[1] - (
(target[1] - objectPosition[1]) / norm) * (self.r)
tip_target[2] = objectPosition[2]
tip_target2 = [0, 0, 0]
tip_target2[0] = objectPosition[0] - (
(target[0] - objectPosition[0]) / norm) * (self.r)
tip_target2[1] = objectPosition[1] - (
(target[1] - objectPosition[1]) / norm) * (self.r)
tip_target2[2] = objectPosition[2]
tip_distance = math.sqrt(
sum((x - y)**2 for x, y in zip(tip, tip_target)))
object_distance = math.sqrt(
sum((x - y)**2 for x, y in itertools.islice(
zip(self.objectPosition, target), 2)))
table_contact = pb.getContactPoints(
self.robot.joints[self.fingerTipName].bodyIndex, self.table.id)
cp = pb.getContactPoints(
self.robot.joints[self.fingerTipName].bodyIndex, self.box.objId)
contact = False
reward = 0
sparse_reward = 0
if cp:
contact = True
if self.reward_type == 'sparse':
reward = int(object_distance <= 0.05)
elif self.reward_type == 'shaped':
reward = 0
cp = pb.getContactPoints(
self.robot.joints[self.fingerTipName].bodyIndex,
self.box.objId)
reward = -tip_distance / 3 + -object_distance
contact = False
if cp:
reward = -object_distance
contact = True
if object_distance < 0.05:
reward = 1
sparse_reward = 1
if self.reward_type == "shaped":
return reward
elif self.reward_type == "sparse":
return sparse_reward
def get_force(self):
""" Returns the force readings as well as the set of active contacts """
active_contacts_frame_ids = []
contact_forces = []
for i in self.bullet_endeff_ids:
cpA = p.getContactPoints(bodyA=self.robot_id, linkIndexA=i)
cpB = p.getContactPoints(bodyB=self.robot_id, linkIndexB=i)
cp = cpA + cpB
if len(cp) > 0:
for ci in cp:
contact_normal = ci[7]
normal_force = ci[9]
lateral_friction_direction_1 = ci[11]
lateral_friction_force_1 = ci[10]
lateral_friction_direction_2 = ci[13]
lateral_friction_force_2 = ci[12]
j = np.where(np.array(self.bullet_endeff_ids) == i)[0][0]
active_contacts_frame_ids.append(
self.pinocchio_endeff_ids[j])
force = np.zeros(6)
force[:3] = normal_force * np.array(contact_normal) + \
lateral_friction_force_1 * np.array(lateral_friction_direction_1) + \
lateral_friction_force_2 * np.array(lateral_friction_direction_2)
contact_forces.append(force)
return active_contacts_frame_ids, contact_forces
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,
self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter > self._maxSteps):
self._observation = self.getExtendedObservation()
return True
maxDist = 0.008
# closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid, maxDist)
closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid,
maxDist, -1, 13)
contactPoints = p.getContactPoints(self.blockUid, self._kuka.kukaUid)
contactPoints_tray = p.getContactPoints(self._kuka.trayUid,
self._kuka.kukaUid)
if ((len(contactPoints))
and contactPoints[0][8] < 0) or len(closestPoints):
self.terminated = 1
print('********** Terminating Normally...')
self._observation = self.getExtendedObservation()
return True
#elif (len(contactPoints_tray) and contactPoints_tray[0][8]<0):
#self._observation = self.getExtendedObservation()
# print('********* Terminating by contacting the tray!!!')
# return True
return False
def get_handcraft_reward(self):
reward = 0
# check whether the seqence number is out of limit
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
# check whether the object are out of order
for axis_dim in range(3):
if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \
self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]:
done = True
reward = self.opt.out_reward
# check whether the object is still in the gripper
left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13,
-1)
right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17,
-1)
if self.opt.obj_away_loss:
if len(left_closet_info) == 0 and len(right_closet_info) == 0:
done = True
# let the model learn the reward automatically, so delete the following line
reward = self.opt.away_reward
self.info += 'reward: {}\n\n'.format(reward)
self.log_info.write(self.info)
print(self.info)
return self.observation, reward, done, self.info
def get_total_force(self):
tool_left_force = 0
tool_right_force = 0
total_force_on_human = 0
tool_left_force_on_human = 0
tool_right_force_on_human = 0
for c in p.getContactPoints(bodyA=self.robot, physicsClientId=self.id):
linkA = c[3]
if linkA == (55
if self.robot_type == 'pr2' else 24 if self.robot_type
== 'sawyer' else 31 if self.robot_type == 'baxter'
else 9 if self.robot_type == 'jaco' else 7):
tool_right_force += c[9]
elif linkA == (78 if self.robot_type == 'pr2' else
24 if self.robot_type == 'sawyer' else
54 if self.robot_type == 'baxter' else
9 if self.robot_type == 'jaco' else 7):
tool_left_force += c[9]
for c in p.getContactPoints(bodyA=self.robot,
bodyB=self.human,
physicsClientId=self.id):
total_force_on_human += c[9]
linkA = c[3]
linkB = c[4]
if linkA == (55
if self.robot_type == 'pr2' else 24 if self.robot_type
== 'sawyer' else 31 if self.robot_type == 'baxter'
else 9 if self.robot_type == 'jaco' else 7):
tool_right_force_on_human += c[9]
elif linkA == (78 if self.robot_type == 'pr2' else
24 if self.robot_type == 'sawyer' else
54 if self.robot_type == 'baxter' else
9 if self.robot_type == 'jaco' else 7):
tool_left_force_on_human += c[9]
return tool_left_force, tool_right_force, total_force_on_human, tool_left_force_on_human, tool_right_force_on_human
def get_food_rewards(self):
# Check all food particles to see if they have left the spoon or entered the person's mouth
# Give the robot a reward or penalty depending on food particle status
food_reward = 0
food_hit_human_reward = 0
food_mouth_velocities = []
foods_to_remove = []
for f in self.foods:
food_pos, food_orient = p.getBasePositionAndOrientation(f, physicsClientId=self.id)
distance_to_mouth = np.linalg.norm(self.target_pos - food_pos)
if distance_to_mouth < 0.02:
# Delete particle and give robot a reward
food_reward += 20
self.task_success += 1
food_velocity = np.linalg.norm(p.getBaseVelocity(f, physicsClientId=self.id)[0])
food_mouth_velocities.append(food_velocity)
foods_to_remove.append(f)
p.resetBasePositionAndOrientation(f, self.np_random.uniform(1000, 2000, size=3), [0, 0, 0, 1], physicsClientId=self.id)
continue
elif food_pos[-1] < 0.5 or len(p.getContactPoints(bodyA=f, bodyB=self.table, physicsClientId=self.id)) > 0 or len(p.getContactPoints(bodyA=f, bodyB=self.bowl, physicsClientId=self.id)) > 0:
# Delete particle and give robot a penalty for spilling food
food_reward -= 5
foods_to_remove.append(f)
continue
if len(p.getContactPoints(bodyA=f, bodyB=self.human, physicsClientId=self.id)) > 0 and f not in self.foods_hit_person:
# Record that this food particle just hit the person, so that we can penalize the robot
self.foods_hit_person.append(f)
food_hit_human_reward -= 1
self.foods = [f for f in self.foods if f not in foods_to_remove]
return food_reward, food_mouth_velocities, food_hit_human_reward
def get_foot_pressure_sensors(self, floor):
"""
Checks if 4 corners of the each feet are in contact with ground
Indicies for looking from above on the feet plates:
Left Right
4-------5 0-------1
| ^ | | ^ | ^
| | | | | | | : forward direction
| | | |
6-------7 2-------3
:param floor: PyBullet body id of the plane the robot is walking on.
:return: boolean array of 8 contact points on both feet, True: that point is touching the ground False: otherwise
"""
locations = [False] * 8
right_pts = pb.getContactPoints(bodyA=self.body, bodyB=floor, linkIndexA=Links.RIGHT_LEG_6)
left_pts = pb.getContactPoints(bodyA=self.body, bodyB=floor, linkIndexA=Links.LEFT_LEG_6)
right_center = np.array(pb.getLinkState(self.body, linkIndex=Links.RIGHT_LEG_6)[4])
left_center = np.array(pb.getLinkState(self.body, linkIndex=Links.LEFT_LEG_6)[4])
right_tr = tr.get_rotation_matrix_from_transformation(
tr(quaternion=pb.getLinkState(self.body, linkIndex=Links.RIGHT_LEG_6)[5]))
left_tr = tr.get_rotation_matrix_from_transformation(
tr(quaternion=pb.getLinkState(self.body, linkIndex=Links.LEFT_LEG_6)[5]))
for point in right_pts:
index = np.signbit(np.matmul(right_tr, point[5] - right_center))[0:2]
locations[index[1] + index[0] * 2] = True
for point in left_pts:
index = np.signbit(np.matmul(left_tr, point[5] - left_center))[0:2]
locations[index[1] + (index[0] * 2) + 4] = True
return locations
def get_video_reward(self):
if ((self.seq_num-1)%self.opt.give_reward_num==self.opt.give_reward_num-1) \
and self.seq_num>=self.opt.cut_frame_num:
if self.opt.use_cycle:
self.cycle.image_transfer(self.epoch_num)
self.eval.update(img_path=self.log_path,
start_id=self.seq_num - 1 -
self.opt.cut_frame_num)
# self.eval.get_caption()
rank, probability = self.eval.eval(self.img_buffer)
reward = probability
self.info += 'rank: {}\n'.format(rank)
else:
reward = 0
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
# check whether the object is still in the gripper
left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13,
-1)
right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17,
-1)
if self.opt.obj_away_loss:
if len(left_closet_info) == 0 and len(right_closet_info) == 0:
done = True
self.info += 'reward: {}\n\n'.format(reward)
# self.log_info.write (self.info)
# print (self.info)
return self.observation, reward, done, self.info
def collect_observations(self, human):
#print("ordered_joint_indices")
#print(ordered_joint_indices)
jointStates = p.getJointStates(human, self.ordered_joint_indices)
j = np.array([
self.current_relative_position(jointStates, human, *jtuple)
for jtuple in self.ordered_joints
]).flatten()
#print("j")
#print(j)
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)
#print("body_xyz")
#print(body_xyz, qx,qy,qz,qw)
z = body_xyz[2]
self.distance = body_xyz[0]
if self.initial_z == None:
self.initial_z = z
(vx, vy, vz), _ = p.getBaseVelocity(human)
more = np.array(
[z - self.initial_z, 0.1 * vx, 0.1 * vy, 0.1 * vz, qx, qy, qz, qw])
rcont = p.getContactPoints(human, -1, self.right_foot, -1)
#print("rcont")
#print(rcont)
lcont = p.getContactPoints(human, -1, self.left_foot, -1)
#print("lcont")
#print(lcont)
feet_contact = np.array([len(rcont) > 0, len(lcont) > 0])
return np.clip(np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def findContact(self):
self.contactBetweenBallAndGround = p.getContactPoints(
bodyA=self.ballId, bodyB=self.planeId)
self.contactBetweenBallAndRobot = p.getContactPoints(
bodyA=self.ballId, bodyB=self.robotId)
if self.contactBetweenBallAndRobot:
self.catchFlag = True
def get_food_rewards(self):
food_reward = 0
foods_to_remove = []
sum_dis = 0
for f in self.foods:
food_pos, food_orient = p.getBasePositionAndOrientation(
f, physicsClientId=self.id)
distance_to_mouth = np.linalg.norm(self.target_pos - food_pos)
sum_dis += distance_to_mouth
if distance_to_mouth < 0.03:
# Delete particle and give robot a reward
food_reward += 20
self.task_success += 1
foods_to_remove.append(f)
p.resetBasePositionAndOrientation(
f,
self.np_random.uniform(1000, 2000, size=3), [0, 0, 0, 1],
physicsClientId=self.id) #重置foodposition,远去
continue
elif food_pos[-1] < 0.5 or len(
p.getContactPoints(
bodyA=f, bodyB=self.table,
physicsClientId=self.id)) > 0 or len(
p.getContactPoints(bodyA=f,
bodyB=self.bowl,
physicsClientId=self.id)) > 0:
# Delete particle and give robot a penalty for spilling food
food_reward -= 5
self.fall_time += 1
foods_to_remove.append(f)
mean_dis = sum_dis / len(self.foods)
self.foods = [f for f in self.foods if f not in foods_to_remove]
return food_reward, mean_dis
def _reward(self):
#rewards is height of target object
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid, self._kuka.kukaUid,
1000, -1, 13)
"""
EndEffector distance is: 0.39769955968108894
Gripper distance is: 0.36823755806508035
"""
reward = -1000
# reward = -500
numPt = len(closestPoints)
contactPoints = p.getContactPoints(self.blockUid, self._kuka.kukaUid)
contactPoints_tray = p.getContactPoints(self._kuka.trayUid,
self._kuka.kukaUid)
#print(numPt)
if (numPt > 0):
reward = -closestPoints[0][8] * 10
if closestPoints[0][8] < 0.008 or (len(contactPoints)
and contactPoints[0][8] < 0):
reward += 1000
print(
'******Less than 0.008! OR contacted the object, adding 1000********'
)
#elif (len(contactPoints_tray) and contactPoints_tray[0][8]<0):
# print('Because of contacting the tray, the reward will be reduced by 1000!')
#reward=reward-1000
return reward
def _feet(self):
"""
Checks if 4 corners of the each feet are in contact with ground
Indicies for looking from above on the feet plates:
Left Right
4-------5 0-------1
| ^ | | ^ | ^
| | | | | | | : forward direction
| | | |
6-------7 2-------3
:return: int array of 8 contact points on both feet, 1: that point is touching the ground -1: otherwise
"""
locations = [-1.] * self._FEET_DIM
right_pts = pb.getContactPoints(bodyA=self.soccerbotUid, bodyB=self.planeUid, linkIndexA=Links.RIGHT_LEG_6)
left_pts = pb.getContactPoints(bodyA=self.soccerbotUid, bodyB=self.planeUid, linkIndexA=Links.LEFT_LEG_6)
right_center = np.array(pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6)[4])
left_center = np.array(pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.LEFT_LEG_6)[4])
right_tr = np.array(pb.getMatrixFromQuaternion(
pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.RIGHT_LEG_6)[5])
, dtype=self.DTYPE).reshape((3, 3))
left_tr = np.array(pb.getMatrixFromQuaternion(
pb.getLinkState(bodyUniqueId=self.soccerbotUid, linkIndex=Links.LEFT_LEG_6)[5])
, dtype=self.DTYPE).reshape((3, 3))
for point in right_pts:
index = np.signbit(np.matmul(right_tr, point[5] - right_center))[0:2]
locations[index[1] + index[0] * 2] = 1.
for point in left_pts:
index = np.signbit(np.matmul(left_tr, point[5] - left_center))[0:2]
locations[index[1] + (index[0] * 2) + 4] = 1.
for i in range(len(locations)): # 5% chance of incorrect reading
locations[i] *= np.sign(self.np_random.uniform(low=- self._FEET_FALSE_CHANCE,
high=1 - (self._FEET_FALSE_CHANCE)), dtype=self.DTYPE)
return np.array(locations)
def get_total_force(self):
robot_force_on_human = 0
cup_force_on_human = 0
for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id):
robot_force_on_human += c[9]
for c in p.getContactPoints(bodyA=self.cup, bodyB=self.human, physicsClientId=self.id):
cup_force_on_human += c[9]
return robot_force_on_human, cup_force_on_human
def get_obs(self):
'''
Returns a suite of observations of the given state of the robot. Not all of these are required.
:return: list of lists
'''
# Torso
torso_pos, torso_quat = p.getBasePositionAndOrientation(
self.robot,
physicsClientId=self.client_ID) # xyz and quat: x,y,z,w
torso_vel, torso_angular_vel = p.getBaseVelocity(
self.robot, physicsClientId=self.client_ID)
#distance of each foot to target position
""" l_vec_target = p.getLinkState(self.robot, self.left_foot)[0] - self.l_foot_target
ctct_l = np.linalg.norm(l_vec_target)
r_vec_target = p.getLinkState(self.robot, self.right_foot)[0] - self.r_foot_target
ctct_r = np.linalg.norm(r_vec_target)
contacts = [ctct_l, ctct_r] """
# Joints
obs = p.getJointStates(self.robot,
self.joints_index,
physicsClientId=self.client_ID
) # pos, vel, reaction(6), prev_torque
joint_angles = []
joint_velocities = []
joint_torques = []
for o in obs:
joint_angles.append(o[0])
joint_velocities.append(o[1])
joint_torques.append(o[3])
# add positions of links to observation instead of contacts>
# [l[4] for l in p.getLinkStates(self.robot, range(0, 13+1), physicsClientId=self.client_ID)]
# link positions and contacts
""" obs = p.getLinkStates(self.robot, range(p.getNumJoints(self.robot)), physicsClientId=self.client_ID)
link_pos_orient = []
for o in obs:
link_pos_orient.append(o[1]) """
a = p.getNumJoints(self.robot)
# 0/1 contact of each link to car and floor
contacts = [0] * (a * 2)
obs = p.getContactPoints(self.robot,
self.car,
physicsClientId=self.client_ID)
for o in obs:
contacts[o[
3]] = 1 # for each contact point, store 1 at index of participating robot's link
obs = p.getContactPoints(self.robot,
self.plane,
physicsClientId=self.client_ID)
for o in obs:
contacts[a + o[
3]] = 1 # for each contact point, store 1 at index of participating robot's link
return torso_pos, torso_quat, torso_vel, torso_angular_vel, joint_angles, joint_velocities, joint_torques, contacts
def get_handcraft_reward (self):
distance = sum ([(x - y) ** 2 for x, y in zip (self.start_pos, self.target_pos)]) ** 0.5
box = p.getAABB (self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip (box[0], box[1])]
obj = p.getAABB (self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip (obj[0], obj[1])]
aabb_dist = sum ([(x - y) ** 2 for x, y in zip (box_center, obj_center)]) ** 0.5
if self.opt.video_reward:
if ((self.seq_num-1)%self.opt.give_reward_num==self.opt.give_reward_num-1) \
and self.seq_num>=self.opt.cut_frame_num:
self.eval.get_caption()
rank,probability = self.eval.eval()
reward = probability
self.info += 'rank: {}\n'.format(rank)
self.eval.update(img_path=self.log_path,start_id=self.seq_num-1-self.opt.cut_frame_num)
else:
reward = 0
else:
if self.opt.reward_diff:
reward = (self.last_aabb_dist - aabb_dist) * 100
else:
reward = (0.5-aabb_dist)*100
# reward = (self.last_aabb_dist-aabb_dist)*100
self.last_aabb_dist = aabb_dist
# calculate whether it is done
self.info += 'now distance:{}\n'.format (distance)
self.info += 'AABB distance:{}\n'.format (aabb_dist)
# check whether the seqence number is out of limit
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
# check whether the object are out of order
for axis_dim in range (3):
if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \
self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]:
done = True
reward = self.opt.out_reward
# check whether the object is still in the gripper
left_closet_info = p.getContactPoints (self.robotId, self.obj_id, 13, -1)
right_closet_info = p.getContactPoints (self.robotId, self.obj_id, 17, -1)
if self.opt.obj_away_loss:
if len (left_closet_info)==0 and len (right_closet_info)==0:
done = True
# let the model learn the reward automatically, so delete the following line
reward = self.opt.away_reward
self.info += 'reward: {}\n\n'.format (reward)
self.log_info.write (self.info)
print (self.info)
return self.observation, reward, done, self.info
def display_contact_forces(self):
# Info about contact points with the ground
contactPoints_FL = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=3) # Front left foot
contactPoints_FR = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=7) # Front right foot
contactPoints_HL = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=11) # Hind left foot
contactPoints_HR = pyb.getContactPoints(self.robotId, self.planeId, linkIndexA=15) # Hind right foot
# print(len(contactPoint_FL), len(contactPoint_FR), len(contactPoint_HL), len(contactPoint_HR))
# Sort contacts points to get only one contact per foot
contactPoints = []
contactPoints.append(self.getContactPoint(contactPoints_FL))
contactPoints.append(self.getContactPoint(contactPoints_FR))
contactPoints.append(self.getContactPoint(contactPoints_HL))
contactPoints.append(self.getContactPoint(contactPoints_HR))
# Display debug lines for contact forces visualization
i_line = 0
# print(len(self.lines))
f_x = 0
f_y = 0
f_z = 0
f_tmp = [0.0] * 3
for contact in contactPoints:
if not isinstance(contact, int): # type(contact) != type(0):
start = [contact[6][0], contact[6][1], contact[6][2]+0.04]
end = [contact[6][0], contact[6][1], contact[6][2]+0.04]
K = 0.02
for i_direction in range(0, 3):
f_tmp[i_direction] = (contact[9] * contact[7][i_direction] + contact[10] *
contact[11][i_direction] + contact[12] * contact[13][i_direction])
end[i_direction] += K * f_tmp[i_direction]
"""if contact[3] < 10:
print("Link ", contact[3], "| Contact force: (", f_tmp[0], ", ", f_tmp[1], ", ", f_tmp[2], ")")
else:
print("Link ", contact[3], "| Contact force: (", f_tmp[0], ", ", f_tmp[1], ", ", f_tmp[2], ")")"""
f_x += f_tmp[0]
f_y += f_tmp[1]
f_z += f_tmp[2]
if (i_line+1) > len(self.lines): # If not enough existing lines in line storage a new item is created
lineID = pyb.addUserDebugLine(start, end, lineColorRGB=[1.0, 0.0, 0.0], lineWidth=8)
self.lines.append(lineID)
else: # If there is already an existing line item we modify it (to avoid flickering)
self.lines[i_line] = pyb.addUserDebugLine(start, end, lineColorRGB=[
1.0, 0.0, 0.0], lineWidth=8, replaceItemUniqueId=self.lines[i_line])
i_line += 1
# Should be around 21,5 (2.2 kg * 9.81 m^2/s)
# print("Total ground reaction force: (", f_x, ", ", f_y, ", ", f_z, ")")
for i_zero in range(i_line, len(self.lines)):
self.lines[i_zero] = pyb.addUserDebugLine([0.0, 0.0, 0.0], [0.0, 0.0, 0.0], lineColorRGB=[
1.0, 0.0, 0.0], lineWidth=8, replaceItemUniqueId=self.lines[i_zero])
def get_collision(self, bodyB):
outA = p.getContactPoints(bodyA=self.robot.id,
bodyB=bodyB,
linkIndexA=14,
linkIndexB=0)
outB = p.getContactPoints(bodyA=self.robot.id,
bodyB=bodyB,
linkIndexA=13,
linkIndexB=0)
return len(outA) + len(outB)
def step(self, action):
sigmoid = lambda x: 1 / (1 + np.exp(-np.clip(x, -709, 709)))
action[0:2] = np.tanh(action[0:2])
action[2] = sigmoid(action[2])
action = [action[mm] * t for mm, t in enumerate(self.max_thrust)]
self.apply_thrust(action[2])
self.apply_control_thrust(action[0], action[1])
p.stepSimulation()
obs = self.get_obs()
reward, info = 0.0, None
done = False
# check for collision nose cone on ground plane
nose_contact_points= p.getContactPoints(self.bot_id, self.plane_id, 1)
landing_gear_contacts = []
for link_idx in range(3,7):
landing_gear_contacts.append(p.getContactPoints(self.bot_id, self.plane_id, link_idx))
landed = True in [len(elem) > 0 for elem in landing_gear_contacts]
if len(nose_contact_points) > 0:
#print("nose ground collsion")
done = True
reward -= self.crash_bonus
# nose cone is on the ground
elif landed and np.abs(np.mean(obs[-7:-4])) < 1e-4:
#print("bell is down")
done = True
reward += self.land_bonus
self.step_count += 1
if self.step_count > self.max_steps:
done = True
# lost in space
#print("lost in space")
reward -= self.timeout_bonus
if done and len(nose_contact_points) == 0:
reward += self.fuel
elif not done:
# survival bonus
reward += self.survival_bonus
#if done: print(self.step_count)
return obs, reward, done, info
def step(self, action, custom_reward=None):
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
p.setJointMotorControlArray(
self.yumiUid, [self.joint2Index[joint] for joint in self.joints],
p.POSITION_CONTROL, action)
p.stepSimulation()
# get joint states
jointStates = {}
for joint in self.joints:
jointStates[joint] = p.getJointState(
self.yumiUid, self.joint2Index[joint]) + p.getLinkState(
self.yumiUid, self.joint2Index[joint])
# recover color
for joint, index in self.joint2Index.items():
if joint in self.jointColor and joint != 'world_joint':
p.changeVisualShape(self.yumiUid,
index,
rgbaColor=self.jointColor[joint])
# check collision
collision = False
for joint in self.joints:
if len(
p.getContactPoints(
bodyA=self.yumiUid,
linkIndexA=self.joint2Index[joint])) > 0:
collision = True
for contact in p.getContactPoints(
bodyA=self.yumiUid,
linkIndexA=self.joint2Index[joint]):
print(
"Collision Occurred in Joint {} & Joint {}!!!".format(
contact[3], contact[4]))
p.changeVisualShape(self.yumiUid,
contact[3],
rgbaColor=[1, 0, 0, 1])
p.changeVisualShape(self.yumiUid,
contact[4],
rgbaColor=[1, 0, 0, 1])
self.step_counter += 1
if custom_reward is None:
# default reward
reward = 0
done = False
else:
# custom reward
reward, done = custom_reward(jointStates=jointStates,
collision=collision,
step_counter=self.step_counter)
info = {'collision': collision}
observation = [jointStates[joint][0] for joint in self.joints]
return observation, reward, done, info
def check_grasped_id(self):
left_index = self.joints['left_inner_finger_pad_joint'].id
right_index = self.joints['right_inner_finger_pad_joint'].id
contact_left = p.getContactPoints(bodyA=self.robotID, linkIndexA=left_index)
contact_right = p.getContactPoints(bodyA=self.robotID, linkIndexA=right_index)
contact_ids = set(item[2] for item in contact_left + contact_right if item[2] in self.obj_ids)
if len(contact_ids) > 1:
print('Warning: Multiple items in hand!')
if len(contact_ids) == 0:
print(contact_left, contact_right)
return list(item_id for item_id in contact_ids if item_id in self.obj_ids)
def get_handcraft_reward(self):
distance = sum([(x - y)**2
for x, y in zip(self.start_pos, self.target_pos)])**0.5
box = p.getAABB(self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
obj = p.getAABB(self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])]
aabb_dist = sum([(x - y)**2
for x, y in zip(box_center, obj_center)])**0.5
if self.opt.reward_diff:
reward = (self.last_aabb_dist - aabb_dist) * 100
else:
reward = (0.5 - aabb_dist) * 100
# calculate whether it is done
self.info += 'now distance:{}\n'.format(distance)
self.info += 'AABB distance:{}\n'.format(aabb_dist)
if self.seq_num >= self.max_seq_num:
done = True
else:
done = False
# check whether the object are out of order
for axis_dim in range(3):
if self.start_pos[axis_dim] < self.axis_limit[axis_dim][0] or \
self.start_pos[axis_dim] > self.axis_limit[axis_dim][1]:
done = True
reward = self.opt.out_reward
# check whether the object is still in the gripper
left_closet_info = p.getContactPoints(self.robotId, self.obj_id, 13,
-1)
right_closet_info = p.getContactPoints(self.robotId, self.obj_id, 17,
-1)
if self.opt.obj_away_loss:
if len(left_closet_info) == 0 and len(right_closet_info) == 0:
done = True
# let the model learn the reward automatically, so delete the following line
# reward = self.opt.away_reward
# if aabb_dist<self.opt.end_distance and abs(max(box[0][2],box[1][2])-min(obj[0][2],obj[1][2]))<0.05:
if aabb_dist < self.opt.end_distance:
done = True
if (not self.opt.video_reward):
reward = 100
# reward = -1
self.info += 'reward: {}\n\n'.format(reward)
self.log_info.write(self.info)
print(self.info)
return self.observation, reward, done, self.info
def get_obs(self):
# Torso
torso_pos, torso_quat = p.getBasePositionAndOrientation(
self.robot,
physicsClientId=self.client_ID) # xyz and quat: x,y,z,w
torso_vel, torso_angular_vel = p.getBaseVelocity(
self.robot, physicsClientId=self.client_ID)
ctct_leg_1 = int(
len(
p.getContactPoints(self.robot,
self.plane,
2,
-1,
physicsClientId=self.client_ID)) > 0) * 2 - 1
ctct_leg_2 = int(
len(
p.getContactPoints(self.robot,
self.plane,
5,
-1,
physicsClientId=self.client_ID)) > 0) * 2 - 1
ctct_leg_3 = int(
len(
p.getContactPoints(self.robot,
self.plane,
8,
-1,
physicsClientId=self.client_ID)) > 0) * 2 - 1
ctct_leg_4 = int(
len(
p.getContactPoints(self.robot,
self.plane,
11,
-1,
physicsClientId=self.client_ID)) > 0) * 2 - 1
contacts = [ctct_leg_1, ctct_leg_2, ctct_leg_3, ctct_leg_4]
# Joints
obs = p.getJointStates(self.robot,
range(12),
physicsClientId=self.client_ID
) # pos, vel, reaction(6), prev_torque
joint_angles = []
joint_velocities = []
joint_torques = []
for o in obs:
joint_angles.append(o[0])
joint_velocities.append(o[1])
joint_torques.append(o[3])
return torso_pos, torso_quat, torso_vel, torso_angular_vel, joint_angles, joint_velocities, joint_torques, contacts
def check_grasped(self):
left_index = self.joints['left_inner_finger_pad_joint'].id
right_index = self.joints['right_inner_finger_pad_joint'].id
contact_left = p.getContactPoints(bodyA=self.robot_id,
linkIndexA=left_index)
contact_right = p.getContactPoints(bodyA=self.robot_id,
linkIndexA=right_index)
contact_ids = set(item[2] for item in contact_left + contact_right
if item[2] in [self.obj_id])
if len(contact_ids) == 1:
return True
return False
def has_collision(self):
"""Check if collision happens which involves objects"""
for object_id in self.object_ids:
if len(p.getContactPoints(object_id)) > 1:
return True
elif len(p.getContactPoints(object_id)) == 1:
contact_point = p.getContactPoints(object_id)[0]
contact_normal = contact_point[7]
if abs(contact_normal[0]) > .1 or abs(contact_normal[1]) > .1:
return True
loc, quat = p.getBasePositionAndOrientation(object_id)
if -4 < loc[0] < -2.8:
return True
return False
def _get_foot_contacts(self):
LF = 0 if p.getContactPoints(
self.quadruped_id, self.plane_id,
linkIndexA=self.feet_ids['LF']) == () else 1
RF = 0 if p.getContactPoints(
self.quadruped_id, self.plane_id,
linkIndexA=self.feet_ids['RF']) == () else 1
LH = 0 if p.getContactPoints(
self.quadruped_id, self.plane_id,
linkIndexA=self.feet_ids['LH']) == () else 1
RH = 0 if p.getContactPoints(
self.quadruped_id, self.plane_id,
linkIndexA=self.feet_ids['RH']) == () else 1
return LF, RF, LH, RH
def get_hits(self, robot1=0, robot2=1, links=None):
if robot1 is None:
hits = p.getContactPoints()
elif robot2 is None:
hits = p.getContactPoints(robot1)
# links=(14,14) for sword+shield
elif links is None:
hits = p.getContactPoints(robot1, robot2)
else:
assert len(links) == 2
hits = p.getContactPoints(robot1, robot2, links[0], links[1])
return hits
def sim_run(self, u_l, u_r):
base_or_p = np.array(p.getBasePositionAndOrientation(bot)[1])
# pybullet gives quaternions in xyzw format
# transforms3d takes quaternions in wxyz format, so you need to shift values
b_orient = np.zeros(4)
b_orient[0] = base_or_p[3] # w
b_orient[1] = base_or_p[0] # x
b_orient[2] = base_or_p[1] # y
b_orient[3] = base_or_p[2] # z
b_orient = transforms3d.quaternions.quat2mat(b_orient)
torque = np.zeros(8)
torque[0:4] = u_l
torque[0] *= -1 # readjust to match motor polarity
torque[4:8] = -u_r
torque[7] *= -1 # readjust to match motor polarity
# print(self.reaction_torques()[0:4])
p.setJointMotorControlArray(bot,
jointArray,
p.TORQUE_CONTROL,
forces=torque)
velocities = p.getBaseVelocity(bot)
self.v = velocities[
0] # base linear velocity in global Cartesian coordinates
self.omega_xyz = velocities[1] # base angular velocity in Euler XYZ
# print(omega_xyz[2], omega_xyz[1], omega_xyz[0])
# base angular velocity in quaternions
# self.omega = transforms3d.euler.euler2quat(omega_xyz[0], omega_xyz[1], omega_xyz[2], axes='rxyz')
# found to be intrinsic Euler angles (r)
# Pull values in from simulator, select relevant ones, reshape to 2D array
q = np.reshape([j[0] for j in p.getJointStates(1, range(0, 8))],
(-1, 1))
# Detect contact of feet with ground plane
c1 = bool(len([c[8] for c in p.getContactPoints(bot, plane, 3)]))
c2 = bool(len([c[8] for c in p.getContactPoints(bot, plane, 7)]))
# dq = [j[1] for j in p.getJointStates(bot, range(8))]
if record_stepped is True:
record_stepped(self.out)
if useRealTime == 0:
p.stepSimulation()
return q, b_orient, c1, c2
def __fingerReach(self, gripperId, objectId, finger1LinkId, finger2LinkId,
indentationDepth):
contactFinger1 = pybullet.getContactPoints(bodyA=gripperId,
bodyB=objectId,
linkIndexA=finger1LinkId)
contactFinger2 = pybullet.getContactPoints(bodyA=gripperId,
bodyB=objectId,
linkIndexA=finger2LinkId)
if len(contactFinger1) < 1 or len(contactFinger2) < 1:
return False
contactList = contactFinger1 + contactFinger2
for contact in contactList:
if contact[8] < indentationDepth:
return True
return False
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
#print("joints=",joints)
for j in range (12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped,-1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1./500.)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
js = p.getJointState(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,(js[0]-jointOffsets[j])/jointDirections[j]))
import pybullet as p
p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
huskypos = [0,0,0.1]
husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2])
numJoints = p.getNumJoints(husky)
for joint in range (numJoints) :
print (p.getJointInfo(husky,joint))
targetVel = 10 #rad/s
maxForce = 100 #Newton
for joint in range (2,6) :
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
for step in range (300):
p.stepSimulation()
targetVel=-10
for joint in range (2,6) :
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
for step in range (400):
p.stepSimulation()
p.getContactPoints(husky)
p.disconnect()
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()
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()
#################################
setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
def contact_list(self): return p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1)
useMaximalCoordinates = True
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
#if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*5*sphereRadius,j*5*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
#else:
# sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.changeDynamics(sphereUid,-1,ccdSweptSphereRadius=0.002)
p.setGravity(0,0,-10)
pts = p.getContactPoints()
print("num points=",len(pts))
print(pts)
while (p.isConnected()):
time.sleep(1./240.)
p.stepSimulation()
