def ur5():
I = Interface()
I.connect()
MOVABLE_ARM_JOINTS = [1, 2, 3, 4, 5, 6]
ROBOT_URDF_PATH = '../models/ur5_description/urdf/ur5.urdf'
END_EFFECTOR_LINK = 'end_effector_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
# ------------------------UR5--------------------------#
x_offset = 0.33
y_offset = 0.15
robot = I.load_model(ROBOT_URDF_PATH)
redBlock1 = I.load_model(CUBE_URDF % 'red',
Pose(Point(0.3 + x_offset, -0.1 + y_offset,
0.265)),
fixed_base=False)
redBlock2 = I.load_model(CUBE_URDF % 'red',
Pose(
Point(0.35 + x_offset, 0.05 + y_offset,
0.265)),
fixed_base=False)
table = I.load_model(TABLE,
Pose(Point(0.35 + x_offset, 0.0 + y_offset, 0.05),
Euler(0.0, 0.0, 1.57)),
fixed_base=False,
scaling=0.3)
# ------------------------Jaco---------------------------#
velocities = Csv('UR5_velocities.csv', [
'joints_1', 'joints_2', 'joints_3', 'joints_4', 'joints_5', 'joints_6',
'joints_11', 'joints_13', 'joints_15', 'joints_16', 'joints_17',
'joints_18'
],
mode='r').read()
torques = Csv('UR5_torques.csv', [
'joints_1', 'joints_2', 'joints_3', 'joints_4', 'joints_5', 'joints_6',
'joints_11', 'joints_13', 'joints_15', 'joints_16', 'joints_17',
'joints_18'
],
mode='r').read()
action = Csv('UR5_action.csv', ['action'], mode='r').read()
for joint, values in velocities.items():
velocities[joint] = map(float, values)
velocities = np.vstack(velocities.values())
for joint, values in torques.items():
torques[joint] = map(float, values)
torques = np.vstack(torques.values())
I.replay(robot, [velocities.T, torques.T, action.values()[0]])
#
time.sleep(5.0)
I.disconnect()
def jaco():
I = Interface()
I.connect()
# ------------------------Jaco---------------------------#
ROBOT_URDF_PATH = '../models/jaco_pkg/jaco_description/urdf/test_jaco.urdf'
BLOCK_URDF = "../ss-pybullet-master/models/drake/objects/block_for_pick_and_place_mid_size.urdf"
END_EFFECTOR_LINK = 'jaco_eef_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
TRAY_URDF = '../models/tray/tray.urdf'
robot = I.load_model(ROBOT_URDF_PATH)
redBlock1 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.22, -0.2, 0.265)), fixed_base = False)
redBlock2 = I.load_model(CUBE_URDF % 'red', Pose(Point(0.35, 0.05, 0.265)), fixed_base = False)
greenBlock1 = I.load_model(CUBE_URDF % 'green', Pose(Point(0.38, -0.1, 0.2599)), fixed_base = False)
greenBlock2 = I.load_model(CUBE_URDF % 'green', Pose(Point(0.38, 0.2, 0.2599)), fixed_base = False)
table = I.load_model(TABLE, Pose(Point(0.35, 0.0, 0.05), Euler(0.0, 0.0, 1.57)), fixed_base = False, scaling = 0.3)
# ------------------------Jaco---------------------------#
velocities = Csv('JACO_velocities.csv',
['joints_3', 'joints_5', 'joints_7', 'joints_9', 'joints_11', 'joints_13', 'joints_16',
'joints_19', 'joints_22'], mode = 'r').read()
for joint, values in velocities.items():
velocities[joint] = map(float, values)
velocities = np.vstack(velocities.values())
I.replay(robot, velocities.T)
time.sleep(2.0)
I.disconnect()
def main():
folders = get_folders(path=DATA_PATH)
p_mode_velocity_file = 'p_mode_velocities.csv'
scene_objects = 'scene_objects.npy'
success = 0
for i in range(507, 999):
folder = DATA_PATH + 'Trial_%d/' % i
with Interface() as I:
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05),
Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
# TODO set camera
I.set_camera_pose(0.6, 155, -50, [0.2, 0.0, 0.5])
scene = np.load(os.path.join(folder, scene_objects))[2:, 0]
block = I.load_model(CUBE_URDF,
Pose(Point(*scene[0])),
fixed_base=False)
basket = I.load_model(BASKET_URDF,
Pose(Point(*scene[1])),
fixed_base=False)
I.save_body_conf()
if os.path.isfile(os.path.join(folder, p_mode_velocity_file)):
velocities = Pd(os.path.join(
folder, p_mode_velocity_file)).read().values
I.replay_velocities(robot, velocities)
if inside_basket(I, block, basket):
I.restore_body_conf()
time.sleep(1.0)
I.replay_velocities(robot,
velocities,
filename=os.path.join(folder, 'img'))
success += 1
if success == 270:
break
print('Trials : ', folder, ' , Success: ', success)
def load_blocks(I):
"""
Load the blocks
:param I: Interface
:return: list of blocks pose and handle parameters
"""
res = []
for block in BLOCKS:
j = 0
while True:
r_x = limits['x_min'] + (limits['x_max'] -
limits['x_min']) * random.random()
r_y = limits['y_min'] + (limits['y_max'] -
limits['y_min']) * random.random()
collision = False
for b in res:
if sqrt((b.pose[0] - r_x)**2 +
(b.pose[1] - r_y)**2) <= block_thresh:
collision = True
break
if not collision:
pose = [r_x, r_y, TABLE_HEIGHT]
handle = I.load_model(CUBE_URDF % block[0:block.find('Block')],
Pose(Point(*pose)),
fixed_base=False)
res.append(Block(pose=pose, handle=handle))
break
if j == 1000:
break
j += 1
return res
def load_objects(I, **kwargs):
"""
Load the blocks
:param I: Interface
:return: list of blocks pose and handle parameters
"""
res = []
for obj in SCENE_OBJECT:
while True:
r_x = limits['x_min'] + (limits['x_max'] -
limits['x_min']) * random.random()
r_y = limits['y_min'] + (limits['y_max'] -
limits['y_min']) * random.random()
collision = False
for b in res:
if sqrt((b.pose[0] - r_x)**2 +
(b.pose[1] - r_y)**2) <= block_thresh:
collision = True
break
if not collision:
pose = [r_x, r_y, TABLE_HEIGHT]
handle = I.load_model(kwargs[obj],
Pose(Point(*pose)),
fixed_base=False)
res.append(Object(pose=pose, handle=handle))
break
return res
def configuration(self, configuration): #[theta_1, theta_2, ..., theta_n]
assert len(configuration) == len(self.joints)
polys = []
origin = None
angle = None
for i in range(len(configuration)):
(joint, link) = self.joints[i]
if origin == None:
angle = configuration[i]
origin = joint.rotate(angle)
else:
origin += joint.rotate(angle)
angle += configuration[i]
polys.append(link.at_pose(Pose(origin, angle)))
return Object(self.reference, polys)
def plan(I,
robot,
gripper_link,
b,
neutralPose,
initialize=False,
x_offset=0.0,
y_offset=0.0):
initial_pose = [neutralPose] if initialize else []
target_pose = initial_pose + \
[Pose(Point(b[0].pose[0], b[0].pose[1], TABLE_HEIGHT + 0.2), Euler(0.0, 3.14, 1.57)),
Pose(Point(b[0].pose[0], b[0].pose[1], TABLE_HEIGHT + 0.02), Euler(0.0, 3.14, 1.57)),
Pose(Point(b[0].pose[0], b[0].pose[1], TABLE_HEIGHT + 0.2), Euler(0.0, 3.14, 1.57)),
Pose(Point(b[1].pose[0] + x_offset, b[1].pose[1] + y_offset, TABLE_HEIGHT + 0.3),
Euler(0.0, 3.14, 1.57)),
Pose(Point(b[1].pose[0] + x_offset, b[1].pose[1] + y_offset, TABLE_HEIGHT + 0.07),
Euler(0.0, 3.14, 1.57)),
Pose(Point(b[1].pose[0], b[1].pose[1], TABLE_HEIGHT + 0.3), Euler(0.0, 3.14, 1.57)),
]
stack_path = []
for poses in target_pose:
end_conf = I.calculate_inverse_kinematics(robot, gripper_link, poses)
if end_conf is not None:
path = I.plan_motion(robot, end_conf, [2, 3, 4, 5])
if path is None:
return None
else:
stack_path.append(path)
else:
return None
I.set_joint_positions(robot, MOVABLE_ARM_JOINTS, end_conf)
return stack_path
def main():
for i in range(TRIALS):
SAVE_PATH = DATA_PATH + 'Trial_%d/' % i
mkdir(SAVE_PATH)
p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv'
scene_object_file = SAVE_PATH + 'scene_objects'
start = time.time()
while time.time() - start <= 120.0:
with Interface(save=True, filename=p_mode_velocity_file) as I:
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05),
Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
#TODO set camera
I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7])
#LOAD_BLOCKS
blocks = load_blocks(I)
if len(blocks) != len(BLOCKS):
continue
time.sleep(1.0)
# Gripper ID
gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK)
# Neutral Pose
neutralPose = Pose(Point(0.35, 0.0, 0.5),
Euler(0.0, 3.14, 1.57))
I.save_body_conf(save=True, filename=scene_object_file)
offset = 0.005
x_offset = [
0.0, 0.0, 0.0, offset, offset, offset, -offset, -offset,
-offset
]
y_offset = [
0.0, offset, -offset, 0.0, offset, -offset, 0.0, offset,
-offset
]
for k in range(9):
# Set initial state
I.restore_body_conf()
I.set_initial_state(robot)
success = False
# Plan motion
paths = {}
for j in range(0, len(blocks), 2):
initialize = True if j == 0 else False
path = plan(I,
robot,
gripper_link,
blocks[j:j + 2],
neutralPose,
initialize=initialize,
x_offset=x_offset[k],
y_offset=y_offset[k])
if path is not None:
paths[j] = path
else:
paths = {}
break
if len(paths) > 0:
I.reset_simulation(robot)
time.sleep(1.0)
I.command(robot, paths[0][0])
I.command(robot, paths[0][1])
I.command(robot, paths[0][2])
I.gripper(robot, 'close')
I.command(robot, paths[0][3])
I.command(robot, paths[0][4])
I.command(robot, paths[0][5])
I.gripper(robot, 'open')
I.command(robot, paths[0][6])
time.sleep(1.0)
if on_top(I, *blocks[0:2]):
I.command(robot, paths[2][0])
I.command(robot, paths[2][1])
I.gripper(robot, 'close')
I.command(robot, paths[2][2])
I.command(robot, paths[2][3])
I.command(robot, paths[2][4])
I.gripper(robot, 'open')
I.command(robot, paths[2][5])
time.sleep(1.0)
if on_top(I, *blocks[2:4]):
I.command(robot, paths[4][0])
I.command(robot, paths[4][1])
I.gripper(robot, 'close')
I.command(robot, paths[4][2])
I.command(robot, paths[4][3])
I.command(robot, paths[4][4])
I.gripper(robot, 'open')
I.command(robot, paths[4][5])
time.sleep(1.0)
if on_top(I, *blocks[4:6]):
success = True
time.sleep(1.0)
I.vel.write_csv()
break
if success:
break
MOVABLE_ARM_JOINTS = [1, 2, 3, 4, 5, 6]
ROBOT_URDF_PATH = '../models/ur5_description/urdf/ur5.urdf'
END_EFFECTOR_LINK = 'end_effector_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
I = Interface(save=True)
I.connect()
x_offset = 0.33
y_offset = 0.15
robot = I.load_model(ROBOT_URDF_PATH)
redBlock1 = I.load_model(CUBE_URDF % 'red',
Pose(Point(0.3 + x_offset, -0.1 + y_offset, 0.265)),
fixed_base=False)
redBlock2 = I.load_model(CUBE_URDF % 'red',
Pose(Point(0.35 + x_offset, 0.05 + y_offset, 0.265)),
fixed_base=False)
table = I.load_model(TABLE,
Pose(Point(0.35 + x_offset, 0.0 + y_offset, 0.05),
Euler(0.0, 0.0, 1.57)),
fixed_base=False,
scaling=0.3)
def move_to(target_pose, gripper_link):
paths = []
for poses in target_pose:
end_conf = I.calculate_inverse_kinematics(robot, gripper_link, poses)
def main():
with open(pathname(os.getcwd(), '..', '..', 'machines.json'), 'r') as f:
js = json.load(f)
DATA_PATH = js[IP]['DATA_PATH']
trial = js[IP]['trial_start']
while True:
if trial == js[IP]['trial_stop']:
break
start = time.time()
while time.time() - start <= 120.0:
SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial
mkdir(SAVE_PATH)
p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv'
scene_object_file = SAVE_PATH + 'scene'
with Interface(mode='direct',
save=True,
filename=p_mode_velocity_file) as I:
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05),
Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
# TODO set camera
I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7])
# LOAD_BLOCKS
blocks = load_blocks(I)
if len(blocks) != len(BLOCKS):
continue
time.sleep(1.0)
# Gripper ID
gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK)
# Neutral Pose
neutralPose = Pose(Point(0.35, 0.0, 0.5),
Euler(0.0, 3.14, 1.57))
I.save_body_conf(save=True, filename=scene_object_file)
I.set_initial_state(robot)
offset = 0.005
x_offset = [0.0, offset, -offset, -offset]
y_offset = [0.0, offset, -offset, 0.0]
shuffle(blocks)
color = ['red']
for k in range(1):
# Set initial state
I.restore_body_conf()
success = False
# Plan motion
paths = []
velocities = OrderedDict()
for j in range(1):
initialize = True if j == 0 else False
path = plan(I,
robot,
gripper_link,
blocks,
neutralPose,
initialize=initialize,
x_offset=x_offset[k],
y_offset=y_offset[k])
if path is not None:
paths.append(path)
else:
paths = []
break
if len(paths) > 0:
I.reset_simulation(robot)
# time.sleep(1.0)
I.command(robot, paths[0][0])
I.command(robot, paths[0][1])
I.command(robot, paths[0][2])
I.gripper(robot, 'close')
I.command(robot, paths[0][3])
I.command(robot, paths[0][4])
I.command(robot, paths[0][5])
I.gripper(robot, 'open')
I.command(robot, paths[0][6])
if on_top(I, *blocks[0:2]):
I.keep_pose(body=robot)
I.vel.write_csv()
velocities = I.vel.array
I.restore_body_conf()
I.reset_simulation(robot)
I.replay_velocities(robot, velocities)
if on_top(I, *blocks[0:2]):
I.restore_body_conf()
I.reset_simulation(robot)
time.sleep(1.0)
I.replay_velocities(
robot,
velocities,
path=os.path.abspath(SAVE_PATH))
if on_top(I, *blocks[0:2]):
success = True
np.save(
os.path.join(SAVE_PATH, 'velocities'),
np.array(I.target_vel))
np.save(
os.path.join(SAVE_PATH,
'joints_configurations'),
np.array(I.joint_val))
trial += 1
break
else:
shutil.rmtree(SAVE_PATH)
if success:
break
else:
if folder_exists(SAVE_PATH):
shutil.rmtree(SAVE_PATH)
from pblt import Interface, MOVABLE_ARM_JOINTS
import time
from geometry import Point, Pose, Euler
ROBOT_URDF_PATH = '../models/jaco_pkg/jaco_description/urdf/test_jaco.urdf'
END_EFFECTOR_LINK = 'jaco_eef_link'
TABLE = '../models/table/table.urdf'
CUBE_URDF = '../models/cube/%s_cube.urdf'
BASKET_URDF = '../models/basket/basket.urdf'
with Interface() as I:
robot = I.load_model(ROBOT_URDF_PATH)
table = I.load_model(TABLE,
Pose(Point(0.35, 0.0, 0.05), Euler(0.0, 0.0, 1.57)),
fixed_base=False,
scaling=0.3)
redBlock1 = I.load_model(CUBE_URDF % 'red',
Pose(Point(0.22, -0.2, 0.265)),
fixed_base=False)
basket = I.load_model(BASKET_URDF,
Pose(Point(0.35, 0.05, 0.265)),
fixed_base=False)
def move_to(target_pose, gripper_link):
paths = []
for poses in target_pose:
end_conf = I.calculate_inverse_kinematics(robot, gripper_link,
poses)
"%(color)sBlock%(indice)d" % ({
'color': name,
'indice': indice
}) for name in ['red', 'green', 'blue'] for indice in [1, 2]
]
BLOCK_SIZE = 0.05
Block = namedtuple('Block', ['pose', 'handle'])
I = Interface()
I.connect()
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05), Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
# TODO set camera
camTargetPos = [0, 0, 0]
cameraUp = [0, 0, 1]
cameraPos = [1, 1, 1]
pitch = -50
roll = 10
yaw = 70
upAxisIndex = 2
camDistance = 1.5
pixelWidth = 256
pixelHeight = 256
def main():
with open(pathname(os.getcwd(), '..', '..', 'machines.json'), 'r') as f:
js = json.load(f)
DATA_PATH = js[IP]['DATA_PATH']
trial = js[IP]['trial_start']
while True:
if trial == js[IP]['trial_stop']:
break
start = time.time()
while time.time() - start <= 120.0:
SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial
mkdir(SAVE_PATH)
p_mode_velocity_file = SAVE_PATH + 'p_mode_velocities.csv'
scene_object_file = SAVE_PATH + 'scene_objects'
with Interface(mode='direct',
save=True,
filename=p_mode_velocity_file) as I:
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05),
Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
# TODO set camera
I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7])
# LOAD SCENE OBJECTS
scene_object = load_objects(
I, **dict(zip(SCENE_OBJECT, [CUBE_URDF, BASKET_URDF])))
if len(scene_object) != len(SCENE_OBJECT):
continue
# Gripper ID
gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK)
# Neutral Pose
neutralPose = Pose(Point(0.35, 0.0, 0.5),
Euler(0.0, 3.14, 1.57))
I.save_body_conf(save=True, filename=scene_object_file)
I.set_initial_state(robot)
offset = 0.005
x_offset = [
0.0, 0.0, 0.0, offset, offset, offset, -offset, -offset,
-offset
]
y_offset = [
0.0, offset, -offset, 0.0, offset, -offset, 0.0, offset,
-offset
]
for k in range(2):
# Set initial state
I.restore_body_conf()
success = False
initialize = True
path = plan(I,
robot,
gripper_link,
scene_object,
neutralPose,
initialize=initialize,
x_offset=x_offset[k],
y_offset=y_offset[k])
if len(path) > 0:
I.reset_simulation(robot)
time.sleep(1.0)
I.command(robot, path[0])
I.command(robot, path[1])
I.command(robot, path[2])
I.gripper(robot)
I.command(robot, path[3])
I.command(robot, path[4])
I.command(robot, path[5])
I.gripper(robot, 'open')
I.command(robot, path[6])
if inside_basket(I, *scene_object):
success = True
I.keep_pose(body=robot)
I.vel.write_csv()
velocities = I.vel.array
I.restore_body_conf()
I.reset_simulation(robot)
I.replay_velocities(robot, velocities)
if inside_basket(I, *scene_object):
I.restore_body_conf()
I.reset_simulation(robot)
time.sleep(1.0)
I.replay_velocities(
robot,
velocities,
path=os.path.abspath(SAVE_PATH))
if inside_basket(I, *scene_object):
np.save(
os.path.join(SAVE_PATH, 'velocities'),
np.array(I.target_vel))
np.save(
os.path.join(SAVE_PATH,
'joints_configurations'),
np.array(I.joint_val))
trial += 1
else:
shutil.rmtree(SAVE_PATH)
else:
shutil.rmtree(SAVE_PATH)
break
else:
shutil.rmtree(SAVE_PATH)
if success:
break
def getPose(self, t: float) -> Pose:
"""Interpolate the pose on the spline where 0 <= t <= self.length."""
point = self.getPoint(t)
heading = self.getHeading(t)
return Pose(point.x, point.y, heading)
def main():
trial = 2495
while True:
if trial == 3000:
break
start = time.time()
while time.time() - start <= 120.0:
SAVE_PATH = DATA_PATH + 'Trial_%d/' % trial
mkdir(SAVE_PATH)
p_mode_velocity_file = SAVE_PATH + 'VELOCITIES/'
scene_object_file = SAVE_PATH + 'SCENE_OBJECTS/'
blocks_goals_file = SAVE_PATH + 'GOALS/'
mkdir(scene_object_file)
mkdir(p_mode_velocity_file)
with Interface(mode='direct',
save=True,
filename=p_mode_velocity_file +
'p_mode_velocities.csv') as I:
# LOAD ROBOT
robot = I.load_model(ROBOT_URDF_PATH)
# LOAD TABLE
table = I.load_model(TABLE,
Pose(Point(0.4, 0.0, 0.05),
Euler(1.57, 0.0, 0.0)),
fixed_base=True,
scaling=1.0)
#TODO set camera
I.set_camera_pose(1.6, 60, -25, [0.2, 0.0, 0.7])
#LOAD_BLOCKS
blocks = load_blocks(I)
if len(blocks) != len(BLOCKS):
continue
time.sleep(1.0)
# Gripper ID
gripper_link = I.get_link_from_name(robot, END_EFFECTOR_LINK)
# Neutral Pose
neutralPose = Pose(Point(0.35, 0.0, 0.5),
Euler(0.0, 3.14, 1.57))
I.save_body_conf(save=True,
filename=scene_object_file + 'scene')
I.set_initial_state(robot)
offset = 0.005
x_offset = [
0.0, 0.0, 0.0, offset, offset, offset, -offset, -offset,
-offset
]
y_offset = [
0.0, offset, -offset, 0.0, offset, -offset, 0.0, offset,
-offset
]
blocks = [blocks[k * 2:2 * (k + 1)] for k in range(3)]
shuffle(blocks)
blocks = blocks[0] + blocks[1] + blocks[2]
color = [blocks[0].name, blocks[2].name, blocks[4].name]
for k in range(9):
# Set initial state
I.restore_body_conf()
success = False
# Plan motion
paths = {}
velocities = OrderedDict()
for j in range(0, len(blocks), 2):
initialize = True if j == 0 else False
path = plan(I,
robot,
gripper_link,
blocks[j:j + 2],
neutralPose,
initialize=initialize,
x_offset=x_offset[k],
y_offset=y_offset[k])
if path is not None:
paths[j] = path
else:
paths = {}
break
if len(paths) > 0:
I.reset_simulation(robot)
# time.sleep(1.0)
I.command(robot, paths[0][0])
I.command(robot, paths[0][1])
I.command(robot, paths[0][2])
I.gripper(robot, 'close')
I.command(robot, paths[0][3])
I.command(robot, paths[0][4])
I.command(robot, paths[0][5])
I.gripper(robot, 'open')
I.command(robot, paths[0][6])
# time.sleep(1.0)
if on_top(I, *blocks[0:2]):
velocities[color[0]] = I.vel.array
shape = I.vel.array.shape[0]
I.command(robot, paths[2][0])
I.command(robot, paths[2][1])
I.gripper(robot, 'close')
I.command(robot, paths[2][2])
I.command(robot, paths[2][3])
I.command(robot, paths[2][4])
I.gripper(robot, 'open')
I.command(robot, paths[2][5])
# time.sleep(1.0)
if on_top(I, *blocks[2:4]):
velocities[color[1]] = I.vel.array[shape:]
shape = I.vel.array.shape[0]
I.command(robot, paths[4][0])
I.command(robot, paths[4][1])
I.gripper(robot, 'close')
I.command(robot, paths[4][2])
I.command(robot, paths[4][3])
I.command(robot, paths[4][4])
I.gripper(robot, 'open')
I.command(robot, paths[4][5])
# time.sleep(1.0)
if on_top(I, *blocks[4:6]):
velocities[color[2]] = I.vel.array[shape:]
success = True
time.sleep(1.0)
for e, colors in enumerate(
velocities.keys()):
mkdir(blocks_goals_file + '%d_' % e +
colors)
np.save(
blocks_goals_file + '%d_' % e +
colors + '/' + colors,
velocities[colors])
I.vel.write_csv()
I.restore_body_conf()
I.reset_simulation(robot)
time.sleep(1.0)
task_velocities = np.concatenate(
velocities.values())
I.replay_velocities(robot, task_velocities)
final_stacking = []
for k in range(0, len(blocks), 2):
final_stacking.append(
on_top(I, *blocks[k:k + 2]))
if all(final_stacking):
I.restore_body_conf()
I.reset_simulation(robot)
time.sleep(1.0)
replay_final_stacking = []
I.replay_velocities(
robot,
task_velocities,
path=os.path.abspath(
os.path.join(
SAVE_PATH, 'VELOCITIES')))
for k in range(0, len(blocks), 2):
replay_final_stacking.append(
on_top(I, *blocks[k:k + 2]))
if all(replay_final_stacking):
#save
np.save(
os.path.join(
SAVE_PATH, 'VELOCITIES',
'velocities'),
np.array(I.target_vel))
np.save(
os.path.join(
SAVE_PATH, 'VELOCITIES',
'joints_configurations'),
np.array(I.joint_val))
trial += 1
else:
shutil.rmtree(SAVE_PATH)
else:
shutil.rmtree(SAVE_PATH)
break
else:
shutil.rmtree(SAVE_PATH)
if success:
break