def init_grab(self, env):
self.path = CartesianPath.create(
show_line=True,
show_orientation=True,
show_position=True,
automatic_orientation=False,
keep_x_up=False) # first point is lasy to execute
np_elevated_final_point = np.add(self.np_pollo_target,
np.array([0.0, 0.0, 0.30]))
self.path.insert_control_points([
list(np_elevated_final_point) + list(self.np_robot_tip_orientation)
])
np_after_pollo = np.add(self.np_pollo_target,
np.array([0.0, 0.0, 0.06]))
self.path.insert_control_points(
[list(np_after_pollo) + list(self.np_robot_tip_orientation)])
np_predicted_pollo = np.add(self.np_pollo_target,
np.array([0.0, -0.10, 0.1]))
self.path.insert_control_points(
[list(np_predicted_pollo) + list(self.np_robot_tip_orientation)])
self.path.insert_control_points([
list(self.np_robot_tip_position) +
list(self.np_robot_tip_orientation)
])
try:
self.ang_path = env.agent.get_path_from_cartesian_path(self.path)
self.state = "GRAB"
except IKError as e:
print('Agent::grasp Could not find joint values')
self.state = "RESET_EPISODE"
def wait_for_chicken(self, env):
#print("WAIT_FOR_CHICKEN")
if time.time() - self.reloj > 6:
self.state = "RESET_EPISODE"
if np.fabs(self.np_pollo_target[0] -
self.np_robot_tip_position[0]) < 0.30:
self.state = "INIT_GRAB"
else:
env.target.set_position(list(self.np_pollo_target))
local_path = CartesianPath.create(
show_line=True,
show_orientation=True,
show_position=True,
automatic_orientation=True) # first point is lasy to execute
np_local_tip = self.np_robot_tip_position
np_local_tip[1] = self.np_pollo_target[1]
local_path.insert_control_points(
[list(np_local_tip) + env.agent.get_tip().get_orientation()])
local_path.insert_control_points([
list(self.np_robot_tip_position) +
env.agent.get_tip().get_orientation()
])
local_ang_path = env.agent.get_path_from_cartesian_path(local_path)
while not local_ang_path.step():
env.pr.step()
local_path.remove()
def wait_for_chicken(self, env):
if time.time() - self.reloj > 5:
self.state = "RESET_EPISODE"
if self.dist < 0.30:
self.state = "INIT_GRAB"
else:
local_path = CartesianPath.create(
show_line=True,
show_orientation=True,
show_position=True,
automatic_orientation=True) # first point is lasy to execute
np_local_tip = self.np_robot_tip_position
np_local_tip[0] = self.np_pollo_target[0]
#local_path.insert_control_points([list(np_local_tip) + list(self.np_robot_tip_orientation)])
local_path.insert_control_points(
[list(np_local_tip) + env.pollo_target.get_orientation()])
local_path.insert_control_points([
list(self.np_robot_tip_position) +
list(self.np_robot_tip_orientation)
])
local_ang_path = env.agent.get_path_from_cartesian_path(local_path)
while not local_ang_path.step():
env.pr.step()
#angles = env.agent.get_joint_positions()
#env.agent.joints[4].set_joint_target_position(env.pollo_target.get_orientation()[1])
local_path.remove()
def init_grasp(self, env):
np_pollo_target = np.array(env.pollo_target.get_position())
np_robot_tip_position = np.array(env.agent.get_tip().get_position())
np_robot_tip_orientation = np.array(env.agent.get_tip().get_orientation())
print("init ", np_robot_tip_orientation)
dist = np.linalg.norm(np_robot_tip_position - np_pollo_target)
c_path = CartesianPath.create()
# LIFO: goto table
c_path.insert_control_points([env.table_target.get_position() + list(np_robot_tip_orientation)])
at100 = np.add(np_pollo_target, np.array([0.0,0.0,0.30]))
c_path.insert_control_points([list(at100) + list(np_robot_tip_orientation)])
# c_path.insert_control_points([env.table_target.get_position() + list(np_robot_tip_orientation)])
# np_robot_tip_orientation[0] += 1
# c_path.insert_control_points([list(np_pollo_target) + list(np_robot_tip_orientation)])
# np_robot_tip_orientation[0] -= 1
np_pollo_target[1] -= 0.1
c_path.insert_control_points([list(np_pollo_target) + list(np_robot_tip_orientation)])
# at10 = np.add(np.array(c_path.get_pose_on_path(0.2)[0]), np.array([0.0,0.0,0.1]))
# c_path.insert_control_points([list(at10) + list(np_robot_tip_orientation)])
c_path.insert_control_points([list(np_robot_tip_position) + list(np_robot_tip_orientation)])
try:
#angles = env.agent.solve_ik(position=list(np_pollo_target), orientation=np_robot_tip_orientation)
self.path = env.agent.get_path_from_cartesian_path(c_path)
print("at init_grasp ")
except IKError as e:
print('Agent::grasp Could not find joint values')
self.state = "GRASP"
print("changing to GRASP")
return None
def step(self, action):
print(action.shape, action)
if action is None:
self.pr.step()
return self._get_state(), 0.0, False, {}
if np.any(np.isnan(action)):
print(action)
return self._get_state(), -1.0, False, {}
# create a path with tip and pollo at its endpoints and 5 adjustable points in the middle
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
np_robot_tip_orientation = np.array(self.agent.get_tip().get_orientation())
c_path = CartesianPath.create()
c_path.insert_control_points(action[4]) # point after pollo to secure the grasp
c_path.insert_control_points([list(np_pollo_target) + list(np_robot_tip_orientation)]) # pollo
c_path.insert_control_points(action[0:3])
c_path.insert_control_points([list(np_robot_tip_position) + list(np_robot_tip_orientation)]) # robot hand
try:
self.path = self.agent.get_path_from_cartesian_path(c_path)
except IKError as e:
print('Agent::grasp Could not find joint values')
return self._get_state(), -1, True, {}
# go through path
reloj = time.time()
while self.path.step():
self.pr.step() # Step the physics simulation
if (time.time()-reloj) > 4:
return self._get_state(), -10.0, True, {} # Too much time
if any((self.agent_hand.check_collision(obj) for obj in self.scene_objects)): # colisión con la mesa
return self._get_state(), -10.0, True, {}
# path ok, compute reward
np_pollo_target = np.array(self.pollo_target.get_position())
np_robot_tip_position = np.array(self.agent.get_tip().get_position())
dist = np.linalg.norm(np_pollo_target-np_robot_tip_position)
altura = np.clip((np_pollo_target[2] - self.initial_pollo_position[2]), 0, 0.5)
if altura > 0.3: # pollo en mano
return self._get_state(), altura, True, {}
elif dist > self.initial_distance: # mano se aleja
return self._get_state(), -10.0, True, {}
if time.time() - self.initial_epoch_time > 5: # too much time
return self._get_state(), -10.0, True, {}
# Reward
pollo_height = np.exp(-altura*10) # para 1m pollo_height = 0.001, para 0m pollo_height = 1
reward = - pollo_height - dist
return self._get_state(), reward, False, {}
def levantar(self, env):
print("LEVANTAR")
local_path = CartesianPath.create(
show_line=True,
show_orientation=True,
show_position=True,
automatic_orientation=True) # first point is lasy to execute
np_high_point = np.add(env.initial_agent_tip_position,
np.array([0.0, 0.0, 0.4]))
local_path.insert_control_points(
[list(np_high_point) + list(self.np_robot_tip_orientation)])
local_path.insert_control_points([
list(self.np_robot_tip_position) +
list(self.np_robot_tip_orientation)
])
local_ang_path = env.agent.get_path_from_cartesian_path(local_path)
env.target.set_position(list(self.np_pollo_target))
while not local_ang_path.step():
pass
local_path.remove()
self.reloj = time.time()
self.state = "DEJAR"
def _update_path_visualization_pyrep(self, multi_spline_points, multi_spline_sdf_vals):
if len(self._spline_paths) > 0:
for spline_path_segs in self._spline_paths:
for spline_path_seg in spline_path_segs:
spline_path_seg.remove()
self._spline_paths.clear()
for spline_points, sdf_vals in zip(multi_spline_points, multi_spline_sdf_vals):
sdf_flags_0 = sdf_vals[1:, 0] > 0
sdf_flags_1 = sdf_vals[:-1, 0] > 0
sdf_borders = sdf_flags_1 != sdf_flags_0
borders_indices = ivy.indices_where(sdf_borders)
if borders_indices.shape[0] != 0:
to_concat = (ivy.array([0], 'int32'), ivy.cast(borders_indices, 'int32')[:, 0],
ivy.array([-1], 'int32'))
else:
to_concat = (ivy.array([0], 'int32'), ivy.array([-1], 'int32'))
border_indices = ivy.concatenate(to_concat, 0)
num_groups = border_indices.shape[0] - 1
spline_path = list()
for i in range(num_groups):
border_idx_i = int(ivy.to_numpy(border_indices[i]).item())
border_idx_ip1 = int(ivy.to_numpy(border_indices[i + 1]).item())
if i < num_groups - 1:
control_group = spline_points[border_idx_i:border_idx_ip1]
sdf_group = sdf_vals[border_idx_i:border_idx_ip1]
else:
control_group = spline_points[border_idx_i:]
sdf_group = sdf_vals[border_idx_i:]
num_points = control_group.shape[0]
orientation_zeros = np.zeros((num_points, 3))
color = (0.2, 0.8, 0.2) if sdf_group[-1] > 0 else (0.8, 0.2, 0.2)
control_poses = np.concatenate((ivy.to_numpy(control_group), orientation_zeros), -1)
spline_path_seg = CartesianPath.create(show_orientation=False, show_position=False, line_size=8,
path_color=color)
spline_path_seg.insert_control_points(control_poses.tolist())
spline_path.append(spline_path_seg)
self._spline_paths.append(spline_path)
def hangup(self, env):
local_path = CartesianPath.create(show_line=True,
show_orientation=True,
show_position=True,
automatic_orientation=False)
local_path.insert_control_points([
env.waypoints[0].get_position() +
env.waypoints[0].get_orientation()
])
local_path.insert_control_points([
env.waypoints[1].get_position() +
env.waypoints[1].get_orientation()
])
local_path.insert_control_points([
env.waypoints[2].get_position() +
env.waypoints[2].get_orientation()
])
local_path.insert_control_points([
env.waypoints[3].get_position() +
env.waypoints[3].get_orientation()
])
local_path.insert_control_points([
list(self.np_robot_tip_position) +
list(self.np_robot_tip_orientation)
])
local_ang_path = env.agent.get_path_from_cartesian_path(local_path)
local_ang_path.visualize() # Let's see what the path looks like
print('Executing plan ...')
while not local_ang_path.step():
env.pr.step()
local_path.remove()
# path = env.agent.get_path(position=env.waypoints[0].get_position(),
# quaternion=env.waypoints[0].get_quaternion())
local_ang_path.clear_visualization()
self.state = "WAIT"
def init_grab(self, env):
print("INIT_GRAB")
self.path = CartesianPath.create(
show_line=True,
show_orientation=True,
show_position=True,
automatic_orientation=False) # first point is lasy to execute
np_predicted_pollo = np.add(self.np_pollo_target,
np.array([-0.24, 0.05, -0.05]))
#print(self.np_robot_tip_orientation, env.initial_agent_tip_euler)
self.path.insert_control_points(
[list(np_predicted_pollo) + list(env.initial_agent_tip_euler)])
self.path.insert_control_points([
list(self.np_robot_tip_position) +
list(self.np_robot_tip_orientation)
])
env.pr.script_call('activate_suction@suctionPad',
env.vrep.sim_scripttype_childscript)
try:
self.ang_path = env.agent.get_path_from_cartesian_path(self.path)
self.state = "GRAB"
except IKError as e:
print('Agent::grasp Could not find joint values')
self.state = "RESET_EPISODE"
def test_create_cartesian_path(self):
p = CartesianPath.create()
self.assertIsInstance(p, CartesianPath)
