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 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 get_path_from_cartesian_path(
self, path: CartesianPath) -> ArmConfigurationPath:
"""Translate a path from cartesian space, to arm configuration space.
Note: It must be possible to reach the start of the path via a linear
path, otherwise an error will be raised.
:param path: A :py:class:`CartesianPath` instance to be translated to
a configuration-space path.
:raises: ConfigurationPathError if no path could be created.
:return: A path in the arm configuration space.
"""
handles = [j.get_handle() for j in self.joints]
_, ret_floats, _, _ = utils.script_call(
'getPathFromCartesianPath@PyRep',
PYREP_SCRIPT_TYPE,
ints=[
path.get_handle(), self._ik_group,
self._ik_target.get_handle()
] + handles)
if len(ret_floats) == 0:
raise ConfigurationPathError(
'Could not create a path from cartesian path.')
return ArmConfigurationPath(self, ret_floats)
def to_type(handle: int) -> Object:
"""Converts an object handle to the correct sub-type.
:param handle: The internal handle of an object.
:return: The sub-type of this object.
"""
t = sim.simGetObjectType(handle)
if t == sim.sim_object_shape_type:
return Shape(handle)
elif t == sim.sim_object_dummy_type:
return Dummy(handle)
elif t == sim.sim_object_path_type:
return CartesianPath(handle)
elif t == sim.sim_object_joint_type:
return Joint(handle)
elif t == sim.sim_object_visionsensor_type:
return VisionSensor(handle)
elif t == sim.sim_object_forcesensor_type:
return ForceSensor(handle)
elif t == sim.sim_object_proximitysensor_type:
return ProximitySensor(handle)
elif t == sim.sim_object_camera_type:
return Camera(handle)
elif t == sim.sim_object_octree_type:
return Octree(handle)
raise ValueError
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()
class TestCartesianPaths(TestCore):
def setUp(self):
super().setUp()
self.cart_path = CartesianPath('cartesian_path')
def test_create_cartesian_path(self):
p = CartesianPath.create()
self.assertIsInstance(p, CartesianPath)
def test_get_pose_on_path(self):
pos, ori = self.cart_path.get_pose_on_path(0.5)
self.assertEqual(len(pos), 3)
self.assertEqual(len(ori), 3)
def test_insert_control_points(self):
points = [[0.1] * 6]
# Just check that it does not throw an exception.
self.cart_path.insert_control_points(points)
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 _get_waypoints(self, validating=False) -> List[Waypoint]:
waypoint_name = 'waypoint%d'
waypoints = []
additional_waypoint_inits = []
i = 0
while True:
name = waypoint_name % i
if not Object.exists(name):
# There are no more waypoints...
break
ob_type = Object.get_object_type(name)
way = None
if ob_type == ObjectType.DUMMY:
waypoint = Dummy(name)
start_func = None
end_func = None
if i in self._waypoint_abilities_start:
start_func = self._waypoint_abilities_start[i]
if i in self._waypoint_abilities_end:
end_func = self._waypoint_abilities_end[i]
way = Point(waypoint,
self.robot,
start_of_path_func=start_func,
end_of_path_func=end_func)
elif ob_type == ObjectType.PATH:
cartestian_path = CartesianPath(name)
way = PredefinedPath(cartestian_path, self.robot)
else:
raise WaypointError(
'%s is an unsupported waypoint type %s' % (name, ob_type),
self)
if name in self._waypoint_additional_inits and not validating:
additional_waypoint_inits.append(
(self._waypoint_additional_inits[name], way))
waypoints.append(way)
i += 1
# Check if all of the waypoints are feasible
feasible, way_i = self._feasible(waypoints)
if not feasible:
raise WaypointError(
"Infeasible episode. Can't reach waypoint %d." % way_i, self)
for func, way in additional_waypoint_inits:
func(way)
return waypoints
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_get_path_from_cartesian_path(self):
arm = Panda()
cartesian_path = CartesianPath('Panda_cartesian_path')
path = arm.get_path_from_cartesian_path(cartesian_path)
self.assertIsNotNone(path)
def setUp(self):
super().setUp()
self.cart_path = CartesianPath('cartesian_path')
def test_create_cartesian_path(self):
p = CartesianPath.create()
self.assertIsInstance(p, CartesianPath)
def test_get_cartesian_path(self):
cube = CartesianPath('cartesian_path')
self.assertIsInstance(cube, CartesianPath)
