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)