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
# create a target orientation
target_abg = np.random.random(3) * np.pi - np.pi / 2.0
# calculate current position quaternion
R = robot_config.R('EE', q=q)
quat_1 = transformations.unit_vector(
transformations.quaternion_from_matrix(R))
# 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))
# calculate the quaternion of the target
quat_target = transformations.unit_vector(
transformations.quaternion_from_euler(
target_abg[0], target_abg[1], target_abg[2], axes='rxyz'))
dist1 = calc_distance(quat_1, np.copy(quat_target))
dist2 = calc_distance(quat_2, np.copy(quat_target))
assert np.abs(dist2) < np.abs(dist1)
def _calc_orientation_forces(self, target_abg, q):
"""Calculate the desired Euler angle forces to apply to the arm to
move the end-effector to the target orientation
Parameters
----------
target_abg: np.array
the target Euler angles orientation for the end-effector:
alpha, beta, gamma
q: np.array
the joint angles of the arm
"""
u_task_orientation = np.zeros(3)
if self.orientation_algorithm == 0:
# transform the orientation target into a quaternion
q_d = transformations.unit_vector(
transformations.quaternion_from_euler(
target_abg[0], target_abg[1], target_abg[2], axes="rxyz"
)
)
# get the quaternion for the end effector
q_e = self.robot_config.quaternion("EE", q=q)
q_r = transformations.quaternion_multiply(
q_d, transformations.quaternion_conjugate(q_e)
)
u_task_orientation = -q_r[1:] * np.sign(q_r[0])
elif self.orientation_algorithm == 1:
# From (Caccavale et al, 1997) Section IV Quaternion feedback
# get rotation matrix for the end effector orientation
R_e = self.robot_config.R("EE", q)
# get rotation matrix for the target orientation
R_d = transformations.euler_matrix(
target_abg[0], target_abg[1], target_abg[2], axes="rxyz"
)[:3, :3]
R_ed = np.dot(R_e.T, R_d) # eq 24
q_ed = transformations.unit_vector(
transformations.quaternion_from_matrix(R_ed)
)
u_task_orientation = -1 * np.dot(R_e, q_ed[1:]) # eq 34
else:
raise Exception(
"Invalid algorithm number %i for calculating "
% self.orientation_algorithm
+ "orientation error"
)
return u_task_orientation
def generate_path(
self,
start_position,
target_position,
max_velocity,
start_orientation=None,
target_orientation=None,
start_velocity=0,
target_velocity=0,
plot=False,
):
"""
Takes a start and target position, along with an optional start and target
velocity, and generates a trajectory that smoothly accelerates, at a rate
defined by vel_profile, from start_velocity to max_v, and back to target_v.
If the path is too short to reach max_v and still decelerate to target_v at a
rate of max_a, then the path will be slowed to the maximum allowable velocity
so that we still reach target_velocity at the moment we are at target_position.
Optionally can pass in a 3D angular state [a, b, g] and target orientation.
Note that the orientation should be given in euler angles, in the ordered
specified by axes on init. The orientation path will follow the same velocity
profile as the position path.
Parameters
----------
start_position: 3x1 np.array of floats
The starting position (x, y, z).
target_position: 3x1 np.array of floats
The target position (x, y, z).
max_velocity: float
The maximum allowable velocity of the path.
start_velocity: float, Optional (Default: 0)
The velocity at start of path.
target_velocity: float, Optional (Default: 0)
The velocity at end of path.
start_orientation: 3x1 np.array of floats, Optional (Default: None)
The orientation at start of path in euler angles, given in the order
specified on __init__ with the axes parameter (default rxyz). When left as
`None`, no orientation path will be planned.
target_orientation: 3x1 np.array of floats, Optional (Default: None)
The target orientation at the end of the path in euler angles, given in the
order specified on __init__ with the axes parameter.
plot: bool, Optional (Default: False)
Set `True` to plot path profiles for debugging.
"""
assert start_velocity <= max_velocity, (
f"{c.red}start velocity({start_velocity}m/s) " +
f"> max velocity({max_velocity}m/s){c.endc}")
assert target_velocity <= max_velocity, (
f"{c.red}target velocity({target_velocity}m/s) " +
f"> max velocity({max_velocity}m/s){c.endc}")
if start_velocity == max_velocity:
self.starting_dist = 0
self.starting_vel_profile = [start_velocity * self.dt]
else:
self.starting_dist = None
if target_velocity == max_velocity:
self.ending_dist = 0
self.ending_vel_profile = [target_velocity * self.dt]
else:
self.ending_dist = None
self.max_velocity = max_velocity
if self.starting_dist is None:
# Regenerate our velocity curves if start or end v have changed
if (self.starting_vel_profile is None
or self.start_velocity != start_velocity):
self.starting_vel_profile = self.vel_profile.generate(
start_velocity=start_velocity,
target_velocity=self.max_velocity)
# calculate the distance covered ramping from start_velocity to
# max_v and from max_v to target_velocity
self.starting_dist = np.sum(self.starting_vel_profile * self.dt)
if self.ending_dist is None:
# if our start and end v are the same, just mirror the curve to
# avoid regenerating
if start_velocity == target_velocity:
self.ending_vel_profile = self.starting_vel_profile[::-1]
# if target velocity is different, generate its unique curve
elif (self.ending_vel_profile is None
or self.target_velocity != target_velocity):
self.ending_vel_profile = self.vel_profile.generate(
start_velocity=target_velocity,
target_velocity=self.max_velocity)[::-1]
# calculate the distance covered ramping from start_velocity to
# max_v and from max_v to target_velocity
self.ending_dist = np.sum(self.ending_vel_profile * self.dt)
# save as self variables so we can check on the next generate call if we
# need a different profile
self.start_velocity = start_velocity
self.target_velocity = target_velocity
if self.verbose:
self.log.append(
f"{c.blue}Generating a path from {start_position} to " +
f"{target_position}{c.endc}")
self.log.append(
f"{c.blue}max_velocity={self.max_velocity}{c.endc}")
self.log.append(
f"{c.blue}start_velocity={self.start_velocity} | " +
f"target_velocity={self.target_velocity}{c.endc}")
# calculate the distance between our current state and the target
target_direction = target_position - start_position
dist = np.linalg.norm(target_direction)
target_norm = target_direction / dist
# the default direction of our path shape
a = 1 / np.sqrt(3)
base_norm = np.array([a, a, a])
# get rotation matrix to rotate our path shape to the target direction
R = self.align_vectors(base_norm, target_norm)
# get the length travelled along our stretched curve
curve_dist_steps = []
warped_xyz = []
for ii, t in enumerate(np.linspace(0, 1, self.n_sample_points)):
# ==== Warp our path ====
# - stretch our curve shape to be the same length as target-start
# - rotate our curve to align with the direction target-start
# - shift our curve to begin at the start state
warped_target = (
np.dot(R, (1 / np.sqrt(3)) * self.pos_profile.step(t) * dist) +
start_position)
warped_xyz.append(warped_target)
if t > 0:
curve_dist_steps.append(
np.linalg.norm(warped_xyz[ii] - warped_xyz[ii - 1]))
else:
curve_dist_steps.append(0)
# get the cumulative distance covered
dist_steps = np.cumsum(curve_dist_steps)
curve_length = np.sum(curve_dist_steps)
# create functions that return our path at a given distance
# along that curve
self.warped_xyz = np.array(warped_xyz)
X = scipy.interpolate.interp1d(dist_steps,
self.warped_xyz.T[0],
fill_value="extrapolate")
Y = scipy.interpolate.interp1d(dist_steps,
self.warped_xyz.T[1],
fill_value="extrapolate")
Z = scipy.interpolate.interp1d(dist_steps,
self.warped_xyz.T[2],
fill_value="extrapolate")
XYZ = [X, Y, Z]
# distance is greater than our ramping up and down distance, add a linear
# velocity between the ramps to converge to the correct position
self.remaining_dist = None
if curve_length >= self.starting_dist + self.ending_dist:
# calculate the remaining steps where we will be at constant max_v
remaining_dist = curve_length - (self.ending_dist +
self.starting_dist)
constant_speed_steps = int(remaining_dist / self.max_velocity /
self.dt)
self.stacked_vel_profile = np.hstack((
self.starting_vel_profile,
np.ones(constant_speed_steps) * self.max_velocity,
self.ending_vel_profile,
))
if plot:
self.remaining_dist = remaining_dist
self.dist = dist
else:
# scale our profile
# TODO to do this properly we should evaluate the integral to get
# the t where the sum of the profile is half our travel distance.
# This way we maintain the same acceleration profile instead of
# maintaining the same number of steps and accelerating more slowly,
# which is what we do by scaling the vel profile down
scale = curve_length / (self.starting_dist + self.ending_dist)
self.stacked_vel_profile = np.hstack(
(scale * self.starting_vel_profile,
scale * self.ending_vel_profile))
self.position_path = []
# the distance covered over time with respect to our velocity profile
path_steps = np.cumsum(self.stacked_vel_profile * self.dt)
# step along our curve, with our next path step being the distance
# determined by our velocity profile, in the direction of the path curve
for ii in range(0, len(self.stacked_vel_profile)):
shiftx = XYZ[0](path_steps[ii])
shifty = XYZ[1](path_steps[ii])
shiftz = XYZ[2](path_steps[ii])
shift = np.array([shiftx, shifty, shiftz])
self.position_path.append(shift)
self.position_path = np.asarray(self.position_path)
# get our 3D vel profile components by differentiating the position path
# our velocity profile is 1D, to get the 3 velocity components we can
# just differentiate the position path. Since the distance between steps
# was determined with our velocity profile, we should still maintain the
# desired velocities. Note this may break down with complex, high
# frequency paths
self.velocity_path = np.asarray(
np.gradient(self.position_path, self.dt, axis=0))
# check if we received start and target orientations
if isinstance(start_orientation,
(list, (np.ndarray, np.generic), tuple)):
if isinstance(target_orientation,
(list, (np.ndarray, np.generic), tuple)):
# Generate the orientation portion of our trajectory.
# We will use quaternions and SLERP for filtering orientation path
quat0 = transform.quaternion_from_euler(
start_orientation[0],
start_orientation[1],
start_orientation[2],
axes=self.axes,
)
quat1 = transform.quaternion_from_euler(
target_orientation[0],
target_orientation[1],
target_orientation[2],
axes=self.axes,
)
self.orientation_path = self.OrientationPlanner.match_position_path(
orientation=quat0,
target_orientation=quat1,
position_path=self.position_path,
)
if self.verbose:
self.log.append(
f"{c.blue}start_orientation={start_orientation} | " +
f"target_orientation={target_orientation}{c.endc}")
else:
raise NotImplementedError(
f"{c.red}A target orientation is required to generate path{c.endc}"
)
self.orientation_path = np.asarray(self.orientation_path)
# TODO should this be included? look at proper derivation here...
# https://physics.stackexchange.com/questions/73961/angular-velocity-expressed-via-euler-angles # pylint: disable=C0301
self.ang_velocity_path = np.asarray(
np.gradient(self.orientation_path, self.dt, axis=0))
self.path = np.hstack((
np.hstack((
np.hstack((self.position_path, self.velocity_path)),
self.orientation_path,
)),
self.ang_velocity_path,
))
else:
self.path = np.hstack((self.position_path, self.velocity_path))
if plot:
self._plot(
start_position=start_position,
target_position=target_position,
)
# Some parameters that are useful to have access to externally,
# used in nengo-control
self.n_timesteps = len(self.path)
self.n = 0
self.time_to_converge = self.n_timesteps * self.dt
self.target_counter += 1
if self.verbose:
self.log.append(
f"{c.blue}Time to converge: {self.time_to_converge}{c.endc}")
self.log.append(f"{c.blue}dt: {self.dt}{c.endc}")
self.log.append(
f"{c.blue}pos x error: " +
f"{self.position_path[-1, 0] - target_position[0]}{c.endc}")
self.log.append(
f"{c.blue}pos y error: " +
f"{self.position_path[-1, 1] - target_position[1]}{c.endc}")
self.log.append(
f"{c.blue}pos x error: " +
f"{self.position_path[-1, 2] - target_position[2]}{c.endc}")
self.log.append(
f"{c.blue}2norm error at target: " +
f"{np.linalg.norm(self.position_path[-1] - target_position[:3])}{c.endc}" # pylint: disable=C0301
)
dash = "".join((["-"] * len(max(self.log, key=len))))
print(f"{c.blue}{dash}{c.endc}")
for log in self.log:
print(log)
print(f"{c.blue}{dash}{c.endc}")
err = np.linalg.norm(self.position_path[-1] - target_position)
if err >= 0.01:
warnings.warn(
(f"\n{c.yellow}WARNING: the distance at the end of the " +
f"generated path to your desired target position is {err}m." +
"If you desire a lower error you can try:" +
"\n\t- a path shape with lower frequency terms" +
"\n\t- more sample points (set on __init__)" +
"\n\t- smaller simulation timestep" +
"\n\t- lower maximum velocity and acceleration" +
f"\n\t- lower start and end velocities{c.endc}"))
return self.path
def generate_path(
self,
position,
target_position,
n_timesteps=200,
dt=0.001,
plot=False,
method=3,
axes="rxyz",
):
"""
Parameters
----------
position: numpy.array
the current position of the system
target_position: numpy.array
the task space target position and orientation
n_timesteps: int, optional (Default: 200)
the number of time steps to reach the target_position
dt: float, optional (Default: 0.001)
the time step for calculating desired velocities [seconds]
plot: boolean, optional (Default: False)
plot the path after generating if True
method: int, optional (Default: 3)
Different ways to compute inverse resolved motion
1. Standard resolved motion
2. Dampened least squares method
3. Nullspace with priority for position, orientation in null space
axes: string, optional (Default: 'rxyz')
the format of the Euler angles passed in target[3:6]
First letter r or s represents 'relative' or 'static'
"""
path = np.zeros((n_timesteps, position.shape[0] * 2))
ee_track = []
ee_err = []
ea_err = []
# set the largest allowable step in joint position
max_dq = self.max_dq * dt
# set the largest allowable step in hand (x,y,z)
max_dx = self.max_dx * dt
# set the largest allowable step in hand (alpha, beta, gamma)
max_dr = self.max_dr * dt
Qd = np.array(
transformations.unit_vector(
transformations.quaternion_from_euler(
target_position[3],
target_position[4],
target_position[5],
axes="sxyz",
)
)
)
q = np.copy(position)
for ii in range(n_timesteps):
J = self.robot_config.J("EE", q=q)
Tx = self.robot_config.Tx("EE", q=q)
ee_track.append(Tx)
dx = target_position[:3] - Tx
Qe = self.robot_config.quaternion("EE", q=q)
# Method 4
dr = Qe[0] * Qd[1:] - Qd[0] * Qe[1:] - np.cross(Qd[1:], Qe[1:])
norm_dx = np.linalg.norm(dx, 2)
norm_dr = np.linalg.norm(dr, 2)
ee_err.append(norm_dx)
ea_err.append(norm_dr)
# limit max step size in operational space
if norm_dx > max_dx:
dx = dx / norm_dx * max_dx
if norm_dr > max_dr:
dr = dr / norm_dr * max_dr
Jx = J[:3]
pinv_Jx = np.linalg.pinv(Jx)
# Different ways to compute inverse resolved motion
if method == 1:
# Standard resolved motion
dq = np.dot(np.linalg.pinv(J), np.hstack([dx, dr]))
if method == 2:
# Dampened least squares method
dq = np.dot(
J.T,
np.linalg.solve(
np.dot(J, J.T) + np.eye(6) * 0.001, np.hstack([dx, dr * 0.3])
),
)
if method == 3:
# Primary position IK, control orientation in null space
dq = np.dot(pinv_Jx, dx) + np.dot(
np.eye(self.robot_config.N_JOINTS) - np.dot(pinv_Jx, Jx),
np.dot(np.linalg.pinv(J[3:]), dr),
)
# limit max step size in joint space
if max(abs(dq)) > max_dq:
dq = dq / max(abs(dq)) * max_dq
path[ii] = np.hstack([q, dq])
q = q + dq
if plot:
ee_track = np.array(ee_track)
plt.subplot(2, 1, 1)
plt.plot(ee_track)
plt.gca().set_prop_cycle(None)
plt.plot(np.ones((n_timesteps, 3)) * target_position[:3], "--")
plt.legend(
["%i" % ii for ii in range(3)] + ["%i_target" % ii for ii in range(3)]
)
plt.title("Trajectory positions")
plt.subplot(2, 1, 2)
plt.plot(ee_err)
plt.plot(ea_err)
plt.legend(["Position error", "Orientation error"])
plt.title("Trajectory orientations")
plt.tight_layout()
plt.show()
plt.savefig("IK_plot.png")
# reset position_path index
self.n_timesteps = n_timesteps
self.n = 0
self.position_path = path[:, : self.robot_config.N_JOINTS]
self.velocity_path = path[:, self.robot_config.N_JOINTS :]
return self.position_path, self.velocity_path
Mx = np.linalg.inv(Mx_inv)
else:
# using the rcond to set singular values < thresh to 0
# singular values < (rcond * max(singular_values)) set to 0
Mx = np.linalg.pinv(Mx_inv, rcond=.005)
u_task = np.zeros(6) # [x, y, z, alpha, beta, gamma]
# calculate position error
u_task[:3] = -kp * (hand_xyz - target[:3])
# Method 1 ------------------------------------------------------------
#
# transform the orientation target into a quaternion
q_d = transformations.unit_vector(
transformations.quaternion_from_euler(
target[3], target[4], target[5], axes='rxyz'))
# get the quaternion for the end effector
q_e = transformations.unit_vector(
transformations.quaternion_from_matrix(
robot_config.R('EE', feedback['q'])))
# calculate the rotation from the end-effector to target orientation
q_r = transformations.quaternion_multiply(
q_d, transformations.quaternion_conjugate(q_e))
u_task[3:] = q_r[1:] * np.sign(q_r[0])
# Method 2 ------------------------------------------------------------
# From (Caccavale et al, 1997) Section IV - Quaternion feedback -------
#
# get rotation matrix for the end effector orientation
orientation_path,
target_position,
target_orientation,
) = get_target(robot_config)
interface.set_mocap_xyz("target_orientation", target_position)
interface.set_mocap_orientation("target_orientation",
target_orientation)
pos, vel = position_planner.next()
orient = orientation_path.next()
target = np.hstack([pos, orient])
interface.set_mocap_xyz("path_planner_orientation", target[:3])
interface.set_mocap_orientation(
"path_planner_orientation",
transformations.quaternion_from_euler(orient[0], orient[1],
orient[2], "rxyz"),
)
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# add gripper forces
u = np.hstack((u, np.zeros(robot_config.N_GRIPPER_JOINTS)))
# apply the control signal, step the sim forward
interface.send_forces(u)
# track data
Mx = np.linalg.inv(Mx_inv)
else:
# using the rcond to set singular values < thresh to 0
# singular values < (rcond * max(singular_values)) set to 0
Mx = np.linalg.pinv(Mx_inv, rcond=.005)
u_task = np.zeros(6) # [x, y, z, alpha, beta, gamma]
# calculate position error
u_task[:3] = -kp * (hand_xyz - target[:3])
# Method 1 ------------------------------------------------------------
#
# transform the orientation target into a quaternion
q_d = transformations.unit_vector(
transformations.quaternion_from_euler(
target[3], target[4], target[5], axes='rxyz'))
# get the quaternion for the end effector
q_e = transformations.unit_vector(
transformations.quaternion_from_matrix(
robot_config.R('EE', feedback['q'])))
# calculate the rotation from the end-effector to target orientation
q_r = transformations.quaternion_multiply(
q_d, transformations.quaternion_conjugate(q_e))
u_task[3:] = q_r[1:] * np.sign(q_r[0])
# Method 2 ------------------------------------------------------------
# From (Caccavale et al, 1997) Section IV - Quaternion feedback -------
#
# get rotation matrix for the end effector orientation
size=3)
path_planner.generate_path(
start_position=hand_xyz,
target_position=target_position,
start_orientation=starting_orientation,
target_orientation=target_orientation,
max_velocity=2,
)
interface.set_mocap_xyz("target_orientation", target_position)
interface.set_mocap_orientation(
"target_orientation",
transformations.quaternion_from_euler(
target_orientation[0],
target_orientation[1],
target_orientation[2],
"rxyz",
),
)
next_target = path_planner.next()
pos = next_target[:3]
vel = next_target[3:6]
orient = next_target[6:9]
target = np.hstack([pos, orient])
interface.set_mocap_xyz("path_planner_orientation", target[:3])
interface.set_mocap_orientation(
"path_planner_orientation",
transformations.quaternion_from_euler(orient[0], orient[1],
orient[2], "rxyz"),
