def generate_path(self, orientation, target_orientation, plot=False):
""" Generates a linear trajectory to the target
Accepts orientations as quaternions and returns an array of orientations
from orientation to target orientation, based on the timesteps defined
in __init__. Orientations are returns as euler angles to match the
format of the OSC class
NOTE: no velocity trajectory is calculated at the moment
Parameters
----------
orientation: list of 4 floats
the starting orientation as a quaternion
target_orientation: list of 4 floats
the target orientation as a quaternion
"""
# stores the target Euler angles of the trajectory
self.orientation = np.zeros((self.n_timesteps, 3))
self.target_angles = transformations.euler_from_quaternion(
target_orientation, axes='rxyz')
for ii in range(self.n_timesteps):
quat = self._step(orientation=orientation,
target_orientation=target_orientation)
self.orientation[ii] = transformations.euler_from_quaternion(
quat, axes='rxyz')
self.n = 0
return self.orientation
def generate_path(self, orientation, target_orientation, plot=False):
""" Generates a linear trajectory to the target
Accepts orientations as quaternions and returns an array of orientations
from orientation to target orientation, based on the timesteps defined
in __init__. Orientations are returns as euler angles to match the
format of the OSC class
NOTE: no velocity trajectory is calculated at the moment
Parameters
----------
orientation: list of 4 floats
the starting orientation as a quaternion
target_orientation: list of 4 floats
the target orientation as a quaternion
"""
if len(orientation) == 3:
raise ValueError(
"\n----------------------------------------------\n" +
"A quaternion is required as input for the orientation " +
"path planner. To convert your " +
"Euler angles into a quaternion run...\n\n" +
"from abr_control.utils import transformations\n" +
"quaternion = transformation.quaternion_from_euler(a, b, g)\n"
+ "----------------------------------------------")
# stores the target Euler angles of the trajectory
self.orientation_path = np.zeros((self.n_timesteps, 3))
self.target_angles = transformations.euler_from_quaternion(
target_orientation, axes="rxyz")
for ii in range(self.n_timesteps):
quat = self._step(orientation=orientation,
target_orientation=target_orientation)
self.orientation_path[ii] = transformations.euler_from_quaternion(
quat, axes="rxyz")
self.n = 0
if plot:
self._plot()
return self.orientation_path
def test_calc_orientation_forces(arm, orientation_algorithm):
robot_config = arm.Config(use_cython=False)
ctrlr = OSC(robot_config, orientation_algorithm=orientation_algorithm)
for ii in range(100):
q = np.random.random(robot_config.N_JOINTS) * 2 * np.pi
quat = robot_config.quaternion("EE", q=q)
theta = np.pi / 2
axis = np.array([0, 0, 1])
quat_rot = transformations.unit_vector(
np.hstack([np.cos(theta / 2),
np.sin(theta / 2) * axis]))
quat_target = transformations.quaternion_multiply(quat, quat_rot)
target_abg = transformations.euler_from_quaternion(quat_target,
axes="rxyz")
# calculate current position quaternion
R = robot_config.R("EE", q=q)
quat_1 = transformations.unit_vector(
transformations.quaternion_from_matrix(R))
dist1 = calc_distance(quat_1, np.copy(quat_target))
# calculate current position quaternion with u_task added
u_task = ctrlr._calc_orientation_forces(target_abg, q=q)
dq = np.dot(np.linalg.pinv(robot_config.J("EE", q)),
np.hstack([np.zeros(3), u_task]))
q_2 = q - dq * 0.001 # where 0.001 represents the time step
R_2 = robot_config.R("EE", q=q_2)
quat_2 = transformations.unit_vector(
transformations.quaternion_from_matrix(R_2))
dist2 = calc_distance(quat_2, np.copy(quat_target))
assert np.abs(dist2) < np.abs(dist1)
interface.set_mocap_xyz(name="target", xyz=target_xyz)
# set the status of the top right text for adaptation
interface.viewer.adapt = True
count = 0.0
while 1:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack([
interface.get_xyz("target"),
transformations.euler_from_quaternion(
interface.get_orientation("target"), "rxyz"),
])
# calculate the control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
u_adapt = np.zeros(robot_config.N_JOINTS)
u_adapt[:5] = adapt.generate(
input_signal=np.hstack((feedback["q"][:5], feedback["dq"][:5])),
training_signal=np.array(ctrlr.training_signal[:5]),
)
u += u_adapt
n_timesteps=n_timesteps)
orientation_planner = traj_planner.generate_orientation_path(
orientation=starting_orientation, target_orientation=target_orientation)
# set up lists for tracking data
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
try:
count = 0
interface.set_xyz('target', target_position)
interface.set_orientation(
'target',
transformations.euler_from_quaternion(target_orientation, axes='rxyz'))
print('\nSimulation starting...\n')
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
pos, vel = traj_planner.next()[:3]
orient = orientation_planner.next()
target = np.hstack([pos, orient])
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=target,
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
# set our target to our ee_xyz since we are only focussing on orinetation
interface.set_mocap_xyz('target_orientation', hand_xyz)
interface.set_mocap_orientation('target_orientation', rand_orient)
target = np.hstack([
interface.get_mocap_xyz('target_orientation'),
transformations.euler_from_quaternion(
interface.get_mocap_orientation('target_orientation'), 'rxyz')
])
rc_matrix = robot_config.R('EE', feedback['q'])
rc_angles = transformations.euler_from_matrix(rc_matrix, axes='rxyz')
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=target,
)
# apply the control signal, step the sim forward
interface.send_forces(u)
# track data
# traj_planner = path_planners.BellShaped(
# error_scale=1, n_timesteps=n_timesteps)
traj_planner = path_planners.SecondOrderFilter(n_timesteps=n_timesteps,
dt=0.004)
orientation_planner = path_planners.Orientation()
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
starting_orientation = robot_config.quaternion('EE', feedback['q'])
target_orientation = np.random.random(3)
target_orientation /= np.linalg.norm(target_orientation)
# convert our orientation to a quaternion
target_orientation = [0] + list(target_orientation)
print(target_orientation)
target_orientation_euler = transformations.euler_from_quaternion(
target_orientation)
target_position = np.array([-0.4, -0.3, 0.5]) #np.random.random(3)
traj_planner.generate_path(position=hand_xyz,
target_position=target_position,
velocity=np.zeros(3))
orientation_planner.match_position_path(
orientation=starting_orientation,
target_orientation=target_orientation,
position_path=traj_planner.position_path)
# set up lists for tracking data
ee_track = []
ee_angles_track = []
target_track = []
target_angles_track = []
def generate_path(self,
orientation,
target_orientation,
dr=None,
plot=False):
"""Generates a linear trajectory to the target
Accepts orientations as quaternions and returns an array of orientations
from orientation to target orientation, based on the timesteps defined
in __init__. Orientations are returns as euler angles to match the
format of the OSC class
NOTE: no velocity trajectory is calculated at the moment
Parameters
----------
orientation: list of 4 floats
the starting orientation as a quaternion
target_orientation: list of 4 floats
the target orientation as a quaternion
dr: float, Optional (Default: None)
if not None the path to target is broken up into n_timesteps segments.
Otherwise the number of timesteps are determined based on the set step
size in radians.
"""
if len(orientation) == 3:
raise ValueError(
"\n----------------------------------------------\n" +
"A quaternion is required as input for the orientation " +
"path planner. To convert your " +
"Euler angles into a quaternion run...\n\n" +
"from abr_control.utils import transformations\n" +
"quaternion = transformation.quaternion_from_euler(a, b, g)\n"
+ "----------------------------------------------")
self.target_angles = transformations.euler_from_quaternion(
target_orientation, axes=self.axes)
if dr is not None:
# angle between two quaternions
# "https://www.researchgate.net/post"
# + "/How_do_I_calculate_the_smallest_angle_between_two_quaternions"
# answer by luiz alberto radavelli
angle_diff = 2 * np.arccos(
np.dot(target_orientation, orientation) /
(np.linalg.norm(orientation) *
np.linalg.norm(target_orientation)))
if angle_diff > np.pi:
min_angle_diff = 2 * np.pi - angle_diff
else:
min_angle_diff = angle_diff
self.n_timesteps = int(min_angle_diff / dr)
print("%i steps to cover %f rad in %f sized steps" %
(self.n_timesteps, angle_diff, dr))
self.timesteps = np.linspace(0, 1, self.n_timesteps)
# stores the target Euler angles of the trajectory
self.orientation_path = np.zeros((self.n_timesteps, 3))
for ii in range(self.n_timesteps):
quat = self._step(orientation=orientation,
target_orientation=target_orientation)
self.orientation_path[ii] = transformations.euler_from_quaternion(
quat, axes=self.axes)
if self.n_timesteps == 0:
print("with the set step size, we reach the target in 1 step")
self.orientation_path = np.array([
transformations.euler_from_quaternion(target_orientation,
axes=self.axes)
])
self.n = 0
if plot:
self._plot()
return self.orientation_path
print("\nSimulation starting...\n")
while 1:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
if first_pass or count == path_planner.n_timesteps + 500:
count = 0
first_pass = False
# pregenerate our path and orientation planners
q = robot_config.quaternion("EE", feedback["q"])
starting_orientation = transformations.euler_from_quaternion(
q, axes="rxyz")
mag = 0.6
target_position = np.random.random(3) * 0.5
target_position = target_position / np.linalg.norm(
target_position) * mag
target_orientation = np.random.uniform(low=-np.pi,
high=np.pi,
size=3)
path_planner.generate_path(
start_position=hand_xyz,
target_position=target_position,
start_orientation=starting_orientation,
target_orientation=target_orientation,
