def robot_collision(self, robot, mesh_body_id, pb_obj_ids):
collision_scene = list(
p.getClosestPoints(bodyA=robot.robot_ids[0],
bodyB=mesh_body_id,
distance=0.001) # small threshold
)
# check collision position on mesh, count as collision if higher than 5cm (filter out the floor, assume floor as z=0)
collision_scene_count = len(
[col[6][2] for col in collision_scene if col[6][2] > 0.05])
collision_shapenet = []
for shapenet_obj_id in pb_obj_ids:
collision_shapenet.extend(
list(
p.getClosestPoints(
bodyA=robot.robot_ids[0],
bodyB=shapenet_obj_id,
distance=self.collision_thres)) # small threshold
)
collision_shapenet_count = len([
col for col in collision_shapenet if col[8] < self.collision_thres
]) # check if negative distance (penetration)
if collision_shapenet_count > 0 or collision_scene_count > 0:
return True
else:
return False
def _multipleobjreward(self):
"""Calculates the reward for the episode.
The reward is 1 if one of the objects is above height .2 at the end of the
episode.
"""
reward = 0
self._graspSuccess = 0
for uid in self._objectUids:
blockPos, _ = p.getBasePositionAndOrientation(uid)
closestPoints1 = p.getClosestPoints(uid, self._tm700.tm700Uid, 10,
-1, self._tm700.tmFingerIndexL)
closestPoints2 = p.getClosestPoints(
uid, self._tm700.tm700Uid, 10, -1, self._tm700.tmFingerIndexR
) # id of object a, id of object b, max. separation, link index of object a (base is -1), linkindex of object b
numPt = len(closestPoints1)
#print(numPt)
if (numPt > 0):
reward = -((closestPoints1[0][8])**2 +
(closestPoints2[0][8])**2) * (1 / 2) * (
1 / 0.17849278457978357)
if (blockPos[2] > 0.2):
reward = reward + 1000
print("successfully grasped a block!!!")
self._graspSuccess = True
break
return reward
def getDistance(self, Drone):
if len(pbl.getClosestPoints(Drone.droneID, self.ID, 40, -1, -1)) > 0:
return pbl.getClosestPoints(
Drone.droneID, self.ID, 40, -1,
-1)[0][8] + self.sensorNoise * np.random.normal(0, 1)
else:
return np.inf
def is_given_state_in_collision(self,
robot_id,
state,
group,
distance=0.02):
collision = False
start_state = self.get_current_states_for_given_joints(robot_id, group)
time_step_count = 0
self.reset_joint_states_to(robot_id, state, group)
for link_index in group: # ['lbr_iiwa_joint_1', 'lbr_iiwa_joint_2', 'lbr_iiwa_joint_3', 'lbr_iiwa_joint_4', 'lbr_iiwa_joint_5', 'lbr_iiwa_joint_6', 'lbr_iiwa_joint_7']
link_index = self.joint_name_to_id[link_index]
for constraint in self.collision_constraints:
closest_points = [
ClosestPointInfo(*x)
for x in sim.getClosestPoints(robot_id,
constraint,
linkIndexA=link_index,
distance=distance)
]
for cp in closest_points:
dist = cp.contact_distance
if dist <= 0 and cp.body_unique_id_a != cp.body_unique_id_b:
# self.print_contact_points(cp, time_step_count, link_index)
collision = True
# breaking if at least on collision hit has found
break
closest_points = [
ClosestPointInfo(*x)
for x in sim.getClosestPoints(robot_id,
robot_id,
linkIndexA=link_index,
distance=distance)
]
for cp in closest_points:
if cp.link_index_a > -1 and cp.link_index_b > -1:
link_a = self.joint_id_to_info[cp.link_index_a].link_name
link_b = self.joint_id_to_info[cp.link_index_b].link_name
if cp.link_index_a == link_index and cp.link_index_a != cp.link_index_b and not \
self.ignored_collisions[link_a, link_b]:
dist = cp.contact_distance
if dist <= 0:
# self.print_contact_points(cp, time_step_count, link_index)
collision = True
# breaking if at least on collision hit has found
break
if collision:
# breaking if at least on collision hit has found
break
self.reset_joint_states(robot_id, start_state, group)
return collision
def hasCollided(Drone, obstacle):
if len(pbl.getClosestPoints(Drone.droneID, obstacle.ID, 40, -1, -1)) > 0:
D = pbl.getClosestPoints(Drone.droneID, obstacle.ID, 40, -1, -1)[0][8]
else:
D = np.inf
if D <= 0.03 or (Drone.pos[2] < 0.1
and np.linalg.norm(Drone.xdot) <= 0.05):
return True
else:
return False
def check_collisions(self, cut_off_distances):
"""
:param cut_off_distances: (robot_link, body_b, link_b) -> cut off distance. Contacts between objects not in this
dict or further away than the cut off distance will be ignored.
:type cut_off_distances: dict
:param self_collision_d: distances grater than this value will be ignored
:type self_collision_d: float
:type enable_self_collision: bool
:return: (robot_link, body_b, link_b) -> ContactInfo
:rtype: dict
"""
collisions = Collisions(self.robot)
robot_name = self.robot.get_name()
for (robot_link, body_b,
link_b), distance in cut_off_distances.items():
if robot_name == body_b:
object_id = self.robot.get_pybullet_id()
link_b_id = self.robot.get_pybullet_link_id(link_b)
else:
object_id = self.__get_pybullet_object_id(body_b)
if link_b != CollisionEntry.ALL:
link_b_id = self.get_object(body_b).get_pybullet_link_id(
link_b)
robot_link_id = self.robot.get_pybullet_link_id(robot_link)
if body_b == robot_name or link_b != CollisionEntry.ALL:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id, link_b_id)
]
else:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id)
]
if len(contacts) > 0:
try:
body_b_object = self.get_object(body_b)
except KeyError:
body_b_object = self.robot
for contact in contacts:
if link_b == CollisionEntry.ALL:
link_b = body_b_object.pybullet_link_id_to_name(
contact.link_index_b)
k = (robot_link, body_b, link_b)
collisions.add(
k,
ClosestPointInfo(contact.position_on_a,
contact.position_on_b,
contact.contact_distance, distance,
robot_link, body_b, link_b,
contact.contact_normal_on_b, k))
return collisions
def getClosestPoint(self, Drone):
if len(pbl.getClosestPoints(Drone.droneID, self.ID, 40, -1, -1)) > 0:
if self.plane:
return np.array(
pbl.getClosestPoints(Drone.droneID, self.ID, 50, -1, -1)[0]
[5]) + self.sensorNoise * np.random.normal(0, 1, size=3)
else:
return np.array(
pbl.getClosestPoints(Drone.droneID, self.ID, 50, -1, -1)[0]
[6]) + self.sensorNoise * np.random.normal(0, 1, size=3)
else:
return np.array([np.inf, np.inf, np.inf])
def get_closest_points(self,
bodyA,
bodyB,
linkA=None,
linkB=None,
dist=0.2):
"""Returns all the closest points between two objects.
:param bodyA: First body.
:type bodyA: iai_bullet_sim.rigid_body.RigidBody, iai_bullet_sim.multibody.MultiBody
:param bodyB: Second body.
:type bodyB: iai_bullet_sim.rigid_body.RigidBody, iai_bullet_sim.multibody.MultiBody
:param linkA: Closest point will be on this link of bodyA.
:type linkA: str, NoneType
:param linkB: Closest point will be on this link of bodyB.
:type linkB: str, NoneType
:rtype: list
"""
bulletA = bodyA.bId() if bodyA != None else -1
bulletB = bodyB.bId() if bodyB != None else -1
bulletLA = bodyA.link_index_map[
linkA] if bodyA != None and linkA != None and isinstance(
bodyA, MultiBody) else -1
bulletLB = bodyB.link_index_map[
linkB] if bodyB != None and linkB != None and isinstance(
bodyB, MultiBody) else -1
contacts = []
if bulletLA == -1 and bulletLB == -1:
contacts = pb.getClosestPoints(bulletA,
bulletB,
distance=dist,
physicsClientId=self.__client_id)
elif bulletLA != -1 and bulletLB == -1:
contacts = pb.getClosestPoints(bulletA,
bulletB,
linkIndexA=bulletLA,
distance=dist,
physicsClientId=self.__client_id)
elif bulletLA == -1 and bulletLB != -1:
contacts = pb.getClosestPoints(bulletA,
bulletB,
linkIndexB=bulletLB,
distance=dist,
physicsClientId=self.__client_id)
else:
contacts = pb.getClosestPoints(bulletA,
bulletB,
dist,
bulletLA,
bulletLB,
physicsClientId=self.__client_id)
return [self.__create_contact_point(c) for c in contacts]
def ball_has_left_gripper(self):
left_closest = pb.getClosestPoints(self.robot.robot, self.ball, 0.05, 8, physicsClientId=self.client)
# print ("LClosest: ", len(left_closest))
right_closest = pb.getClosestPoints(self.robot.robot, self.ball, 0.05, 17, physicsClientId=self.client)
# print ("RClosest: ", len(right_closest))
min_dist = np.inf
for lc in left_closest:
if lc[8] < min_dist:
min_dist = lc[8]
for lc in right_closest:
if lc[8] < min_dist:
min_dist = lc[8]
return min_dist > 0.05
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self.kinova.kinovaUid,
self.kinova.EndEffectorIndex)
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.005
closestPoints_1 = p.getClosestPoints(self.teacupUid,
self.kinova.kinovaUid, maxDist)
closestPoints_2 = p.getClosestPoints(self.tableUid,
self.kinova.kinovaUid, maxDist)
if (len(closestPoints_1)): #(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("terminating, closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range(100):
graspAction = [0, 0, 0.0001, 0, fingerAngle]
self.kinova.ApplyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle - (0.3 / 100.)
if (fingerAngle < 0):
fingerAngle = 0
for i in range(1000):
graspAction = [0, 0, 0.001, 0, fingerAngle]
self.kinova.ApplyAction(graspAction)
p.stepSimulation()
cupPos, cupOrn = p.getBasePositionAndOrientation(
self.teacupUid)
if (cupPos[2] > 0.23):
#print("BLOCKPOS!")
#print(blockPos[2])
break
state = p.getLinkState(self.kinova.kinovaUid,
self.kinova.EndEffectorIndex)
actualEndEffectorPos = state[0] ##TODO COM need to modified
if (actualEndEffectorPos[2] > 0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
def check_collisions(self, cut_off_distances):
"""
:param cut_off_distances: (robot_link, body_b, link_b) -> cut off distance. Contacts between objects not in this
dict or further away than the cut off distance will be ignored.
:type cut_off_distances: dict
:param self_collision_d: distances grater than this value will be ignored
:type self_collision_d: float
:type enable_self_collision: bool
:return: (robot_link, body_b, link_b) -> ContactInfo
:rtype: dict
"""
# TODO I think I have to multiply distance with something
collisions = defaultdict(lambda: None)
checked_things = set()
for k, distance in cut_off_distances.items():
(robot_link, body_b, link_b) = k
r_k = (link_b, body_b, robot_link)
if r_k in checked_things:
if r_k in collisions:
collisions[k] = self.__flip_contact_info(collisions[r_k])
continue
robot_link_id = self.robot.get_pybullet_link_id(robot_link)
if self.robot.get_name() == body_b:
object_id = self.robot.get_pybullet_id()
link_b_id = self.robot.get_pybullet_link_id(link_b)
else:
object_id = self.__get_pybullet_object_id(body_b)
if link_b != CollisionEntry.ALL:
link_b_id = self.get_object(body_b).get_pybullet_link_id(
link_b)
if body_b == self.robot.get_name() or link_b != CollisionEntry.ALL:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id, link_b_id)
]
else:
contacts = [
ContactInfo(*x) for x in p.getClosestPoints(
self.robot.get_pybullet_id(), object_id, distance *
3, robot_link_id)
]
if len(contacts) > 0:
collisions.update(
{k: min(contacts, key=lambda x: x.contact_distance)})
pass
checked_things.add(k)
return collisions
def _table_collision(self):
maxDist = 0.005
closestPoints = pb.getClosestPoints(
self._kuka.trayUid, self._kuka.kukaUid, maxDist
)
collided_with_table = len(closestPoints) > 0
return collided_with_table
def collision_report(self):
closestPts = p.getClosestPoints(self.kinova, self.fridge, 0.5)
sorted_pts = sorted(closestPts, key=lambda pt: pt[8])
numerOfFridge = p.getNumJoints(self.fridge)
# define a set of closest points.
resultClosestPts = dict()
for i in range(-1,numerOfFridge):
resultClosestPts[i] = None
# print p.getJointInfo(self.fridge, i)[1]
print "length is {}".format(len(sorted_pts))
j = len(resultClosestPts)
i = 0
while j > 0 or i < 20:
# print sorted_pts[i][3], sorted_pts[i][4]
# print np.array(sorted_pts[i][5]) - np.array([0,0,0.675])
# print np.array(sorted_pts[i][6]) - np.array([0,0,0.675])
# print np.array(sorted_pts[i][5]) - np.array(sorted_pts[i][6]), sorted_pts[i][8]
# print
if resultClosestPts[sorted_pts[i][4]] is None:
print np.array(sorted_pts[i][5]) - np.array([0,0,0.675])
print np.array(sorted_pts[i][6]) - np.array([0,0,0.675])
print np.array(sorted_pts[i][5]) - np.array(sorted_pts[i][6]), sorted_pts[i][8]
print
resultClosestPts[sorted_pts[i][4]] = (np.array(sorted_pts[i][5]), np.array(sorted_pts[i][6]))
j -= 1
i += 1
# sorted_pts[0][5] is first object in the getClosestPoints method which is kinova
# sorted_pts[0][6] is first object in the getClosestPoints method which is box
return resultClosestPts
def step(self, action):
self.take_step(action,
robot_arm='left',
gains=self.config('robot_gains'),
forces=self.config('robot_forces'),
human_gains=0.05)
total_force, tool_force, tool_force_on_human, total_force_on_human, new_contact_points = self.get_total_force(
)
end_effector_velocity = np.linalg.norm(
p.getLinkState(self.tool,
1,
computeForwardKinematics=True,
computeLinkVelocity=True,
physicsClientId=self.id)[6])
obs = self._get_obs([tool_force],
[total_force_on_human, tool_force_on_human])
# Get human preferences
preferences_score = self.human_preferences(
end_effector_velocity=end_effector_velocity,
total_force_on_human=total_force_on_human,
tool_force_at_target=tool_force_on_human)
reward_distance = -min([
c[8] for c in p.getClosestPoints(
self.tool, self.human, distance=4.0, physicsClientId=self.id)
])
reward_action = -np.sum(np.square(action)) # Penalize actions
reward_new_contact_points = new_contact_points # Reward new contact points on a person
reward = self.config(
'distance_weight') * reward_distance + self.config(
'action_weight') * reward_action + self.config(
'wiping_reward_weight'
) * reward_new_contact_points + preferences_score
if self.gui and tool_force_on_human > 0:
print('Task success:', self.task_success,
'Force at tool on human:', tool_force_on_human,
reward_new_contact_points)
info = {
'total_force_on_human':
total_force_on_human,
'task_success':
int(self.task_success >= (self.total_target_count *
self.config('task_success_threshold'))),
'action_robot_len':
self.action_robot_len,
'action_human_len':
self.action_human_len,
'obs_robot_len':
self.obs_robot_len,
'obs_human_len':
self.obs_human_len
}
done = False
return obs, reward, done, info
def _isObstacleNear(self,
drone_id,
max_sensor_dist=shared_constants.MAX_SENSOR_DIST,
max_sensor_angle=shared_constants.MAX_SENSOR_ANGLE):
obstacle_state = np.ones(4)
drone_state = self._getDroneStateVector(drone_id)
for obst_id in list(self.DRONE_IDS[:drone_id]) + list(
self.DRONE_IDS[drone_id + 1:]) + self.OBSTACLE_IDS:
list_cp = p.getClosestPoints(self.DRONE_IDS[drone_id],
obst_id,
max_sensor_dist,
physicsClientId=self.CLIENT)
if (len(list_cp) != 0): # there is obstacle near drone
for cp in list_cp:
x_dr, y_dr, z_dr = drone_state[0:3]
x_ob, y_ob, z_ob = cp[6]
dist = np.linalg.norm([x_ob - x_dr, y_ob - y_dr])
theta = np.arctan2(y_ob - y_dr, x_ob - x_dr) * 180 / np.pi
eps = max_sensor_angle
yaw = drone_state[9] * 180 / np.pi
if (-eps < theta - yaw < eps):
obstacle_state[0] = np.clip(
dist, 0, max_sensor_dist) / max_sensor_dist
elif ((90 - eps) < theta - yaw < (90 + eps)):
obstacle_state[1] = np.clip(
dist, 0, max_sensor_dist) / max_sensor_dist
elif (-180 < theta - yaw <= (-180 + eps)
or (180 - eps) < theta - yaw <= 180):
obstacle_state[2] = np.clip(
dist, 0, max_sensor_dist) / max_sensor_dist
elif ((-90 - eps) < theta - yaw < (-90 + eps)):
obstacle_state[3] = np.clip(
dist, 0, max_sensor_dist) / max_sensor_dist
return obstacle_state
def check_self_collision(self, d=0.2, whitelist=None):
if whitelist is None:
whitelist = self.sometimes
def default_contact_info(k):
return ContactInfo(None, self.id, self.id, k[0], k[1], (0, 0, 0),
(0, 0, 0), (0, 0, 0), 1e9, 0)
contact_infos = keydefaultdict(default_contact_info)
contact_infos.update({
(self.link_id_to_name[link_a], self.name,
self.link_id_to_name[link_b]): ContactInfo(*x)
for (link_a, link_b) in whitelist
for x in p.getClosestPoints(self.id, self.id, d, link_a, link_b)
})
contact_infos.update({
(link_b, name, link_a):
ContactInfo(ci.contact_flag, ci.body_unique_id_a,
ci.body_unique_id_b, ci.link_index_b, ci.link_index_a,
ci.position_on_b, ci.position_on_a,
[-x for x in ci.contact_normal_on_b],
ci.contact_distance, ci.normal_force)
for (link_a, name, link_b), ci in contact_infos.items()
})
return contact_infos
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, self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
reward = reward+10000
print("successfully grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print(reward)
return reward
def reset(self):
"""Resets the state of the environment and returns an initial observation.
Returns: observation (object): the initial observation of the
space.
"""
return self._reset()
def _reward(self):
picked_up = 0
# rewards is height of target object
block_pos, block_orn = p.getBasePositionAndOrientation(self.block_uid)
# Computes the distance between base (-1) of self.block_uid and the
# self.kuka_end_effector_idx component of self.robot.kuka_uid
# Any objects outside of 1000 units not counted
# Returns list of closest points
closest_points = p.getClosestPoints(self.block_uid, self.robot.kuka_uid, 1000, -1,
self.robot.kuka_end_effector_idx)
reward = -1000
num_pt = len(closest_points)
if num_pt > 0:
reward = -closest_points[0][8] * 10
# Could we make the task easier by lowering the height condition?
# 3rd item from block_pos vector is the block's Z-coordinate
if block_pos[2] > 0.2:
picked_up = 1
reward = reward + 10000
print("Successfully grasped a block!")
return reward, picked_up
def test_sphere(self):
print("------------------------------ testing sphere ------------------------------")
start = [0.28, 1.0, 0.3]
end = [0.28, 1.8, 0.3]
sphere = self.create_constraint(shape=p.GEOM_SPHERE, radius=0.3, position=start, mass=1)
wall = self.create_constraint(shape=p.GEOM_BOX, size=[0.5, 0.01, 0.5], position=[0, 1.2, 0], mass=1)
trajectory = []
for i in range(3):
traj = self.interpolate(start[i], end[i], 3).tolist()
trajectory.append(traj)
trajectory = np.vstack(trajectory).T
for i in range(len(trajectory)):
p.resetBasePositionAndOrientation(sphere, trajectory[i], [0.0, 0.0, 0.0, 1])
if i < len(trajectory) - 1:
closet_points = p.getClosestPoints(sphere, wall, distance=0.10)
start = time.time()
cast_points = p.getConvexSweepClosestPoints(sphere, wall, distance=0.10,
bodyAfromPosition=trajectory[i],
bodyAtoPosition=trajectory[i + 1],
bodyAfromOrientation=[0.0, 0.0, 0.0, 1],
bodyAtoOrientation=[0.0, 0.0, 0.0, 1]
)
for i in range(len(closet_points)):
if len(closet_points):
if closet_points[i][8] < 0:
print("-------------closest points -----------------")
print("link A index: ", closet_points[i][3])
print("link B index: ", closet_points[i][4])
print("point on A: ", closet_points[i][5])
print("point on B: ", closet_points[i][6])
print("contact normal on B: ", closet_points[i][7])
print("contact distance: ", closet_points[i][8])
print("-------------************ -----------------")
for k in range(len(cast_points)):
if len(cast_points):
if cast_points[i][9] < 0:
print("-------------cast_points -----------------")
print("link A index: ", cast_points[k][3])
print("link B index: ", cast_points[k][4])
print("point on A(t): ", cast_points[k][5])
print("point on A(t+1): ", cast_points[k][6])
print("point on B: ", cast_points[k][7])
print("contact normal on B: ", cast_points[k][8])
print("contact distance: ", cast_points[k][9])
print("contact fraction: ", cast_points[k][10])
print("-------------************ -----------------")
if cast_points[k][10] == 0:
self.assertEquals(np.isclose(cast_points[k][9], closet_points[i][8], atol=0.01), True)
else:
self.assertEquals(np.isclose(cast_points[k][10], 0.5, atol=0.01), True)
print("------------------------------ testing sphere done------------------------------")
def penetrating_objects_exist(self):
for box_id in self.boxes:
contact_pts = p.getClosestPoints(self.robot, box_id, 0.5)
for contact_pt in contact_pts:
if contact_pt[8] < -0.005:
return True
return False
def pairwise_collision(body1, body2, max_distance=MAX_DISTANCE): # 10000
# TODO: confirm that this doesn't just check the base link
return len(
p.getClosestPoints(bodyA=body1,
bodyB=body2,
distance=max_distance,
physicsClientId=CLIENT)) != 0 # getContactPoints
def check_collision(self, collision_distance=0.0):
self.update_closest_points()
# Collisions with others
for i, closest_points_to_other in enumerate(
self.closest_points_to_others):
if i == 0:
# Treat plane specially
for point in p.getClosestPoints(bodyA=self.body_id,
bodyB=0,
distance=0.0):
if point[8] < collision_distance:
self.prev_collided_with = point
return True
else:
# Check whether closest point's distance is
# less than collision distance
for point in closest_points_to_other:
if point[8] < collision_distance:
self.prev_collided_with = point
return True
# Self Collision
for closest_points_to_self_link in self.closest_points_to_self:
for point in closest_points_to_self_link:
if len(point) > 0:
self.prev_collided_with = point
return True
self.prev_collided_with = None
return False
def get_dist(self, other, MAX_DIST=float('inf'), body=False):
contactObjList = p.getClosestPoints(bodyA=self.uid,
bodyB=other.uid,
distance=MAX_DIST,
physicsClientId=self.sim.id)
if len(contactObjList) == 0:
return {
'closest pos self': torch.zeros(0, 3),
'closest pos other': torch.zeros(0, 3),
'distance': torch.zeros(0)
}
contacts = {}
contacts['closest pos self'] = torch.stack(
[torch.tensor(contactObj[5]) for contactObj in contactObjList],
dim=0)
contacts['closest pos other'] = torch.stack(
[torch.tensor(contactObj[6]) for contactObj in contactObjList],
dim=0)
contacts['distance'] = torch.tensor(
[contactObj[8] for contactObj in contactObjList])
if body:
R = self.get_ori(mat=True)
pos = self.get_pos()
contacts['closest pos self'] = (
R.T @ contacts['closest pos self'].T - R.T @ pos).T
contacts['closest pos other'] = (
R.T @ contacts['closest pos other'].T - R.T @ pos).T
return contacts
def _reward(self):
maxDist = 0.003
#rewards is height of target object
cupPos, cupOrn = p.getBasePositionAndOrientation(self.teacupUid)
closestPoints = p.getClosestPoints(self.teacupUid,
self.kinova.kinovaUid, maxDist, -1,
self.kinova.EndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt > 0):
#print("reward:")
reward = -closestPoints[0][8] * 10
if (closestPoints):
reward = reward + 10000
print("successfully grasped a cup!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print(reward)
return reward
def pybullet_collision_checking(pos_new, orn_new):
'''
get contact object id and return boolean
True: not collide
False: collision happens
'''
pos_pre = env.robot.get_position()
orn_pre = env.robot.get_orientation()
(x, y, z) = orn_new
(qw, qx, qy, qz) = euler_to_quaternion(x, y, z)
env.robot.reset_new_pose(pos_new, [qw, qx, qy, qz])
obj_id = env.robot.robot_ids
ground_id, = env.ground_ids
id2 = p.getClosestPoints(
obj_id[0], ground_id[1],
0.3) #id=p.getContactPoints(obj_id[0],ground_id[1])
if len(id2) >= 20:
#collision happened
#go back to previuos state
env.robot.reset_new_pose(pos_pre, orn_pre)
return False
return True
def pairwise_collision_diagnosis(body1,
body2,
max_distance=MAX_DISTANCE): # 10000
return p.getClosestPoints(bodyA=body1,
bodyB=body2,
distance=max_distance,
physicsClientId=CLIENT)
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, self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
reward = reward+10000
print("successfully grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print(reward)
return reward
def _reward(self):
if self.objsOrNot:
closestPoints = p.getClosestPoints(
self._neobotixschunk.neobotixschunkUid, self.obsUid, 0.2)
if len(closestPoints):
rewardgghghg = -closestPoints[0][8] # contact distance
#
# numPt = len(closestPoints)
# reward = -1000
# if (numPt > 0):
#
# return reward
delta_dis = self.ee_dis - self._dis_vor
self._dis_vor = self.ee_dis
tau = (self.ee_dis / self.dis_init)**2
if tau > 1:
penalty = (
1 - tau
) * self.ee_dis - self.dis_init #+ self._envStepCounter/self._maxSteps/2
else:
penalty = 0 #self._envStepCounter/self._maxSteps/2
if self._rewardtype == 'rdense':
# reward = -(1-tau)*self.ee_dis - tau*self.base_dis + self.r_penalty -np.linalg.norm(self._actions)#+ penalty
reward = 1 / (self.ee_dis**2) - self.ee_dis**2 + self.r_penalty
# reward = -reward
elif self._rewardtype == 'rsparse':
if delta_dis > 0:
reward = 0
else:
reward = 1
return reward
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 _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 _reward(self):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
reward = reward+10000
print("successfully grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print(reward)
return reward
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.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("terminating, closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (100):
graspAction = [0,0,0.0001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
if (blockPos[2] > 0.23):
#print("BLOCKPOS!")
#print(blockPos[2])
break
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
pitch += 0.01
if (pitch >= 3.1415 * 2.):
pitch = 0
yaw += 0.01
if (yaw >= 3.1415 * 2.):
yaw = 0
baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
if (obB >= 0):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1,
bodyB=-1,
distance=100,
collisionShapeA=geom,
collisionShapeB=geomBox,
collisionShapePositionA=[0.5, 0, 1],
collisionShapePositionB=basePositionB,
collisionShapeOrientationB=baseOrientationB)
#pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
if len(pts) > 0:
#print(pts)
distance = pts[0][8]
#print("distance=",distance)
ptA = pts[0][5]
ptB = pts[0][6]
p.addUserDebugLine(lineFromXYZ=ptA,
lineToXYZ=ptB,
