def q_tilde_quat(self, q, target):
"""Compute the error signal when there are quaternions in the state
space and target signals. If there are quaternions in the state space,
calculate the error and then transform them to 3D space for the control
signal.
NOTE: Assumes that for every quaternion there are 3 motors, that effect
movement along the x, y, and z axes, applied in that order.
Parameters
----------
q : float numpy.array
mix of joint angles and quaternions [quaternions & radians]
target : float numpy.array
mix of target joint angles and quaternions [quaternions & radians]
"""
# for each quaternion in the list, the output size is reduced by 1
q_tilde = np.zeros(len(q) - np.sum(self.quaternions))
q_index = 0
q_tilde_index = 0
for quat_bool in self.quaternions:
if quat_bool:
# if the joint position is a quaternion, calculate error
joint_q = q[q_index:q_index + 4]
error = quaternion_multiply(
target[q_index:q_index + 4],
quaternion_conjugate(joint_q),
)
# NOTE: we need to rotate this back to undo the current angle
# because the actuators don't rotate with the ball joint
u = quaternion_multiply(
quaternion_conjugate(joint_q),
quaternion_multiply(
error,
joint_q,
),
)
# convert from quaternion to 3D angular forces to apply
# https://studywolf.wordpress.com/2018/12/03/force-control-of-task-space-orientation/
q_tilde[q_tilde_index:q_tilde_index +
3] = u[1:] * np.sign(u[0])
q_index += 4
q_tilde_index += 3
else:
q_tilde[q_tilde_index] = self.q_tilde_angle(
q[q_index], target[q_index])
q_index += 1
q_tilde_index += 1
return q_tilde
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 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)
# 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
# R_EE = robot_config.R('EE', feedback['q'])
# # get rotation matrix for the target orientation
# R_d = transformations.euler_matrix(
# target[3], target[4], target[5], axes='rxyz')[:3, :3]
# R_ed = np.dot(R_EE.T, R_target) # eq 24
# q_e = transformations.quaternion_from_matrix(R_ed)
# q_e = transformations.unit_vector(q_e)
# u_task[3:] = np.dot(R_EE, q_e[1:]) # eq 34
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# send forces into Mujoco, step the sim forward
interface.send_forces(u)
# track data
q_track.append(np.copy(feedback["q"]))
target_track.append(np.copy(target))
# calculate the distance between quaternions
error = quaternion_multiply(
target,
quaternion_conjugate(feedback["q"]),
)
error = 2 * np.arctan2(
np.linalg.norm(error[1:]) * -np.sign(error[0]), error[0])
# quaternion distance for same angles can be 0 or 2*pi, so use a sine
# wave here so 0 and 2*np.pi = 0
error = np.sin(error / 2)
error_track.append(np.copy(error))
if abs(error) < threshold:
interface.sim.model.geom_rgba[target_geom_id] = green
count += 1
else:
count = 0
interface.sim.model.geom_rgba[target_geom_id] = red
# 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
# R_EE = robot_config.R('EE', feedback['q'])
# # get rotation matrix for the target orientation
# R_d = transformations.euler_matrix(
# target[3], target[4], target[5], axes='rxyz')[:3, :3]
# R_ed = np.dot(R_EE.T, R_target) # eq 24
# q_e = transformations.quaternion_from_matrix(R_ed)
# q_e = transformations.unit_vector(q_e)
# u_task[3:] = np.dot(R_EE, q_e[1:]) # eq 34
dq=feedback["dq"],
target=target,
)
# send forces into Mujoco, step the sim forward
interface.send_forces(u)
# track data
q_track.append(np.copy(feedback["q"]))
target_track.append(np.copy(target))
# calculate the distance between quaternions
error = 0.0
for ii in range(2):
temp = quaternion_multiply(
target[ii * 4:(ii * 4) + 4],
quaternion_conjugate(feedback["q"][ii * 4:(ii * 4) + 4]),
)
temp = 2 * np.arctan2(
np.linalg.norm(temp[1:]) * -np.sign(temp[0]), temp[0])
# quaternion distance for same angles can be 0 or 2*pi, so use a sine
# wave here so 0 and 2*np.pi = 0
error += np.sin(temp / 2)
error_track.append(np.copy(error))
if abs(error) < threshold:
interface.sim.model.geom_rgba[target_geom_id] = green
count += 1
else:
count = 0
interface.sim.model.geom_rgba[target_geom_id] = red