def initial_context(self, request, ptypes, entity=None):
point = get_point(request, entity)
entity_types = tuple(get_object_or_404(EntityType, slug=t) for t in ptypes.split(";"))
if point:
entities = Entity.objects.filter(location__isnull=False, is_sublocation=False)
if ptypes:
for et in entity_types:
entities = entities.filter(all_types_completion=et)
else:
entity_types = []
entities = entities.distance(point).order_by("distance")[:99]
else:
entities = []
context = super(NearbyDetailView, self).initial_context(request, ptypes, entities)
context.update(
{
"entity_types": entity_types,
"point": point,
"entities": entities,
"entity": entity,
"exposes_user_data": entity is None, # entity is None => point is the user's location
}
)
return context
def reset(self):
table_pos = np.array([0.0, 0.0, 0.0])
utils.set_point(self.table, table_pos)
utils.set_zrot(self.table, pi*0.5)
table_x = np.random.rand()-0.5
table_y = np.random.rand()-0.5
utils.set_point(self.plate, [table_x, table_y, 0.63])
plate_pos = utils.get_point(self.plate) #Get target obj center position
self.rgripper.set_basepose(np.array([0, 0.25, 0.78]) + np.array([plate_pos[0], plate_pos[1], 0]), [-1.54, 0.5, -1.57])
self.rgripper.set_state([0, 0, 0])
self.rgripper.set_angle(self.rgripper.gripper, 0)
self.lgripper.set_basepose(np.array([0, -0.24, 0.78]) + np.array([plate_pos[0], plate_pos[1], 0]), [1.54, 0.65, 1.57])
self.lgripper.set_state([0, 0, 0])
self.lgripper.set_angle(self.lgripper.gripper, 0)
self.rgripper.set_gripper_width(0.5, force=True)
self.lgripper.set_gripper_width(0.5, force=True)
"""
Currently,
If plate is on the right side, grasping tactics is rotational grasping.
If plate is on the left side, grasping tactics is pushing and grasping.
"""
if(plate_pos[1] < 0):
tactics = 0
else:
tactics = 1
for i in range(100):
pb.stepSimulation()
return tactics
def handle_GET(self, request, context, entity=None):
point = get_point(request, entity)
if entity:
return_url = reverse("places:entity-nearby-list", args=[entity.identifier_scheme, entity.identifier_value])
else:
return_url = reverse("places:nearby-list")
entity_types_map = dict((e.slug, e) for e in EntityType.objects.all())
entity_types = tuple(
(name, tuple(entity_types_map[t] for t in types)) for (name, types) in self.conf.nearby_entity_types
)
flat_entity_types = set(chain(*[types for name, types in entity_types]))
entities = Entity.objects.filter(location__isnull=False, all_types_completion__in=flat_entity_types)
entities = entities.distance(point).order_by("distance")
for et in flat_entity_types:
et.max_distance = 0
et.entities_found = 0
for e in entities:
for et in e.all_types_slugs:
et = entity_types_map[et]
if not et in flat_entity_types:
continue
if (e.distance.m ** 0.75) * (et.entities_found + 1) > 500:
flat_entity_types.remove(et)
continue
et.max_distance = e.distance
et.entities_found += 1
if len(flat_entity_types) == 0 or e.distance.m > 5000:
break
entity_types = tuple((name, tuple(t for t in types if t.entities_found > 0)) for name, types in entity_types)
context.update(
{
"entity_types": entity_types,
"entity": entity,
"return_url": return_url,
"exposes_user_data": entity is None, # entity is None => we've searched around the user's location
}
)
if entity and not entity.location:
return self.render(request, context, "places/entity_without_location")
return self.render(request, context, "places/nearby_list")
def rollout(self):
try:
for try_count in six.moves.range(self.try_num):
tactics = self.reset()
# Get target obj state
plate_pos = utils.get_point(
self.plate) #Get target obj center position
if tactics:
print("Rotational Grasping!!!")
# Approaching and close right gripper
pitch = np.random.rand() * pi / 8 + pi / 8
self.rgripper.set_state([-0.3, 0.5, 0]) #Success position
self.lgripper.set_state([-0.2, 0.1, 0]) #Success position
for i in range(40):
pb.stepSimulation()
if len(pb.getContactPoints(
bodyA=1,
bodyB=3)): #Contact Rgripper and table
self.rgripper.set_state([0.3 - i, 0.5 + i,
0]) #rgripper up
else:
self.rgripper.set_state([0.3 + i, 0.5 + i,
0]) #rgripper down
width, height, rgbImg, depthImg, segImg = pb.getCameraImage(
360, 240, viewMatrix=self.viewMatrix)
self.frames.append(rgbImg)
if len(pb.getContactPoints(
bodyA=2,
bodyB=3)): #Contact Rgripper and plate
self.rgripper.set_pose([-1.54, pitch,
-1.57]) #Random Scooping
#self.rgripper.set_pose([-1.54, 0.8, -1.57]) #Success Scooping
self.rgripper.set_gripper_width(
0.0) #Close gripper
else:
print("Moving Grasping!!!")
pitch = np.random.rand() * pi / 8 + pi / 8
self.rgripper.set_state([-0.3, 0.5, 0]) #Success position
self.lgripper.set_state([-0.2, 0.1, 0]) #Success position
for i in range(100):
pb.stepSimulation()
plate_pos = utils.get_point(
self.plate) #Get target obj center position
if plate_pos[1] > -0.5:
if len(pb.getContactPoints(
bodyA=1,
bodyB=3)): #Contact Rgripper and table
self.rgripper.set_state(
[0.3 - i * 0.05, 0.5 + i * 0.05,
0]) #rgripper up
self.lgripper.set_state(
[-0.2 - i * 0.01, 0.1 - i * 0.02,
0]) #lgripper up
else:
self.rgripper.set_state(
[0.3 + i * 0.05, 0.5 + i * 0.05,
0]) #rgripper down
self.lgripper.set_state(
[-0.2 + i * 0.01, 0.1 - i * 0.02,
0]) #lgripper down
elif (len(pb.getContactPoints(
bodyA=2,
bodyB=4))): #Contact Rgripper and plate
self.lgripper.set_gripper_width(
0.0) #close gripper
self.lgripper.set_state(
[-0.2 + i * 0.01, 0.1 + i * 0.02,
0]) #lgripper move left
self.rgripper.set_state(
[-i * 0.01, -0.6 - i * 0.03,
0]) #rgripper move right
width, height, rgbImg, depthImg, segImg = pb.getCameraImage(
360, 240, viewMatrix=self.viewMatrix)
self.frames.append(rgbImg)
# Picking up
self.rgripper.set_state([0.0, -0.5, 0.0])
self.lgripper.set_state([0.0, -0.5, 0.0])
contact_len = 0 #If gripper contact plate, contact_len increase
for i in range(50):
pb.stepSimulation()
width, height, rgbImg, depthImg, segImg = pb.getCameraImage(
width=360, height=240, viewMatrix=self.viewMatrix)
self.frames.append(rgbImg)
#time.sleep(0.005)
contact_len += len(pb.getContactPoints(
bodyA=1, bodyB=2)) #Judge if plate and table collision
contact_len += len(pb.getContactPoints(
bodyA=0, bodyB=2)) #Judge if plate and table collision
if contact_len > 1: #Judge if gripper contact plate
print("Failed!!!")
else:
print("Succeeded!!!")
save_video(self.frames, "sample.mp4")
except KeyboardInterrupt:
sys.exit()
