position_min, position_max = [0.8, -0.2, 1.0], [1.0, 0.2, 1.4]
starting_joint_positions = agent.get_joint_positions()
for i in range(LOOPS):
# Reset the arm at the start of each 'episode'
agent.set_joint_positions(starting_joint_positions)
# Get a random position within a cuboid and set the target position
pos = list(np.random.uniform(position_min, position_max))
target.set_position(pos)
# Get a path to the target (rotate so z points down)
try:
path = agent.get_path(position=pos, euler=[0, math.radians(180), 0])
except ConfigurationPathError as e:
print('Could not find path')
continue
# Step the simulation and advance the agent along the path
done = False
while not done:
done = path.step()
pr.step()
print('Reached target %d!' % i)
pr.stop()
pr.shutdown()
for i in range(0, 6):
cube = Shape.create(type=PrimitiveShape.CUBOID,
size=[0.1, 0.1, 0.1],
color=[1.0, 0.1, 0.1])
cubes.append(cube)
cube.set_position(pos[i])
dummies[i].set_position(pos[i])
dummies[i].set_parent(cubes[i])
pr.step()
prev_pos, quat = panda.get_tip().get_position(), panda.get_tip(
).get_quaternion()
prev_pos1, quat1 = panda1.get_tip().get_position(), panda1.get_tip(
).get_quaternion()
for i in range(0, 3):
try:
path = panda.get_path(position=prev_pos, quaternion=quat)
except ConfigurationPathError as e:
print('Could not find path')
done = False
while not done:
done = path.step()
pr.step()
try:
path = panda1.get_path(position=prev_pos1, quaternion=quat1)
except ConfigurationPathError as e:
print('Could not find path')
done = False
while not done:
done = path.step()
pr.step()
