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
