class Arm(RobotComponent): """Base class representing a robot arm with path planning support. """ def __init__(self, count: int, name: str, num_joints: int, base_name: str = None, max_velocity=1.0, max_acceleration=4.0, max_jerk=1000): """Count is used for when we have multiple copies of arms""" joint_names = ['%s_joint%d' % (name, i + 1) for i in range(num_joints)] super().__init__(count, name, joint_names, base_name) # Used for motion planning self.max_velocity = max_velocity self.max_acceleration = max_acceleration self.max_jerk = max_jerk # Motion planning handles suffix = '' if count == 0 else '#%d' % (count - 1) self._ik_target = Dummy('%s_target%s' % (name, suffix)) self._ik_tip = Dummy('%s_tip%s' % (name, suffix)) self._ik_group = vrep.simGetIkGroupHandle('%s_ik%s' % (name, suffix)) self._collision_collection = vrep.simGetCollectionHandle( '%s_arm%s' % (name, suffix)) def get_configs_for_tip_pose(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, ignore_collisions=False, trials=300, max_configs=60) -> List[List[float]]: """Gets a valid joint configuration for a desired end effector pose. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs :param max_configs: The maximum number of configurations we want to generate before ranking them. :raises: ConfigurationError if no joint configuration could be found. :return: A list of valid joint configurations for the desired end effector pose. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) handles = [j.get_handle() for j in self.joints] # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'findSeveralCollisionFreeConfigsAndCheckApproach@PyRep', PYREP_SCRIPT_TYPE, ints=[ self._ik_group, self._collision_collection, int(ignore_collisions), trials, max_configs ] + handles) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationError( 'Could not find a valid joint configuration for desired end effector pose.' ) num_configs = int(len(ret_floats) / len(handles)) return [[ ret_floats[len(handles) * i + j] for j in range(len(handles)) ] for i in range(num_configs)] def solve_ik(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None) -> List[float]: """Solves an IK group and returns the calculated joint values. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :return: A list containing the calculated joint values. """ self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) ik_result, joint_values = vrep.simCheckIkGroup( self._ik_group, [j.get_handle() for j in self.joints]) if ik_result == vrep.sim_ikresult_fail: raise IKError( 'IK failed. Perhaps the distance was between the tip ' ' and target was too large.') elif ik_result == vrep.sim_ikresult_not_performed: raise IKError('IK not performed.') return joint_values 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 get_linear_path(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, steps=50, ignore_collisions=False) -> ArmConfigurationPath: """Gets a linear configuration path given a target pose. Generates a path that drives a robot from its current configuration to its target dummy in a straight line (i.e. shortest path in Cartesian space). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param steps: The desired number of path points. Each path point contains a robot configuration. A minimum of two path points is required. If the target pose distance is large, a larger number of steps leads to better results for this function. :param ignore_collisions: If collision checking should be disabled. :raises: ConfigurationPathError if no path could be created. :return: A linear path in the arm configuration space. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) handles = [j.get_handle() for j in self.joints] # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'getLinearPath@PyRep', PYREP_SCRIPT_TYPE, ints=[ steps, self._ik_group, self._collision_collection, int(ignore_collisions) ] + handles) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_nonlinear_path(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, ignore_collisions=False, trials=100, max_configs=60, trials_per_goal=6, algorithm=Algos.SBL) -> ArmConfigurationPath: """Gets a non-linear (planned) configuration path given a target pose. A path is generated by finding several configs for a pose, and ranking them according to the distance in configuration space (smaller is better). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs :param max_configs: The maximum number of configurations we want to generate before ranking them. :param trials_per_goal: The number of paths per config we want to trial. :param algorithm: The algorithm for path planning to use. :raises: ConfigurationPathError if no path could be created. :return: A non-linear path in the arm configuration space. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position) if euler is not None: self._ik_target.set_orientation(euler) elif quaternion is not None: self._ik_target.set_quaternion(quaternion) handles = [j.get_handle() for j in self.joints] # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'getNonlinearPath@PyRep', PYREP_SCRIPT_TYPE, ints=[ self._ik_group, self._collision_collection, int(ignore_collisions), trials, max_configs, trials_per_goal ] + handles, strings=[algorithm.value]) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_path(self, position: List[float], euler: List[float] = None, quaternion: List[float] = None, ignore_collisions=False, trials=100, max_configs=60, trials_per_goal=6, algorithm=Algos.SBL) -> ArmConfigurationPath: """Tries to get a linear path, failing that tries a non-linear path. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. (Only applicable if a non-linear path is needed) :param max_configs: The maximum number of configurations we want to generate before ranking them. (Only applicable if a non-linear path is needed) :param trials_per_goal: The number of paths per config we want to trial. (Only applicable if a non-linear path is needed) :param algorithm: The algorithm for path planning to use. (Only applicable if a non-linear path is needed) :raises: ConfigurationPathError if neither a linear or non-linear path can be created. :return: A linear or non-linear path in the arm configuration space. """ try: p = self.get_linear_path(position, euler, quaternion, ignore_collisions=ignore_collisions) return p except ConfigurationPathError: pass # Allowed. Try again, but with non-linear. # This time if an exception is thrown, we dont want to catch it. p = self.get_nonlinear_path(position, euler, quaternion, ignore_collisions, trials, max_configs, trials_per_goal, algorithm) return p def get_tip(self) -> Dummy: """Gets the tip of the arm. Each arm is required to have a tip for path planning. :return: The tip of the arm. """ return self._ik_tip def get_jacobian(self): """Calculates the Jacobian. :return: the row-major Jacobian matix. """ self._ik_target.set_matrix(self._ik_tip.get_matrix()) vrep.simCheckIkGroup(self._ik_group, [j.get_handle() for j in self.joints]) jacobian, (rows, cols) = vrep.simGetIkGroupMatrix(self._ik_group, 0) jacobian = np.array(jacobian).reshape((rows, cols), order='F') return jacobian
class TestObjects(TestCore): def setUp(self): super().setUp() self.dynamic_cube = Shape('dynamic_cube') self.cubes_under_dummy = Dummy('cubes_under_dummy') self.cube0 = Shape('cube0') self.dummy = Dummy('dummy') self.simple_model = Shape('simple_model') def test_get_object_type(self): self.assertEqual(Object.get_object_type('dynamic_cube'), ObjectType.SHAPE) self.assertEqual(Object.get_object_type('dummy'), ObjectType.DUMMY) def test_get_object_name(self): self.assertEqual(Object.get_object_name('dynamic_cube'), 'dynamic_cube') self.assertEqual( Object.get_object_name(self.dynamic_cube.get_handle()), 'dynamic_cube') def test_get_object(self): self.assertEqual(Object.get_object('dynamic_cube'), self.dynamic_cube) self.assertEqual(Object.get_object('dummy'), self.dummy) self.assertEqual(Object.get_object(self.dynamic_cube.get_handle()), self.dynamic_cube) self.assertEqual(Object.get_object(self.dummy.get_handle()), self.dummy) def test_equality(self): cube2 = Shape('dynamic_cube') self.assertEqual(self.dynamic_cube, cube2) def test_get_handle(self): self.assertGreater(self.dynamic_cube.get_handle(), 0) def test_get_type(self): self.assertEqual(self.dynamic_cube.get_type(), ObjectType.SHAPE) def test_still_exists(self): self.assertTrue(self.dynamic_cube.still_exists()) self.dynamic_cube.remove() self.assertFalse(self.dynamic_cube.still_exists()) def test_object_exists(self): yes = Object.exists('dynamic_cube') no = Object.exists('dynamic_cubeee') self.assertTrue(yes) self.assertFalse(no) def test_get_set_name(self): self.dynamic_cube.set_name('test1') self.assertEqual(self.dynamic_cube.get_name(), 'test1') def test_get_set_position(self): position = self.cube0.get_position() self.assertIsInstance(position, np.ndarray) self.assertEqual(position.shape, (3, )) self.cube0.set_position([0.1, 0.1, 0.1], self.cubes_under_dummy) self.assertTrue( np.allclose(self.cube0.get_position(self.cubes_under_dummy), [0.1, 0.1, 0.1])) self.cube0.set_position([0.2, 0.2, 0.2]) self.assertTrue(np.allclose(self.cube0.get_position(), [0.2, 0.2, 0.2])) def test_get_set_orientation(self): orientation = self.cube0.get_orientation() self.assertIsInstance(orientation, np.ndarray) self.assertEqual(orientation.shape, (3, )) self.cube0.set_orientation([0.0, 0.0, 0.2], self.cubes_under_dummy) self.assertTrue( np.allclose(self.cube0.get_orientation(self.cubes_under_dummy), [0.0, 0.0, 0.2])) self.cube0.set_orientation([0.0, 0.0, 0.3]) self.assertTrue( np.allclose(self.cube0.get_orientation(), [0.0, 0.0, 0.3])) def test_get_set_quaternion(self): quaternion = self.cube0.get_quaternion() self.assertIsInstance(quaternion, np.ndarray) self.assertEqual(quaternion.shape, (4, )) # x, y, z, w self.cube0.set_quaternion([1., 0., 0., 0.], self.cubes_under_dummy) self.assertTrue( np.allclose(self.cube0.get_quaternion(self.cubes_under_dummy), [1., 0., 0., 0.])) self.cube0.set_quaternion([np.sqrt(0.5), 0, 0., np.sqrt(0.5)]) self.assertTrue( np.allclose( self.cube0.get_quaternion(), [np.sqrt(0.5), 0, 0., np.sqrt(0.5)])) def test_get_velocity(self): linear_vel, angular_vel = self.cube0.get_velocity() self.assertIsInstance(linear_vel, np.ndarray) self.assertEqual(linear_vel.shape, (3, )) self.assertIsInstance(angular_vel, np.ndarray) self.assertEqual(angular_vel.shape, (3, )) def test_get_set_parent(self): self.dynamic_cube.set_parent(self.dummy) parent = self.dynamic_cube.get_parent() self.assertEqual(parent, self.dummy) def test_get_set_parent_not_in_place(self): init_pos = self.dynamic_cube.get_position() self.dynamic_cube.set_parent(self.dummy, keep_in_place=False) parent = self.dynamic_cube.get_parent() self.assertEqual(parent, self.dummy) self.assertFalse( np.allclose(init_pos, self.dynamic_cube.get_position())) def test_get_parent_when_orphan(self): parent = self.dummy.get_parent() self.assertIsNone(parent) def test_get_matrix(self): self.assertEqual(len(self.dynamic_cube.get_matrix()), 12) def test_get_set_collidable(self): self.dynamic_cube.set_collidable(False) self.assertFalse(self.dynamic_cube.is_collidable()) self.dynamic_cube.set_collidable(True) self.assertTrue(self.dynamic_cube.is_collidable()) def test_get_set_measurable(self): self.dynamic_cube.set_measurable(False) self.assertFalse(self.dynamic_cube.is_measurable()) self.dynamic_cube.set_measurable(True) self.assertTrue(self.dynamic_cube.is_measurable()) def test_get_set_detectable(self): self.dynamic_cube.set_detectable(False) self.assertFalse(self.dynamic_cube.is_detectable()) self.dynamic_cube.set_detectable(True) self.assertTrue(self.dynamic_cube.is_detectable()) def test_get_set_renderable(self): self.dynamic_cube.set_renderable(False) self.assertFalse(self.dynamic_cube.is_renderable()) self.dynamic_cube.set_renderable(True) self.assertTrue(self.dynamic_cube.is_renderable()) def test_is_model(self): self.assertFalse(self.dynamic_cube.is_model()) self.assertTrue(self.simple_model.is_model()) def test_set_model(self): self.simple_model.set_model(False) self.dynamic_cube.set_model(True) self.assertFalse(self.simple_model.is_model()) self.assertTrue(self.dynamic_cube.is_model()) def test_remove(self): self.dynamic_cube.remove() self.simple_model.remove() self.assertFalse(self.dynamic_cube.still_exists()) self.assertFalse(self.simple_model.still_exists()) self.assertFalse(Object.exists('dynamic_cube')) self.assertFalse(Object.exists('simple_model')) def test_dynamic_object(self): # Can't really test this. So lets just make sure it doesn't error self.dynamic_cube.reset_dynamic_object() def test_get_bounding_box(self): bb = self.dynamic_cube.get_bounding_box() self.assertTrue(np.allclose(bb, [-0.05, 0.05] * 3)) def test_get_objects_in_tree(self): dummys = [Dummy('nested_dummy%d' % i) for i in range(3)] objects = dummys[0].get_objects_in_tree(exclude_base=False, first_generation_only=False) self.assertListEqual(objects, dummys) for obj in objects: self.assertIs(type(obj), Dummy) self.assertListEqual( dummys[0].get_objects_in_tree(exclude_base=True, first_generation_only=False), dummys[1:]) self.assertListEqual( dummys[0].get_objects_in_tree(exclude_base=False, first_generation_only=True), dummys[:-1]) def test_get_extention_string(self): self.assertEqual(self.dynamic_cube.get_extension_string(), 'test') def test_get_configuration_tree(self): config = self.dynamic_cube.get_configuration_tree() self.assertIsNotNone(config) def test_rotate(self): self.dynamic_cube.rotate([0.02, 0.04, 0.06]) self.assertTrue( np.allclose(self.dynamic_cube.get_orientation(), [0.02, 0.04, 0.06])) def test_get_set_model_collidable(self): self.simple_model.set_model_collidable(False) self.assertFalse(self.simple_model.is_model_collidable()) self.simple_model.set_model_collidable(True) self.assertTrue(self.simple_model.is_model_collidable()) def test_get_set_model_measurable(self): self.simple_model.set_model_measurable(False) self.assertFalse(self.simple_model.is_model_measurable()) self.simple_model.set_model_measurable(True) self.assertTrue(self.simple_model.is_model_measurable()) def test_get_set_model_detectable(self): self.simple_model.set_model_detectable(False) self.assertFalse(self.simple_model.is_model_detectable()) self.simple_model.set_model_detectable(True) self.assertTrue(self.simple_model.is_model_detectable()) def test_get_set_model_renderable(self): self.simple_model.set_model_renderable(False) self.assertFalse(self.simple_model.is_model_renderable()) self.simple_model.set_model_renderable(True) self.assertTrue(self.simple_model.is_model_renderable()) def test_get_set_model_dynamic(self): self.simple_model.set_model_dynamic(False) self.assertFalse(self.simple_model.is_model_dynamic()) self.simple_model.set_model_dynamic(True) self.assertTrue(self.simple_model.is_model_dynamic()) def test_get_set_model_respondable(self): self.simple_model.set_model_respondable(False) self.assertFalse(self.simple_model.is_model_respondable()) self.simple_model.set_model_respondable(True) self.assertTrue(self.simple_model.is_model_respondable()) def test_check_collision(self): c1 = Shape('colliding_cube0') c2 = Shape('colliding_cube1') self.assertTrue(c1.check_collision(c2)) def test_check_collision_all(self): c1 = Shape('colliding_cube0') self.assertTrue(c1.check_collision(None)) def test_copy(self): cube1 = self.cube0.copy() self.assertGreater(cube1.get_handle(), 0) self.assertIsInstance(cube1, Shape) self.assertNotEqual(self.cube0, cube1) def test_check_distance(self): dist = self.dummy.check_distance(self.cube0) self.assertAlmostEqual(dist, 1.4629, places=3) def test_set_get_bullet_friction(self): self.dynamic_cube.set_bullet_friction(0.7) friction = self.dynamic_cube.get_bullet_friction() self.assertAlmostEqual(friction, 0.7, places=1) def test_set_get_explicit_handling(self): cam = VisionSensor.create((640, 480)) flag_orig = cam.get_explicit_handling() cam.set_explicit_handling(value=1) flag = cam.get_explicit_handling() self.assertEqual(flag, 1) cam.set_explicit_handling(value=0) flag = cam.get_explicit_handling() self.assertEqual(flag, 0) cam.set_explicit_handling(flag_orig) cam.remove()
class PioneerP3dx(RobotComponent): """Base class representing a robot mobile base with path planning support. """ def __init__(self, count: int, num_wheels: int, num_sensors: int, name: str, sensor_name: str, laser_name: str): """Count is used for when we have multiple copies of mobile bases. :param count: used for multiple copies of robots :param num_wheels: number of actuated wheels :param num_laser: number of lasers :param name: string with robot name (same as base in vrep model). """ joint_names = [ '%s_m_joint%s' % (name, str(i + 1)) for i in range(num_wheels) ] super().__init__(count, name, joint_names) self.num_wheels = num_wheels suffix = '' if count == 0 else '#%d' % (count - 1) wheel_names = [ '%s_wheel%s%s' % (name, str(i + 1), suffix) for i in range(self.num_wheels) ] self.wheels = [Shape(name) for name in wheel_names] #joint_slipping_names = [ # '%s_slipping_m_joint%s%s' % (name, str(i + 1), suffix) for i in # range(self.num_wheels)] #self.joints_slipping = [Joint(jsname) # for jsname in joint_slipping_names] self.num_sensors = num_sensors sensor_names = [ '%s_%s%s' % (name, sensor_name, str(i + 1)) for i in range(self.num_sensors) ] self.sensors = [ProximitySensor(name) for name in sensor_names] self.lasers = Laser(laser_name, '') # Motion planning handles self.intermediate_target_base = Dummy('%s_intermediate_target_base%s' % (name, suffix)) self.target_base = Dummy('%s_target_base%s' % (name, suffix)) self._collision_collection = vrep.simGetCollectionHandle('Robot') self.target_z = self.target_base.get_position()[-1] # Robot parameters and handle self.z_pos = self.get_position()[2] #self.target_z = self.target_base.get_position()[-1] self.wheel_radius = self.wheels[0].get_bounding_box()[1] self.wheel_distance = np.linalg.norm( np.array(self.wheels[0].get_position()) - np.array(self.wheels[1].get_position())) # Make sure dummies are orphan if loaded with ttm self.intermediate_target_base.set_parent(None) self.target_base.set_parent(None) self.cummulative_error = 0 self.prev_error = 0 # PID controller values. # TODO: expose to user through constructor. self.Kp = 0.1 self.Ki = 0.0 self.Kd = 0.0 self.desired_velocity = 0.1 self._path_done = False self.i_path = -1 self.inter_done = True self.drawing_handle = vrep.simAddDrawingObject( objectType=vrep.sim_drawing_points, size=10, duplicateTolerance=0, parentObjectHandle=-1, maxItemCount=99999, ambient_diffuse=[1, 0, 0]) #def get_wheel_radius(self): # min_x = vrep.simGetObjectFloatParameter(self.wheels[0]._handle, 15) # max_x = vrep.simGetObjectFloatParameter(self.wheels[0]._handle, 18) # return (max_x - min_x)/2. def get_velocity(self) -> List[float]: #print(self.wheel_radius) joint_velocities = self.get_joint_velocities() v_l = joint_velocities[0] v_r = joint_velocities[1] #print('get_velocity') #print(v_l) #print(v_r) v = self.wheel_radius / 2. * (v_l + v_r) omega = self.wheel_radius / self.wheel_distance * (v_r - v_l) return [v, omega] def set_base_angular_velocites(self, velocity: List[float]): v = velocity[0] omega = velocity[1] vr = ((2. * v + omega * self.wheel_distance) / (2. * self.wheel_radius)) vl = ((2. * v - omega * self.wheel_distance) / (2. * self.wheel_radius)) self.set_joint_target_velocities([vl, vr]) def get_sensor_data(self): data = [] for i in range(self.num_sensors): ret, detected_point = self.sensors[i].get_sensor_data() dist = np.linalg.norm(detected_point) data.append(ret) data.append(dist) return data def get_2d_pose(self) -> List[float]: """Gets the 2D (top-down) pose of the robot [x, y, yaw]. :return: A List containing the x, y, yaw (in radians). """ return (self.get_position()[:2] + self.get_orientation()[-1:]) def set_2d_pose(self, pose: List[float]) -> None: """Sets the 2D (top-down) pose of the robot [x, y, yaw] :param pose: A List containing the x, y, yaw (in radians). """ x, y, yaw = pose self.set_position([x, y, self.z_pos]) self.set_orientation([0, 0, yaw]) def assess_collision(self): """Silent detection of the robot base with all other entities present in the scene. :return: True if collision is detected """ return vrep.simCheckCollision(self._collision_collection, vrep.sim_handle_all) == 1 def get_nonlinear_path_points( self, position: List[float], angle=0, boundaries=8, path_pts=600, ignore_collisions=False, algorithm=Algos.RRTConnect) -> List[List[float]]: """Gets a non-linear (planned) configuration path given a target pose. :param position: The x, y, z position of the target. :param angle: The z orientation of the target (in radians). :param boundaries: A float defining the path search in x and y direction [[-boundaries,boundaries],[-boundaries,boundaries]]. :param path_pts: number of sampled points returned from the computed path :param ignore_collisions: If collision checking should be disabled. :param algorithm: Algorithm used to compute path :raises: ConfigurationPathError if no path could be created. :return: A non-linear path (x,y,angle) in the xy configuration space. """ # Base dummy required to be parent of the robot tree # self.base_ref.set_parent(None) # self.set_parent(self.base_ref) # Missing the dist1 for intermediate target self.target_base.set_position( [position[0], position[1], self.target_z]) self.target_base.set_orientation([0, 0, angle]) handle_base = self.get_handle() handle_target_base = self.target_base.get_handle() # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'getNonlinearPathMobile@PyRep', PYREP_SCRIPT_TYPE, ints=[ handle_base, handle_target_base, self._collision_collection, int(ignore_collisions), path_pts ], floats=[boundaries], strings=[algorithm.value]) # self.set_parent(None) # self.base_ref.set_parent(self) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') path = [] for i in range(0, len(ret_floats) // 3): inst = ret_floats[3 * i:3 * i + 3] if i > 0: dist_change = sqrt((inst[0] - prev_inst[0])**2 + (inst[1] - prev_inst[1])**2) else: dist_change = 0 inst.append(dist_change) path.append(inst) prev_inst = inst self._path_points = path self._set_inter_target(0) return path def visualize(self, path_points) -> None: """Draws a visualization of the path in the scene. The visualization can be removed with :py:meth:`ConfigurationPath.clear_visualization`. """ if len(self._path_points) <= 0: raise RuntimeError("Can't visualise a path with no points.") self._drawing_handle = vrep.simAddDrawingObject( objectType=vrep.sim_drawing_lines, size=3, duplicateTolerance=0, parentObjectHandle=-1, maxItemCount=99999, ambient_diffuse=[1, 0, 1]) vrep.simAddDrawingObjectItem(self._drawing_handle, None) init_pose = self.get_2d_pose() self.set_2d_pose(self._path_points[0][:3]) prev_point = self.get_position() for i in range(len(self._path_points)): points = self._path_points[i] self.set_2d_pose(points[:3]) p = self.get_position() vrep.simAddDrawingObjectItem(self._drawing_handle, prev_point + p) prev_point = p # Set the arm back to the initial config self.set_2d_pose(init_pose[:3]) def get_base_actuation(self): """A controller using PID. :return: A list with left and right joint velocity, and bool if target is reached. """ d_x, d_y, _ = self.intermediate_target_base.get_position( relative_to=self) #print('dx and dy') print(d_x) print(d_y) d_x_final, d_y_final, _ = self.target_base.get_position( relative_to=self) if sqrt((d_x_final)**2 + (d_y_final)**2) < 1.0: return [0., 0.] alpha = atan2(d_y, d_x) e = atan2(sin(alpha), cos(alpha)) e_P = e e_I = self.cummulative_error + e e_D = e - self.prev_error w = self.Kp * e_P + self.Ki * e_I + self.Kd * e_D w = atan2(sin(w), cos(w)) self.cummulative_error = self.cummulative_error + e self.prev_error = e return [0.1, 0] def step(self) -> bool: #print(self._path_points[0][0], self._path_points[0][1]) pos_inter = self.intermediate_target_base.get_position( relative_to=None) vrep.simAddDrawingObjectItem(self.drawing_handle, pos_inter[:2] + [0.0]) pos_inter = self.intermediate_target_base.get_position( relative_to=self) #print(pos_inter) if self.inter_done: self._next_i_path() self._set_inter_target(self.i_path) self.inter_done = False if sqrt((pos_inter[0])**2 + (pos_inter[1])**2) < 0.1: self.inter_done = True #actuation, _ = self.get_base_actuation() #else: #actuation, _ = self.get_base_actuation() #self._mobile.set_joint_target_velocities(actuation) #if self.i_path == len(self._path_points) - 1: # self._path_done = True #actuation, self._path_done = self._mobile.get_base_actuation() #self._mobile.set_joint_target_velocities(actuation) return self.get_base_actuation(), False def _set_inter_target(self, i): self.intermediate_target_base.set_position( [self._path_points[i][0], self._path_points[i][1], self.target_z]) self.intermediate_target_base.set_orientation( [0, 0, self._path_points[i][2]]) def _next_i_path(self): incr = 0.01 dist_to_next = 0 while dist_to_next < incr: self.i_path += 1 if self.i_path == len(self._path_points) - 1: self.i_path = len(self._path_points) - 1 break dist_to_next += self._path_points[self.i_path][-1]
class MobileBase(RobotComponent): """Base class representing a robot mobile base with path planning support. """ def __init__(self, count: int, num_wheels: int, name: str): """Count is used for when we have multiple copies of mobile bases. :param count: used for multiple copies of robots :param num_wheels: number of actuated wheels :param name: string with robot name (same as base in vrep model). """ joint_names = [ '%s_m_joint%s' % (name, str(i + 1)) for i in range(num_wheels) ] super().__init__(count, name, joint_names) self.num_wheels = num_wheels suffix = '' if count == 0 else '#%d' % (count - 1) wheel_names = [ '%s_wheel%s%s' % (name, str(i + 1), suffix) for i in range(self.num_wheels) ] self.wheels = [Shape(name) for name in wheel_names] # Motion planning handles self.intermediate_target_base = Dummy('%s_intermediate_target_base%s' % (name, suffix)) self.target_base = Dummy('%s_target_base%s' % (name, suffix)) self._collision_collection = sim.simGetCollectionHandle('%s_base%s' % (name, suffix)) # Robot parameters and handle self.z_pos = self.get_position()[2] self.target_z = self.target_base.get_position()[-1] self.wheel_radius = self.wheels[0].get_bounding_box()[5] # Z self.wheel_distance = np.linalg.norm( np.array(self.wheels[0].get_position()) - np.array(self.wheels[1].get_position())) # Make sure dummies are orphan if loaded with ttm self.intermediate_target_base.set_parent(None) self.target_base.set_parent(None) def get_2d_pose(self) -> np.ndarray: """Gets the 2D (top-down) pose of the robot [x, y, yaw]. :return: A List containing the x, y, yaw (in radians). """ return np.r_[self.get_position()[:2], self.get_orientation()[-1:]] def set_2d_pose(self, pose: Union[List[float], np.ndarray]) -> None: """Sets the 2D (top-down) pose of the robot [x, y, yaw] :param pose: A List containing the x, y, yaw (in radians). """ x, y, yaw = pose self.set_position([x, y, self.z_pos]) self.set_orientation([0, 0, yaw]) def assess_collision(self): """Silent detection of the robot base with all other entities present in the scene. :return: True if collision is detected """ return sim.simCheckCollision(self._collision_collection, sim.sim_handle_all) == 1 def set_cartesian_position(self, position: List[float]): """Set a delta target position (x,y) and rotation position :param position: length 3 list containing x and y position, and angle position NOTE: not supported for nonholonomic mobile bases. """ vel = [0, 0, 0] vel[-1] = position[-1] for i in range(2): vel[i] = position[i] / (0.05 * self.wheel_radius / 2 ) # "0.05 is dt" self.set_base_angular_velocites(vel) def set_base_angular_velocites(self, velocity: List[float]): """This function has no effect for two_wheels robot. More control is required for omnidirectional robot. :param velocity: for two wheels robot: each wheel velocity, for omnidirectional robot forwardBackward, leftRight and rotation velocity """ raise NotImplementedError() def _get_nonlinear_path_points( self, position: List[float], angle=0, boundaries=2, path_pts=600, ignore_collisions=False, algorithm=Algos.RRTConnect) -> List[List[float]]: """Gets a non-linear (planned) configuration path given a target pose. :param position: The x, y, z position of the target. :param angle: The z orientation of the target (in radians). :param boundaries: A float defining the path search in x and y direction [[-boundaries,boundaries],[-boundaries,boundaries]]. :param path_pts: number of sampled points returned from the computed path :param ignore_collisions: If collision checking should be disabled. :param algorithm: Algorithm used to compute path :raises: ConfigurationPathError if no path could be created. :return: A non-linear path (x,y,angle) in the xy configuration space. """ # Base dummy required to be parent of the robot tree # self.base_ref.set_parent(None) # self.set_parent(self.base_ref) # Missing the dist1 for intermediate target self.target_base.set_position( [position[0], position[1], self.target_z]) self.target_base.set_orientation([0, 0, angle]) handle_base = self.get_handle() handle_target_base = self.target_base.get_handle() # Despite verbosity being set to 0, OMPL spits out a lot of text with utils.suppress_std_out_and_err(): _, ret_floats, _, _ = utils.script_call( 'getNonlinearPathMobile@PyRep', PYREP_SCRIPT_TYPE, ints=[ handle_base, handle_target_base, self._collision_collection, int(ignore_collisions), path_pts ], floats=[boundaries], strings=[algorithm.value]) # self.set_parent(None) # self.base_ref.set_parent(self) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') path = [] for i in range(0, len(ret_floats) // 3): inst = ret_floats[3 * i:3 * i + 3] if i > 0: dist_change = sqrt((inst[0] - prev_inst[0])**2 + (inst[1] - prev_inst[1])**2) else: dist_change = 0 inst.append(dist_change) path.append(inst) prev_inst = inst return path def _check_collision_linear_path(self, path): """Check for collision on a linear path from start to goal :param path: A list containing start and goal as [x,y,yaw] :return: A bool, True if collision was detected """ start = path[0] end = path[1] m = (end[1] - start[1]) / (end[0] - start[0]) b = start[1] - m * start[0] x_range = [start[0], end[0]] x_span = start[0] - end[0] incr = round(abs(x_span) / 50, 3) if x_range[1] < x_range[0]: incr = -incr x = x_range[0] for k in range(50): x += incr y = m * x + b self.set_2d_pose([x, y, start[-1] if k < 46 else end[-1]]) status_collision = self.assess_collision() if status_collision == True: break return status_collision def get_base_actuation(self): """Controller for mobile bases. """ raise NotImplementedError() def copy(self) -> RobotComponent: self.intermediate_target_base.set_parent(self) self.target_base.set_parent(self) c = super().copy() self.intermediate_target_base.set_parent(None) self.target_base.set_parent(None) return c
class Arm(RobotComponent): """Base class representing a robot arm with path planning support. """ def __init__(self, count: int, name: str, num_joints: int, base_name: str = None, max_velocity=1.0, max_acceleration=4.0, max_jerk=1000): """Count is used for when we have multiple copies of arms""" joint_names = ['%s_joint%d' % (name, i+1) for i in range(num_joints)] super().__init__(count, name, joint_names, base_name) # Used for motion planning self.max_velocity = max_velocity self.max_acceleration = max_acceleration self.max_jerk = max_jerk # Motion planning handles suffix = '' if count == 0 else '#%d' % (count - 1) self._ik_target = Dummy('%s_target%s' % (name, suffix)) self._ik_tip = Dummy('%s_tip%s' % (name, suffix)) self._ik_group = sim.simGetIkGroupHandle('%s_ik%s' % (name, suffix)) self._collision_collection = sim.simGetCollectionHandle( '%s_arm%s' % (name, suffix)) def set_ik_element_properties(self, constraint_x=True, constraint_y=True, constraint_z=True, constraint_alpha_beta=True, constraint_gamma=True) -> None: constraints = 0 if constraint_x: constraints |= sim.sim_ik_x_constraint if constraint_y: constraints |= sim.sim_ik_y_constraint if constraint_z: constraints |= sim.sim_ik_z_constraint if constraint_alpha_beta: constraints |= sim.sim_ik_alpha_beta_constraint if constraint_gamma: constraints |= sim.sim_ik_gamma_constraint sim.simSetIkElementProperties( ikGroupHandle=self._ik_group, tipDummyHandle=self._ik_tip.get_handle(), constraints=constraints, precision=None, weight=None, ) def set_ik_group_properties(self, resolution_method='pseudo_inverse', max_iterations=6, dls_damping=0.1) -> None: try: res_method = {'pseudo_inverse': sim.sim_ik_pseudo_inverse_method, 'damped_least_squares': sim.sim_ik_damped_least_squares_method, 'jacobian_transpose': sim.sim_ik_jacobian_transpose_method}[resolution_method] except KeyError: raise Exception('Invalid resolution method,' 'Must be one of ["pseudo_inverse" | "damped_least_squares" | "jacobian_transpose"]') sim.simSetIkGroupProperties( ikGroupHandle=self._ik_group, resolutionMethod=res_method, maxIterations=max_iterations, damping=dls_damping ) def solve_ik_via_sampling(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions: bool = False, trials: int = 300, max_configs: int = 1, distance_threshold: float = 0.65, max_time_ms: int = 10, relative_to: Object = None ) -> np.ndarray: """Solves an IK group and returns the calculated joint values. This IK method performs a random searches for manipulator configurations that matches the given end-effector pose in space. When the tip pose is close enough then IK is computed in order to try to bring the tip onto the target. This is the method that should be used when the start pose is far from the end pose. We generate 'max_configs' number of samples within X number of 'trials', before ranking them according to angular distance. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. :param max_configs: The maximum number of configurations we want to generate before sorting them. :param distance_threshold: Distance indicating when IK should be computed in order to try to bring the tip onto the target. :param max_time_ms: Maximum time in ms spend searching for each configuation. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationError if no joint configuration could be found. :return: 'max_configs' number of joint configurations, ranked according to angular distance. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position, relative_to) if euler is not None: self._ik_target.set_orientation(euler, relative_to) elif quaternion is not None: self._ik_target.set_quaternion(quaternion, relative_to) handles = [j.get_handle() for j in self.joints] cyclics, intervals = self.get_joint_intervals() low_limits, max_limits = list(zip(*intervals)) # If there are huge intervals, then limit them low_limits = np.maximum(low_limits, -np.pi*2).tolist() max_limits = np.minimum(max_limits, np.pi*2).tolist() collision_pairs = [] if not ignore_collisions: collision_pairs = [self._collision_collection, sim.sim_handle_all] metric = joint_options = None valid_joint_positions = [] for i in range(trials): config = sim.simGetConfigForTipPose( self._ik_group, handles, distance_threshold, int(max_time_ms), metric, collision_pairs, joint_options, low_limits, max_limits) if len(config) > 0: valid_joint_positions.append(config) if len(valid_joint_positions) >= max_configs: break self._ik_target.set_pose(prev_pose) if len(valid_joint_positions) == 0: raise ConfigurationError( 'Could not find a valid joint configuration for desired ' 'end effector pose.') if len(valid_joint_positions) > 1: current_config = np.array(self.get_joint_positions()) # Sort based on angular distance valid_joint_positions.sort( key=lambda x: np.linalg.norm(current_config - x)) return np.array(valid_joint_positions) def get_configs_for_tip_pose(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=300, max_configs=60, relative_to: Object = None ) -> List[List[float]]: """Gets a valid joint configuration for a desired end effector pose. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs :param max_configs: The maximum number of configurations we want to generate before ranking them. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationError if no joint configuration could be found. :return: A list of valid joint configurations for the desired end effector pose. """ warnings.warn("Please use 'solve_ik_via_sampling' instead.", DeprecationWarning) return list(self.solve_ik_via_sampling( position, euler, quaternion, ignore_collisions, trials, max_configs, relative_to=relative_to)) def solve_ik_via_jacobian( self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, relative_to: Object = None) -> List[float]: """Solves an IK group and returns the calculated joint values. This IK method performs a linearisation around the current robot configuration via the Jacobian. The linearisation is valid when the start and goal pose are not too far away, but after a certain point, linearisation will no longer be valid. In that case, the user is better off using 'solve_ik_via_sampling'. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :return: A list containing the calculated joint values. """ self._ik_target.set_position(position, relative_to) if euler is not None: self._ik_target.set_orientation(euler, relative_to) elif quaternion is not None: self._ik_target.set_quaternion(quaternion, relative_to) ik_result, joint_values = sim.simCheckIkGroup( self._ik_group, [j.get_handle() for j in self.joints]) if ik_result == sim.sim_ikresult_fail: raise IKError('IK failed. Perhaps the distance was between the tip ' ' and target was too large.') elif ik_result == sim.sim_ikresult_not_performed: raise IKError('IK not performed.') return joint_values def solve_ik(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, relative_to: Object = None) -> List[float]: """Solves an IK group and returns the calculated joint values. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :return: A list containing the calculated joint values. """ warnings.warn("Please use 'solve_ik_via_jacobian' instead.", DeprecationWarning) return self.solve_ik_via_jacobian( position, euler, quaternion, relative_to) 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 get_linear_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, steps=50, ignore_collisions=False, relative_to: Object = None) -> ArmConfigurationPath: """Gets a linear configuration path given a target pose. Generates a path that drives a robot from its current configuration to its target dummy in a straight line (i.e. shortest path in Cartesian space). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param steps: The desired number of path points. Each path point contains a robot configuration. A minimum of two path points is required. If the target pose distance is large, a larger number of steps leads to better results for this function. :param ignore_collisions: If collision checking should be disabled. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationPathError if no path could be created. :return: A linear path in the arm configuration space. """ if not ((euler is None) ^ (quaternion is None)): raise ConfigurationPathError( 'Specify either euler or quaternion values, but not both.') prev_pose = self._ik_target.get_pose() self._ik_target.set_position(position, relative_to) if euler is not None: self._ik_target.set_orientation(euler, relative_to) elif quaternion is not None: self._ik_target.set_quaternion(quaternion, relative_to) handles = [j.get_handle() for j in self.joints] collision_pairs = [] if not ignore_collisions: collision_pairs = [self._collision_collection, sim.sim_handle_all] joint_options = None ret_floats = sim.generateIkPath( self._ik_group, handles, steps, collision_pairs, joint_options) self._ik_target.set_pose(prev_pose) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_nonlinear_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=300, max_configs=1, distance_threshold: float = 0.65, max_time_ms: int = 10, trials_per_goal=1, algorithm=Algos.RRTConnect, relative_to: Object = None ) -> ArmConfigurationPath: """Gets a non-linear (planned) configuration path given a target pose. A path is generated by finding several configs for a pose, and ranking them according to the distance in configuration space (smaller is better). Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. See 'solve_ik_via_sampling'. :param max_configs: The maximum number of configurations we want to generate before sorting them. See 'solve_ik_via_sampling'. :param distance_threshold: Distance indicating when IK should be computed in order to try to bring the tip onto the target. See 'solve_ik_via_sampling'. :param max_time_ms: Maximum time in ms spend searching for each configuation. See 'solve_ik_via_sampling'. :param trials_per_goal: The number of paths per config we want to trial. :param algorithm: The algorithm for path planning to use. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationPathError if no path could be created. :return: A non-linear path in the arm configuration space. """ handles = [j.get_handle() for j in self.joints] try: configs = self.solve_ik_via_sampling( position, euler, quaternion, ignore_collisions, trials, max_configs, distance_threshold, max_time_ms, relative_to) except ConfigurationError as e: raise ConfigurationPathError('Could not create path.') from e _, ret_floats, _, _ = utils.script_call( 'getNonlinearPath@PyRep', PYREP_SCRIPT_TYPE, ints=[self._collision_collection, int(ignore_collisions), trials_per_goal] + handles, floats=configs.flatten().tolist(), strings=[algorithm.value]) if len(ret_floats) == 0: raise ConfigurationPathError('Could not create path.') return ArmConfigurationPath(self, ret_floats) def get_path(self, position: Union[List[float], np.ndarray], euler: Union[List[float], np.ndarray] = None, quaternion: Union[List[float], np.ndarray] = None, ignore_collisions=False, trials=300, max_configs=1, distance_threshold: float = 0.65, max_time_ms: int = 10, trials_per_goal=1, algorithm=Algos.RRTConnect, relative_to: Object = None ) -> ArmConfigurationPath: """Tries to get a linear path, failing that tries a non-linear path. Must specify either rotation in euler or quaternions, but not both! :param position: The x, y, z position of the target. :param euler: The x, y, z orientation of the target (in radians). :param quaternion: A list containing the quaternion (x,y,z,w). :param ignore_collisions: If collision checking should be disabled. :param trials: The maximum number of attempts to reach max_configs. See 'solve_ik_via_sampling'. :param max_configs: The maximum number of configurations we want to generate before sorting them. See 'solve_ik_via_sampling'. :param distance_threshold: Distance indicating when IK should be computed in order to try to bring the tip onto the target. See 'solve_ik_via_sampling'. :param max_time_ms: Maximum time in ms spend searching for each configuation. See 'solve_ik_via_sampling'. :param trials_per_goal: The number of paths per config we want to trial. :param algorithm: The algorithm for path planning to use. :param relative_to: Indicates relative to which reference frame we want the target pose. Specify None to retrieve the absolute pose, or an Object relative to whose reference frame we want the pose. :raises: ConfigurationPathError if neither a linear or non-linear path can be created. :return: A linear or non-linear path in the arm configuration space. """ try: p = self.get_linear_path(position, euler, quaternion, ignore_collisions=ignore_collisions, relative_to=relative_to) return p except ConfigurationPathError: pass # Allowed. Try again, but with non-linear. # This time if an exception is thrown, we dont want to catch it. p = self.get_nonlinear_path( position, euler, quaternion, ignore_collisions, trials, max_configs, distance_threshold, max_time_ms, trials_per_goal, algorithm, relative_to) return p def get_tip(self) -> Dummy: """Gets the tip of the arm. Each arm is required to have a tip for path planning. :return: The tip of the arm. """ return self._ik_tip def get_jacobian(self): """Calculates the Jacobian. :return: the row-major Jacobian matix. """ self._ik_target.set_matrix(self._ik_tip.get_matrix()) sim.simCheckIkGroup(self._ik_group, [j.get_handle() for j in self.joints]) jacobian, (rows, cols) = sim.simGetIkGroupMatrix(self._ik_group, 0) jacobian = np.array(jacobian).reshape((rows, cols), order='F') return jacobian def check_arm_collision(self, obj: 'Object' = None) -> bool: """Checks whether two entities are colliding. :param obj: The other collidable object to check collision against, or None to check against all collidable objects. Note that objects must be marked as collidable! :return: If the object is colliding. """ handle = sim.sim_handle_all if obj is None else obj.get_handle() return sim.simCheckCollision(self._collision_collection, handle) == 1