def compute_relative_imu_orientation(self):
"""name convention: parentFrame_childFrame_quat"""
cone_imu_vec_quat = tfms.quaternion_from_euler(0, 0, math.pi, 'rxyz')
earth_initImu_vec_quat = [
-0.71086415, 0.02551225, 0.00909408, 0.70280765
]
initImu_anchor_vec_quat = tfms.quaternion_from_euler(
math.pi / 2, 0, math.pi, 'rxyz')
earth_anchor_vec_quat = tfms.quaternion_multiply(
earth_initImu_vec_quat, initImu_anchor_vec_quat)
self._anchor_imu_vec_quat = tfms.quaternion_multiply(
tfms.quaternion_inverse(earth_anchor_vec_quat),
self._unit_imu_quaternion)
anchor_cone = tfms.quaternion_multiply(
self._anchor_imu_vec_quat,
tfms.quaternion_inverse(cone_imu_vec_quat))
self._relative_body_quaternion = Quaternion(anchor_cone[0],
anchor_cone[1],
anchor_cone[2],
anchor_cone[3])
def publish_relative_frames(self, data):
tmp_dict = {}
for segment in data.segments:
if(segment.is_quaternion):
tmp_dict[segment.id] = (segment.position,segment.quat)
else:
tmp_dict[segment.id] = (segment.position,segment.euler)
for idx in tmp_dict.keys():
p_idx = xsens_client.get_segment_parent_id(idx)
child_data = tmp_dict[idx]
if(p_idx==-1):
continue
elif(p_idx==0):
new_quat = tft.quaternion_multiply(child_data[1],tft.quaternion_inverse((1,1,1,1)))#if(segment.is_quaternion): TODO Handle Euler
#self.sendTransform(child_data[0],
self.sendTransform(child_data[0],
child_data[1],
#(1.0,0,0,0),#FIXME
rospy.Time.now(),
xsens_client.get_segment_name(idx),
self.ref_frame)
else:
parent_data = tmp_dict[p_idx]
parent_transform = tf.Transform(parent_data[1],parent_data[0])
child_transform = tf.Transform(child_data[1],child_data[0])
new_trans = parent_transform.inversetimes(child_transform)
new_quat = tft.quaternion_multiply(child_data[1],tft.quaternion_inverse(parent_data[1]))
tmp_pos = (child_data[0][0]-parent_data[0][0],child_data[0][1]-parent_data[0][1] ,child_data[0][2]-parent_data[0][2] )
#tmp_pos = (child_data[0][0] - parent_data[0][0],child_data[0][1] - parent_data[0][1],child_data[0][2] - parent_data[0][2])
#self.sendTransform(qv_mult(parent_data[1],tmp_pos),
#self.sendTransform(tmp_pos,
# new_quat,
#child_data[1],
self.sendTransform(new_trans.getOrigin(), new_trans.getRotation(),rospy.Time.now(),xsens_client.get_segment_name(idx),xsens_client.get_segment_name(p_idx))
def update_display(self):
self.clear_plot()
x = [1.0,0.0,0.0,0.0]
x2 = [0.8,0.0,0.0,0.0]
z = [0.8,0.0,0.1,0.0]
q = self.normalize_tf_quaternion([self.val.x, self.val.y, self.val.z,self.val.w])
xr = tft.quaternion_multiply(tft.quaternion_multiply(q,x),tft.quaternion_inverse(q))
x2r = tft.quaternion_multiply(tft.quaternion_multiply(q,x2),tft.quaternion_inverse(q))
zr = tft.quaternion_multiply(tft.quaternion_multiply(q,z),tft.quaternion_inverse(q))
self.plot_3d_ax.plot([0.0,0.5], [0.0,0.0], [0.0,0.0], 'r-')
self.plot_3d_ax.plot([0.0,0.0], [0.0,0.5], [0.0,0.0], 'g-')
self.plot_3d_ax.plot([0.0,0.0], [0.0,0.0], [0.0,0.5], 'b-')
self.plot_3d_ax.plot([0.0,xr[0]], [0.0,xr[1]], [0.0,xr[2]], 'k-', linewidth=2)
self.plot_3d_ax.plot([zr[0],xr[0]], [zr[1],xr[1]], [zr[2],xr[2]], 'k-', linewidth=2)
self.plot_3d_ax.plot([zr[0],x2r[0]], [zr[1],x2r[1]], [zr[2],x2r[2]], 'k-', linewidth=2)
self.plot_3d_ax.set_aspect('equal', 'box')
self.plot_3d_ax.set_xlim(-1.0, 1.0)
self.plot_3d_ax.set_ylim(-1.0, 1.0)
self.plot_3d_ax.set_zlim(-1.0, 1.0)
self.plot_3d_canvas.draw()
if not self.manual_mode:
self.update_normalized_displays()
def increment_reference(self):
'''
Steps the model reference (trajectory to track) by one self.timestep.
'''
# Frame management quantities
R_ref = trns.quaternion_matrix(self.q_ref)[:3, :3]
y_ref = trns.euler_from_quaternion(self.q_ref)[2]
# Convert body PD gains to world frame
kp = R_ref.dot(self.kp_body).dot(R_ref.T)
kd = R_ref.dot(self.kd_body).dot(R_ref.T)
# Compute error components (desired - reference), using "smartyaw"
p_err = self.p_des - self.p_ref
v_err = -self.v_ref
w_err = -self.w_ref
if npl.norm(p_err) <= self.heading_threshold:
q_err = trns.quaternion_multiply(
self.q_des, trns.quaternion_inverse(self.q_ref))
else:
q_direct = trns.quaternion_from_euler(
0, 0, np.angle(p_err[0] + (1j * p_err[1])))
q_err = trns.quaternion_multiply(
q_direct, trns.quaternion_inverse(self.q_ref))
y_err = trns.euler_from_quaternion(q_err)[2]
# Combine error components into error vectors
err = np.concatenate((p_err, [y_err]))
errdot = np.concatenate((v_err, [w_err]))
wrench = (kp.dot(err)) + (kd.dot(errdot))
# Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
B = np.concatenate((R_ref.dot(self.thruster_directions.T),
R_ref.dot(self.lever_arms.T)))
B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
command = self.thruster_mapper(wrench, B_3dof)
wrench_saturated = B.dot(command)
# Use model drag to find drag force on virtual boat
twist_body = R_ref.T.dot(np.concatenate((self.v_ref, [self.w_ref])))
D_body = np.zeros_like(twist_body)
for i, v in enumerate(twist_body):
if v >= 0:
D_body[i] = self.D_body_positive[i]
else:
D_body[i] = self.D_body_negative[i]
drag_ref = R_ref.dot(D_body * twist_body * abs(twist_body))
# Step forward the dynamics of the virtual boat
self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref
self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref
self.p_ref = self.p_ref + (self.v_ref * self.timestep)
self.q_ref = trns.quaternion_from_euler(
0, 0, y_ref + (self.w_ref * self.timestep))
self.v_ref = self.v_ref + (self.a_ref * self.timestep)
self.w_ref = self.w_ref + (self.aa_ref * self.timestep)
def calc_offset_angle(current):
"""Calculates the offset angle from the x axis positiion"""
x_axis = [1, 0, 0, 0]
a = quaternion_multiply(x_axis, quaternion_inverse(current))
rotation = quaternion_multiply(quaternion_multiply(a, x_axis), quaternion_inverse(a))
angle = atan2(rotation[1], rotation[0])
return angle
def increment_reference(self):
'''
Steps the model reference (trajectory to track) by one self.timestep.
'''
# Frame management quantities
R_ref = trns.quaternion_matrix(self.q_ref)[:3, :3]
y_ref = trns.euler_from_quaternion(self.q_ref)[2]
# Convert body PD gains to world frame
kp = R_ref.dot(self.kp_body).dot(R_ref.T)
kd = R_ref.dot(self.kd_body).dot(R_ref.T)
# Compute error components (desired - reference), using "smartyaw"
p_err = self.p_des - self.p_ref
v_err = -self.v_ref
w_err = -self.w_ref
if npl.norm(p_err) <= self.heading_threshold:
q_err = trns.quaternion_multiply(self.q_des, trns.quaternion_inverse(self.q_ref))
else:
q_direct = trns.quaternion_from_euler(0, 0, np.angle(p_err[0] + (1j * p_err[1])))
q_err = trns.quaternion_multiply(q_direct, trns.quaternion_inverse(self.q_ref))
y_err = trns.euler_from_quaternion(q_err)[2]
# Combine error components into error vectors
err = np.concatenate((p_err, [y_err]))
errdot = np.concatenate((v_err, [w_err]))
wrench = (kp.dot(err)) + (kd.dot(errdot))
# Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
B = np.concatenate((R_ref.dot(self.thruster_directions.T), R_ref.dot(self.lever_arms.T)))
B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
command = self.thruster_mapper(wrench, B_3dof)
wrench_saturated = B.dot(command)
# Use model drag to find drag force on virtual boat
twist_body = R_ref.T.dot(np.concatenate((self.v_ref, [self.w_ref])))
D_body = np.zeros_like(twist_body)
for i, v in enumerate(twist_body):
if v >= 0:
D_body[i] = self.D_body_positive[i]
else:
D_body[i] = self.D_body_negative[i]
drag_ref = R_ref.dot(D_body * twist_body * abs(twist_body))
# Step forward the dynamics of the virtual boat
self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref
self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref
self.p_ref = self.p_ref + (self.v_ref * self.timestep)
self.q_ref = trns.quaternion_from_euler(0, 0, y_ref + (self.w_ref * self.timestep))
self.v_ref = self.v_ref + (self.a_ref * self.timestep)
self.w_ref = self.w_ref + (self.aa_ref * self.timestep)
def _get_quaternion_error(self, q, target_q):
"""
Returns an angluar differnce between q and target_q in radians
"""
dq = trns.quaternion_multiply(np.array(target_q),
trns.quaternion_inverse(np.array(q)))
return 2 * np.arccos(dq[3])
def _generate_vel(self, s=None):
if self._stamped_pose_only:
return np.zeros(6)
cur_s = (self._cur_s if s is None else s)
last_s = cur_s - self.interpolator.s_step
if last_s < 0 or cur_s > 1:
return np.zeros(6)
q_cur = self.interpolator.generate_quat(cur_s)
q_last = self.interpolator.generate_quat(last_s)
cur_pos = self.interpolator.generate_pos(cur_s)
last_pos = self.interpolator.generate_pos(last_s)
########################################################
# Computing angular velocities
########################################################
# Quaternion difference to the last step in the inertial frame
q_diff = quaternion_multiply(q_cur, quaternion_inverse(q_last))
# Angular velocity
ang_vel = 2 * q_diff[0:3] / self._t_step
vel = [(cur_pos[0] - last_pos[0]) / self._t_step,
(cur_pos[1] - last_pos[1]) / self._t_step,
(cur_pos[2] - last_pos[2]) / self._t_step,
ang_vel[0],
ang_vel[1],
ang_vel[2]]
return np.array(vel)
def deltaPose(currPose, desPose, posFrame=None, rotFrame=None):
"""
Returns pose0 - pose1
"""
currPose, desPose = tfx.pose(currPose), tfx.pose(desPose)
currPoseFrame, desPoseFrame = currPose.frame, desPose.frame
currPos, desPos = currPose.position, desPose.position
currRot, desRot = currPose.orientation, desPose.orientation
if posFrame is not None and currPoseFrame is not None:
tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10)
currPos = tf_currPos_to_posFrame * currPos
tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10)
desPos = tf_desPos_to_posFrame * desPos
if rotFrame is not None and currPoseFrame is not None:
tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10)
currRot = tf_currRot_to_rotFrame * currRot
tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10)
desRot = tf_desRot_to_rotFrame * desRot
deltaPosition = desPos.array - currPos.array
currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion
deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat)
deltaPose = tfx.pose(deltaPosition, deltaQuat)
return deltaPose
def _generate_vel(self, s=None):
if self._stamped_pose_only:
return np.zeros(6)
cur_s = (self._cur_s if s is None else s)
last_s = cur_s - self.interpolator.s_step
if last_s < 0 or cur_s > 1:
return np.zeros(6)
q_cur = self.interpolator.generate_quat(cur_s)
q_last = self.interpolator.generate_quat(last_s)
cur_pos = self.interpolator.generate_pos(cur_s)
last_pos = self.interpolator.generate_pos(last_s)
########################################################
# Computing angular velocities
########################################################
# Quaternion difference to the last step in the inertial frame
q_diff = quaternion_multiply(q_cur, quaternion_inverse(q_last))
# Angular velocity
ang_vel = 2 * q_diff[0:3] / self._t_step
vel = [(cur_pos[0] - last_pos[0]) / self._t_step,
(cur_pos[1] - last_pos[1]) / self._t_step,
(cur_pos[2] - last_pos[2]) / self._t_step, ang_vel[0],
ang_vel[1], ang_vel[2]]
return np.array(vel)
def update(self, dt, desired, current):
(p, o), (p_dot, o_dot) = current
(desired_p, desired_o), (desired_p_dot, desired_o_dot), (desired_p_dotdot, desired_o_dotdot) = desired
world_from_body = transformations.quaternion_matrix(o)[:3, :3]
x_dot = numpy.concatenate([
world_from_body.dot(p_dot),
world_from_body.dot(o_dot),
])
# world_from_desiredbody = transformations.quaternion_matrix(desired_o)[:3, :3]
desired_x_dot = numpy.concatenate([
world_from_body.dot(desired_p_dot),
world_from_body.dot(desired_o_dot),
])
desired_x_dotdot = numpy.concatenate([
world_from_body.dot(desired_p_dotdot),
world_from_body.dot(desired_o_dotdot),
])
error_position_world = numpy.concatenate([
desired_p - p,
quat_to_rotvec(transformations.quaternion_multiply(
desired_o,
transformations.quaternion_inverse(o),
)),
])
if self.config['two_d_mode']:
error_position_world = error_position_world * [1, 1, 0, 0, 0, 1]
world_from_body2 = numpy.zeros((6, 6))
world_from_body2[:3, :3] = world_from_body2[3:, 3:] = world_from_body
# Permitting lambda assignment b/c legacy
body_gain = lambda x: world_from_body2.dot(x).dot(world_from_body2.T) # noqa
error_velocity_world = (desired_x_dot + body_gain(numpy.diag(self.config['k'])).dot(error_position_world)) - x_dot
if self.config['two_d_mode']:
error_velocity_world = error_velocity_world * [1, 1, 0, 0, 0, 1]
pd_output = body_gain(numpy.diag(self.config['ks'])).dot(error_velocity_world)
output = pd_output
if self.config['use_rise']:
rise_term_int = body_gain(numpy.diag(self.config['ks'] * self.config['alpha'])).dot(error_velocity_world) + \
body_gain(numpy.diag(self.config['beta'])).dot(numpy.sign(error_velocity_world))
self._rise_term = self._rise_term + dt / 2 * (rise_term_int + self._rise_term_int_prev)
self._rise_term_int_prev = rise_term_int
output = output + self._rise_term
else:
# zero rise term so it doesn't wind up over time
self._rise_term = numpy.zeros(6)
self._rise_term_int_prev = numpy.zeros(6)
output = output + body_gain(numpy.diag(self.config['accel_feedforward'])).dot(desired_x_dotdot)
output = output + body_gain(numpy.diag(self.config['vel_feedforward'])).dot(desired_x_dot)
# Permitting lambda assignment b/c legacy
wrench_from_vec = lambda output: (world_from_body.T.dot(output[0:3]), world_from_body.T.dot(output[3:6])) # noqa
return wrench_from_vec(pd_output), wrench_from_vec(output)
def plot_command_matrix_in_matplotlib(command_matrix, control_dimensions,
title):
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
from tf.transformations import (
quaternion_inverse,
quaternion_multiply,
)
fig = plt.figure()
ax = fig.gca(projection='3d')
for i in range(command_matrix.shape[0]):
qxyzw = command_matrix[i][qxyzw_idx]
uvw = quaternion_multiply(
qxyzw,
quaternion_multiply(
[1, 0, 0, 0],
quaternion_inverse(qxyzw),
))
x, y, z = command_matrix[i][pxyz_idx]
u, v, w = uvw[:3]
ax.quiver(x, y, z, u, v, w, length=0.01)
ax.set_title(title)
fig.show()
def update_attractor_rotvels(self):
diff_quat = tfs.quaternion_multiply(
self.goal_quats, tfs.quaternion_inverse(self.eef_quat))
diff_quat = diff_quat / np.linalg.norm(diff_quat)
self.theta_to_goals = 2 * math.acos(
diff_quat[3]) #0 to 2pi. only rotation in one direction.
if self.theta_to_goals > math.pi:
self.theta_to_goals -= 2 * math.pi
self.theta_to_goals = abs(self.theta_to_goals)
diff_quat = -diff_quat
self.pfieldrot_array = (np.zeros((1, 3)))[0]
norm_den = math.sqrt(1 - diff_quat[3] * diff_quat[3])
if norm_den < 0.001:
self.pfieldrot_array[0] = diff_quat[0]
self.pfieldrot_array[1] = diff_quat[1]
self.pfieldrot_array[2] = diff_quat[2]
else:
self.pfieldrot_array[0] = diff_quat[0] / norm_den
self.pfieldrot_array[1] = diff_quat[1] / norm_den
self.pfieldrot_array[2] = diff_quat[2] / norm_den
self.pfieldrot_array[:] = 0.375 * self.pfieldrot_array[:] #sclae the velocity
# if self.stage == 1:
# if abs(self.theta_to_goals) < 0.1:
# self.stage = 2
# self.update_stage()
# print "Got into stage 2"
# self.pfieldrot_array = (np.zeros((1,3)))[0]
# else:
# if abs(self.theta_to_goals) < 0.12:
# self.pfieldrot_array = (np.zeros((1,3)))[0]
if abs(self.theta_to_goals) < 0.12:
self.pfieldrot_array = (np.zeros((1, 3)))[0]
def compute_quaternion_euler_offset(original_quaternion, new_quaternion):
offset_quaternion = transformations.quaternion_multiply(
transformations.quaternion_inverse(new_quaternion),
original_quaternion)
yaw, pitch, roll = transformations.euler_from_quaternion(offset_quaternion,
axes='szyx')
return (abs(yaw) + abs(pitch) + abs(roll))
def inverse_transform(t):
"""
Return the inverse transformation of t
:param t: A transform [[x, y, z], [x, y, z, w]]
:return: t2 such as multiply_transform_(t, t2) = [[0, 0, 0], [0, 0, 0, 1]]
"""
return [quat_rotate(transformations.quaternion_inverse(t[1]), [-t[0][0], -t[0][1], -t[0][2]]), transformations.quaternion_inverse(t[1])]
def get_orientation_difference(self, desired_orientation):
"""
Finds difference between rigid body position and inputted position.
Utilizes the following formula:
difference = desired_orientation * inverse(current_orientation)
where difference, desired_orientation, and current_orientation are
quaternions, and inverse() denotes the quaternion inverse.
Parameters
----------
desired_orientation : QuaternionArray
Array of [x, y, z, w] of desired orientation
Returns
-------
difference: QuaternionArray
Difference between rigid bodies orientation and inputted orientation
"""
inverted = quaternion_inverse(self.orientation)
return quaternion_multiply(desired_orientation, inverted)
def get_velocity(self, prev_pose, cur_pose):
velocity = TwistStamped()
velocity.header.stamp = prev_pose.header.stamp
velocity.header.frame_id = prev_pose.header.frame_id
delta = (prev_pose.header.stamp - cur_pose.header.stamp).to_sec()
velocity.twist.linear.x = (cur_pose.pose.pose.position.x -
prev_pose.pose.pose.position.x) / delta
velocity.twist.linear.y = (cur_pose.pose.pose.position.y -
prev_pose.pose.pose.position.y) / delta
velocity.twist.linear.z = (cur_pose.pose.pose.position.z -
prev_pose.pose.pose.position.z) / delta
#get angular velocity
prev_pose_quat = numpy.array([
prev_pose.pose.pose.orientation.x,
prev_pose.pose.pose.orientation.y,
prev_pose.pose.pose.orientation.z,
prev_pose.pose.pose.orientation.w
],
dtype=numpy.float64)
cur_pose_quat = numpy.array([
cur_pose.pose.pose.orientation.x, cur_pose.pose.pose.orientation.y,
cur_pose.pose.pose.orientation.z, cur_pose.pose.pose.orientation.w
],
dtype=numpy.float64)
resudal_quat = cur_pose_quat - prev_pose_quat
der_quat = numpy.float64(2.0) * resudal_quat / numpy.float64(delta)
vel_quat = transformations.quaternion_multiply(
transformations.quaternion_inverse(prev_pose_quat), der_quat)
velocity.twist.angular.x, velocity.twist.angular.y, velocity.twist.angular.z = vel_quat.tolist(
)[0:3]
return velocity
def handle_odometry(self, odom):
self.last_odom = self.curr_odom
self.curr_odom = odom
if self.last_odom:
p_curr = np.array([
self.curr_odom.pose.pose.position.x,
self.curr_odom.pose.pose.position.y,
self.curr_odom.pose.pose.position.z
])
p_last = np.array([
self.last_odom.pose.pose.position.x,
self.last_odom.pose.pose.position.y,
self.last_odom.pose.pose.position.z
])
q_curr = np.array([
self.curr_odom.pose.pose.orientation.x,
self.curr_odom.pose.pose.orientation.y,
self.curr_odom.pose.pose.orientation.z,
self.curr_odom.pose.pose.orientation.w
])
q_last = np.array([
self.last_odom.pose.pose.orientation.x,
self.last_odom.pose.pose.orientation.y,
self.last_odom.pose.pose.orientation.z,
self.last_odom.pose.pose.orientation.w
])
rot_last = tr.quaternion_matrix(q_last)[0:3, 0:3]
p_last_curr = rot_last.transpose().dot(p_curr - p_last)
q_last_curr = tr.quaternion_multiply(tr.quaternion_inverse(q_last),
q_curr)
_, _, diff = tr.euler_from_quaternion(q_last_curr)
self.dtheta = diff % (2 * np.pi)
self.dx = (p_last_curr[0]) * 1000
self.dy = (p_last_curr[1]) * 1000
def deltaPose(currPose, desPose, posFrame=None, rotFrame=None):
"""
Returns pose0 - pose1
"""
currPose, desPose = tfx.pose(currPose), tfx.pose(desPose)
currPoseFrame, desPoseFrame = currPose.frame, desPose.frame
currPos, desPos = currPose.position, desPose.position
currRot, desRot = currPose.orientation, desPose.orientation
if posFrame is not None and currPoseFrame is not None:
tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10)
currPos = tf_currPos_to_posFrame * currPos
tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10)
desPos = tf_desPos_to_posFrame * desPos
if rotFrame is not None and currPoseFrame is not None:
tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10)
currRot = tf_currRot_to_rotFrame * currRot
tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10)
desRot = tf_desRot_to_rotFrame * desRot
deltaPosition = desPos.array - currPos.array
currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion
deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat)
deltaPose = tfx.pose(deltaPosition, deltaQuat)
return deltaPose
def handler(self, req):
#define messages
print("Message received")
res = MoveToWaypointResponse()
traj = Odometry()
rate = rospy.Rate(10)
while (self.boat_pos.size == 0):
rate.sleep()
pos = req.target_p.position
ori = req.target_p.orientation
#create boat trajectory
traj.header.frame_id = "enu"
traj.child_frame_id = "wamv/base_link"
traj.pose.pose.position.x = pos.x
traj.pose.pose.position.y = pos.y
traj.pose.pose.orientation.x = ori.x
traj.pose.pose.orientation.y = ori.y
traj.pose.pose.orientation.z = ori.z
traj.pose.pose.orientation.w = ori.w
print("sending trajectory")
self.pub.publish(traj)
#go to waypoint
#TODO: Account for orientation to determine when we are finished
x_err = 100
y_err = 100
yaw_err = 100
start_force_end = False
start_force_end_time = None
self.orientation = np.array([ori.x, ori.y, ori.z, ori.w])
while ( (x_err > self.x_thresh) or (y_err > self.y_thresh) or (yaw_err > self.yaw_thresh) ):
yaw_err = trns.euler_from_quaternion(trns.quaternion_multiply(
self.boat_ori, trns.quaternion_inverse(self.orientation)))[2]
x_err = abs(self.boat_pos[0] - pos.x)
y_err = abs(self.boat_pos[1] - pos.y)
if (x_err < 1) and (y_err < 1) and (yaw_err < 0.02) and not start_force_end:
start_force_end = True
start_force_end_time = rospy.get_rostime()
if start_force_end and ((rospy.get_rostime() - start_force_end_time).to_sec() > 10):
#Exit out of while loop after 10 seconds of being in same point
break
rate.sleep()
print("Arrived")
self.boat_pos = np.array([])
res.success = True
return res
def __sub__(self, other):
return numpy.concatenate([
self._p - other._p,
_rotvec_from_quat(transformations.quaternion_multiply(
self._q,
transformations.quaternion_inverse(other._q),
)),
])
def plot_command_matrix_in_matplotlib(command_matrix, control_dimensions, title):
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
from tf.transformations import (
quaternion_inverse,
quaternion_multiply,
)
fig = plt.figure()
ax = fig.gca(projection='3d')
#ax.set_xlim(0,1)
#ax.set_ylim(0,1)
#ax.set_zlim(0,1)
pxyz_idx = util.get_position_xyz_index(control_dimensions)
qxyzw_idx = util.get_quaternion_xyzw_index(control_dimensions)
for i in range(command_matrix.shape[0]):
qxyzw = command_matrix[i][qxyzw_idx]
uvw = quaternion_multiply(
qxyzw,
quaternion_multiply(
[1,0,0,0], quaternion_inverse(qxyzw),
)
)
uvw_ = quaternion_multiply(
qxyzw,
quaternion_multiply(
[0,1,0,0], quaternion_inverse(qxyzw),
)
)
uvw__ = quaternion_multiply(
qxyzw,
quaternion_multiply(
[0,0,1,0], quaternion_inverse(qxyzw),
)
)
x, y, z = command_matrix[i][pxyz_idx]
u, v, w = uvw[:3]
u_, v_, w_ = uvw_[:3]
u__, v__, w__ = uvw__[:3]
ax.quiver(x, y, z, u, v, w, length=0.003,color='red')
ax.quiver(x, y, z, u_, v_, w_, length=0.005,color='green')
ax.quiver(x, y, z, u__, v__, w__, length=0.002,color='blue')
ax.set_title(title)
fig.show()
def rotation_from(self, *args, **kwargs):
"""Get the rotation (as tb_angles) which, when applied to the input rotation
by right-multiplication, produces this rotation"""
other = tb_angles(*args, **kwargs)
out = tb_angles(
tft.quaternion_multiply(tft.quaternion_inverse(other.quaternion),
self.quaternion))
return out
def publish_relative_frames2(self, data):
tmp_dict = {}
for segment in data.segments:
if(segment.is_quaternion):
tmp_dict[segment.id] = (segment.position,segment.quat,self.tr.fromTranslationRotation(segment.position,segment.quat))
else:
tmp_dict[segment.id] = (segment.position,segment.euler)
for idx in tmp_dict.keys():
p_idx = xsens_client.get_segment_parent_id(idx)
child_data = tmp_dict[idx]
if(p_idx==-1):
continue
elif(p_idx==0):
helper = tft.quaternion_about_axis(1,(-1,0,0))
new_quat = tft.quaternion_multiply(tft.quaternion_inverse(helper),child_data[1])#if(segment.is_quaternion): TODO Handle Euler
#self.sendTransform(child_data[0],
self.sendTransform(child_data[0],
child_data[1],
#(1.0,0,0,0),#FIXME
rospy.Time.now(),
xsens_client.get_segment_name(idx),
self.ref_frame)
elif(p_idx>0):
parent_data = tmp_dict[p_idx]
new_quat = tft.quaternion_multiply(tft.quaternion_inverse(parent_data[1]),child_data[1])
tmp_trans = (parent_data[0][0] - child_data[0][0],parent_data[0][1] - child_data[0][1],parent_data[0][2] - child_data[0][2])
new_trans = qv_mult(parent_data[1],tmp_trans)
self.sendTransform(
new_trans,
new_quat,
rospy.Time.now(),
xsens_client.get_segment_name(idx),
xsens_client.get_segment_name(p_idx))
else:
parent_data = tmp_dict[p_idx]
this_data = np.multiply(tft.inverse_matrix(child_data[2]) , parent_data[2])
self.sendTransform(
tft.translation_from_matrix(this_data),
tft.quaternion_from_matrix(this_data),
rospy.Time.now(),
xsens_client.get_segment_name(idx),
xsens_client.get_segment_name(p_idx))
def getRobotGridPosition(transMsg, gridInfo):
pos = np.array([transMsg.transform.translation.x - gridInfo.origin.position.x, transMsg.transform.translation.y - gridInfo.origin.position.y, 0, 1])
quat = gridInfo.origin.orientation
mat = tft.quaternion_matrix(tft.quaternion_inverse([quat.x, quat.y, quat.z, quat.w]))
gridPos = (mat.dot(pos[np.newaxis].T).flatten()[:2]) / gridInfo.resolution
roundedPos = np.round(gridPos)
pos = roundedPos if np.allclose(gridPos, roundedPos) else np.floor(gridPos)
return pos
def plot_command_matrix_in_matplotlib(command_matrix):
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
from tf.transformations import (
quaternion_inverse,
quaternion_multiply,
)
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
pxyz_idx = [0, 1, 2]
qxyzw_idx = [3, 4, 5, 6]
for i in range(command_matrix.shape[0]):
qxyzw = command_matrix[i][qxyzw_idx]
uvw = quaternion_multiply(
qxyzw,
quaternion_multiply(
[1, 0, 0, 0],
quaternion_inverse(qxyzw),
))
uvw_ = quaternion_multiply(
qxyzw,
quaternion_multiply(
[0, 1, 0, 0],
quaternion_inverse(qxyzw),
))
uvw__ = quaternion_multiply(
qxyzw,
quaternion_multiply(
[0, 0, 1, 0],
quaternion_inverse(qxyzw),
))
x, y, z = command_matrix[i][pxyz_idx]
u, v, w = uvw[:3]
u_, v_, w_ = uvw_[:3]
u__, v__, w__ = uvw__[:3]
ax.quiver(x, y, z, u, v, w, length=0.01, color='red')
ax.quiver(x, y, z, u_, v_, w_, length=0.01, color='green')
ax.quiver(x, y, z, u__, v__, w__, length=0.01, color='blue')
fig.show()
plt.show()
def error(pose1,pose2):
xyzerr = pose1[0:3,:] - pose2[0:3,:]
rpy_list = []
for q1,q2 in zip(pose1[3:7,:].T,pose2[3:7,:].T):
quaterr = tf.quaternion_multiply(tf.quaternion_inverse(q2),q1)
eulererr = tf.euler_from_quaternion(quaterr)
rpy_list.append(eulererr)
rpy_list = np.array(rpy_list).T
return np.vstack((xyzerr,rpy_list))
def compute_imu_pose(self):
"""This function should compute pose of IMU in world frame"""
self._world_camera_position = [
-0.943419533992, -0.509536543674, 0.111264370752
]
# self._world_camera_quat = tfms.quaternion_from_euler(-math.pi/2,- math.pi/2, 0, 'rxyz')
world_camera_init = tfms.quaternion_from_euler(-math.pi / 2,
-math.pi / 2, 0, 'rxyz')
self._world_camera_quat = tfms.quaternion_multiply(
world_camera_init,
tfms.quaternion_from_euler(math.radians(-28.5), 0, 0, 'rxyz'))
camera_imu_position_cameraframe = [
self._object_pose_from_tag.position.x,
self._object_pose_from_tag.position.y,
self._object_pose_from_tag.position.z, 0
]
camera_imu_position = np.matmul(
tfms.quaternion_matrix(self._world_camera_quat),
camera_imu_position_cameraframe)
world_imu_position = [
self._world_camera_position[0] + camera_imu_position[0],
self._world_camera_position[1] + camera_imu_position[1],
self._world_camera_position[2] + camera_imu_position[2]
]
"Some additions here"
earth_initIMU_quat = [
-0.71889202, -0.00903345, -0.02179016, 0.69472142
]
earth_world_quat = tfms.quaternion_multiply(
earth_initIMU_quat,
tfms.quaternion_from_euler(math.pi / 2, 0, math.pi / 2, 'rxyz'))
# print(earth_world_quat)
# earth_world_quat = [ 0.0078735, -0.01568597, 0.67480203, 0.73779006]
#[-0.02563422, 0.02972348, 0.72386144, 0.68882801] -old one
world_imu_quat = tfms.quaternion_multiply(
tfms.quaternion_inverse(earth_world_quat),
self._unit_imu_quaternion)
self._world_imu_pose = Pose(
Point(world_imu_position[0], world_imu_position[1],
world_imu_position[2]),
Quaternion(world_imu_quat[0], world_imu_quat[1], world_imu_quat[2],
world_imu_quat[3]))
self.compute_object_pose(world_imu_position, world_imu_quat)
def get_control_cmd(self, cur_value, reference, eular=False):
if eular:
reference = quaternion_from_euler(*reference)
quaternion_diff = quaternion_multiply(reference, quaternion_inverse(cur_value))
euler_diff = np.array(euler_from_quaternion(quaternion_diff))
control_cmd = self.k_p * euler_diff
return list(control_cmd)
def getDiff(self,futur=0,past=1):
if len(self.TransList)-futur < 0 or len(self.TransList)-past < 0:
ROS_ERROR("trying to see past the buffer size")
return 0
else:
euler = trans.euler_from_quaternion(self.rotList[-futur]*trans.quaternion_inverse(self.rotList[-past]))
TransVect = (self.TransList[-futur][0]-self.TransList[-past][0],
self.TransList[-futur][1]-self.TransList[-past][1],
self.TransList[-futur][2]-self.TransList[-past][2])
return (TransVect,euler)
def on_imu_msg(self, msg):
if self.initial_orientation is None:
self.initial_orientation = msg.orientation
return
current_orientation = transformations.quaternion_multiply(
quat_msg_to_array(msg.orientation),
transformations.quaternion_inverse(
quat_msg_to_array(self.initial_orientation)))
data = transformations.euler_from_quaternion(current_orientation)
self.orientation = Vector3(data[0], data[1], data[2])
def on_imu_msg(self, msg):
if self.initial_orientation is None:
self.initial_orientation = msg.orientation
return
current_orientation = transformations.quaternion_multiply(
quat_msg_to_array(msg.orientation),
transformations.quaternion_inverse(quat_msg_to_array(self.initial_orientation)))
print(map(lambda v: degrees(v), transformations.euler_from_quaternion(current_orientation)))
print(map(lambda v: degrees(v), transformations.euler_from_quaternion(quat_msg_to_array(msg.orientation))))
print("")
def testRotation():
rospy.init_node('ar_image_detection')
imageDetector = ARImageDetector()
listener = tf.TransformListener()
tf_br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(Constants.Arm.Left):
obj = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped()
gripper = imageDetector.getGripperPose(Constants.Arm.Left)
print('gripper')
print(gripper)
# obj_tb = tfx.tb_angles(obj.pose.orientation)
gripper_tb = tfx.tb_angles(gripper.pose.orientation)
print "gripper ori", gripper_tb
obj_tb = tfx.tb_angles(imageDetector.normal)
print "obj ori", obj_tb
pt = gripper.pose.position
ori = imageDetector.normal
tf_br.sendTransform((pt.x, pt.y, pt.z), (ori.x, ori.y, ori.z, ori.w),
gripper.header.stamp, '/des_pose', Constants.AR.Frames.Base)
between = Util.angleBetweenQuaternions(ori, gripper_tb.msg)
print('Angle between')
print(between)
quat = tft.quaternion_multiply(gripper_tb.quaternion, tft.quaternion_inverse(obj_tb.quaternion))
print 'new', tfx.tb_angles(quat)
#rot = gripper_tb.rotation_to(ori)
rot = gripper_tb.rotation_to(obj_tb)
print('Rotation from gripper to obj')
print(rot)
deltaPoseTb = tfx.pose(Util.deltaPose(gripper, obj)).orientation
print('deltaPose')
print(deltaPoseTb)
deltaPose = tfx.pose([0,0,0], deltaPoseTb).msg.PoseStamped()
time = listener.getLatestCommonTime('0_link', 'tool_L')
deltaPose.header.stamp = time
deltaPose.header.frame_id = '0_link'
deltaPose = listener.transformPose('tool_L', deltaPose)
print "transformed", tfx.tb_angles(deltaPose.pose.orientation)
endQuatMat = gripper_tb.matrix * rot.matrix
print 'desOri', tfx.tb_angles(endQuatMat)
rospy.sleep(1)
def update(self, now, accel, gyro, quat):
self.accel = accel
self.gyro = gyro
w, x, y, z = quat
# w, x, y, z = -w, y, x, z
w, x, y, z = w, -y, x, z
quat = w, x, y, z
# quat = t.quaternion_multiply(t.quaternion_about_axis(math.pi, [0, 0, 1]), quat)
self.quat_absolute = quat
# create relative quat, based on our absolute quaternion and our parent's absolute quaternion
self.quat_relative = (t.quaternion_multiply(
self.quat_absolute, t.quaternion_inverse(
self.parent.quat_absolute))
if self.parent else self.quat_absolute)
self.quat_relative = t.quaternion_multiply(
self.quat_relative, t.quaternion_about_axis(math.pi, [1, 0, 0]))
# euler = t.euler_from_quaternion(quat_relative, axes='syzx')
# quat_relative = t.quaternion_from_euler(euler[0], euler[1], -euler[2], axes='sxyz')
# self.quat_relative = t.quaternion_multiply(quat_relative, t.quaternion_inverse(self.parent.quat_relative))
if self.lastUpdatedAt and self.relative_average_rate:
dt = (now - self.lastUpdatedAt).to_sec()
f = dt * self.relative_average_rate
self.quat_average = self.quat_relative \
if self.quat_average is None \
else t.quaternion_slerp(self.quat_average, self.quat_relative, f)
self.quat_relative_to_average = t.quaternion_multiply(
self.quat_relative, t.quaternion_inverse(self.quat_average))
else:
self.quat_average = self.quat_relative
self.quat_relative_to_average = self.quat_relative
self.lastUpdatedAt = now
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0, 1, 2], 3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def computeGraspToPickMatrix(self):
pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation
grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation
pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance
pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w]
grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w]
pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion)
rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion)
translation = [pick_translation_distance, 0, 0]
rotation_matrix[[0,1,2],3] = translation
pick_to_grasp_matrix = np.mat(rotation_matrix)
grasp_to_pick_matrix = pick_to_grasp_matrix.getI()
return grasp_to_pick_matrix
def reset(self, msg):
#save old physics properties turn off gravity
physics_properties = self.get_physics_properties()
old_gravity = physics_properties.gravity.z
physics_properties.gravity.z = 0
self.set_physics_properties(physics_properties.time_step, physics_properties.max_update_rate, physics_properties.gravity, physics_properties.ode_config)
#teleport robot
self.set_model_state(ModelState(self.model_name, Pose(Point(0, 0, 2), Quaternion()), Twist(), self.world_frame))
#set initial positions, wait for the joints to converge
self.fieldCommandPub.publish(msg)
robot_pose_sub = rospy.Subscriber("/gazebo/joint_states", JointState, self.processRobotPose)
prev_joint_position = msg.position
while not rospy.is_shutdown():
time.sleep(0.1)
diff = np.mean([(math.fabs(msg.position[i] - self.joints[i]) + math.fabs(msg.position[i] - prev_joint_position[i])) for i in xrange(len(msg.setThisJoint)) if msg.setThisJoint[i]])
print "Diff: ", diff
if diff < 0.025:
break
robot_pose_sub.unregister()
#rotate the robot, so that feet will get on the ground
newAtlasPose = self.get_model_state(self.model_name, self.world_frame).pose
footOrientation = self.get_link_state(self.link_names[0], self.world_frame).link_state.pose.orientation
a = [newAtlasPose.orientation.x, newAtlasPose.orientation.y, newAtlasPose.orientation.z, newAtlasPose.orientation.w]
b = [footOrientation.x, footOrientation.y, footOrientation.z, footOrientation.w]
res = t.quaternion_multiply(a, t.quaternion_inverse(b))
newAtlasPose.orientation = Quaternion(res[0], res[1], res[2], res[3])
self.set_model_state(ModelState(self.model_name, newAtlasPose, Twist(), self.world_frame))
#extract foot positions and offset them by foot_z_offset
newAtlasPose = self.get_model_state(self.model_name, self.world_frame).pose
footPosition = self.get_link_state(self.link_names[0], self.world_frame).link_state.pose.position
newAtlasPose.position.z = newAtlasPose.position.z - footPosition.z + self.foot_z_offsets[0] + 0.01
#teleport the robot down
self.set_model_state(ModelState(self.model_name, newAtlasPose, Twist(), self.world_frame))
#turn on gravity
physics_properties.gravity.z = old_gravity
self.set_physics_properties(physics_properties.time_step, physics_properties.max_update_rate, physics_properties.gravity, physics_properties.ode_config)
self.resetDonePub.publish(Empty())
self.recordPerformance()
def pose_err(self, ps1, ps2):
p1 = [ps1.pose.position.x, ps1.pose.position.y, ps1.pose.position.z]
p2 = [ps2.pose.position.x, ps2.pose.position.y, ps2.pose.position.z]
d_err = np.linalg.norm(np.subtract(p1,p2))
q1 = [ps1.pose.orientation.x, ps1.pose.orientation.y,
ps1.pose.orientation.z, ps1.pose.orientation.w]
q2 = [ps2.pose.orientation.x, ps2.pose.orientation.y,
ps2.pose.orientation.z, ps2.pose.orientation.w]
q1_inv = tft.quaternion_inverse(q1)
q_err = tft.quaternion_multiply(q2, q1_inv)
euler_angles = tft.euler_from_quaternion(q_err)
theta_err = np.linalg.norm(euler_angles)
return d_err, theta_err
def position_control(user_data, self):
if self.inside_workspace(self.master_pos):
self.command_pos = self.slave_synch_pos + self.master_pos / self.position_ratio
# TODO: Rotations are driving me crazy!
relative_rot = tr.quaternion_multiply(self.master_synch_rot, tr.quaternion_inverse(self.master_rot))
self.command_rot = tr.quaternion_multiply(self.slave_synch_rot, relative_rot)
#~ self.command_rot = np.array(self.master_rot)
return 'stay'
else:
self.command_pos = np.array(self.slave_pos)
self.command_rot = np.array(self.slave_rot)
self.vib_start_time = rospy.get_time()
self.loginfo('State machine transitioning: POSITION_CONTROL:leave-->VIBRATORY_PHASE')
return 'leave'
def test_triad():
q = transformations.random_quaternion()
a = np.random.standard_normal(3)
b = np.random.standard_normal(3)
m = transformations.quaternion_matrix(q)[:3, :3]
q_ = triad((m.dot(a), m.dot(b)), (a, b))
assert np.linalg.norm(quat_to_rotvec(
transformations.quaternion_multiply(
q,
transformations.quaternion_inverse(q_),
)
)) < 1e-6
def rate_control(user_data, self):
if not self.inside_workspace(self.master_pos):
# Send the force feedback to the master
self.force_feedback = (self.k_rate * self.master_pos + self.b_rate * self.master_vel) * -1.0
# Send the rate command to the slave
distance = sqrt(np.sum((self.master_pos - self.rate_pivot) ** 2)) / self.position_ratio
self.command_pos += (self.rate_gain * distance * self.normalize_vector(self.master_pos)) / self.position_ratio
relative_rot = tr.quaternion_multiply(self.master_synch_rot, tr.quaternion_inverse(self.master_rot))
self.command_rot = tr.quaternion_multiply(self.slave_synch_rot, relative_rot)
return 'stay'
else:
self.command_pos = np.array(self.slave_pos)
self.command_rot = np.array(self.slave_rot)
self.force_feedback = np.zeros(3)
self.loginfo('State machine transitioning: RATE_CONTROL:leave-->GO_TO_CENTER')
return 'leave'
def convert_pose_inverse_transform(pose):
""" Helper method to invert a transform (this is built into the tf C++ classes, but ommitted from Python) """
translation = np.zeros((4,1))
translation[0] = -pose.position.x
translation[1] = -pose.position.y
translation[2] = -pose.position.z
translation[3] = 1.0
rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
rotation_inverse = quaternion_inverse(rotation)
rot_mat = quaternion_matrix(rotation_inverse)
transformed_translation = rot_mat.dot(translation)
translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2])
rotation = rotation_inverse
return (translation, rotation)
def cross_correlation_mat(self, est_mean, est_sigmas, meas_mean, meas_sigmas):
cross_correlation_mat = zeros((est_mean.shape[0], meas_mean.shape[0]))
est_diff = zeros(est_mean.shape)
mean_quat = SF9DOF_UKF.recover_quat(est_mean)
inverse_mean_quat = tf_math.quaternion_inverse(mean_quat.flatten())
for i in range(0,self.n*2+1):
#Calculate the error for the quaternion
sig_quat = SF9DOF_UKF.recover_quat(est_sigmas[i])
error_quat = tf_math.quaternion_multiply(sig_quat, \
inverse_mean_quat)
est_diff[0:3,0] = error_quat[0:3,0]
#Calculate the error for the rest of the state
est_diff[3:,0] = est_sigmas[i][3:,0] - est_mean[3:,0]
meas_diff = meas_sigmas[i] - meas_mean
prod = dot(est_diff, meas_diff.T)
term = self.weight_covariance[i] * prod
cross_correlation_mat += term
return cross_correlation_mat
def estimate_covariance(self, est_mean, transformed_sigmas):
est_covariance = zeros(self.kalman_covariance.shape)
diff = zeros(est_mean.shape)
#Calculate the inverse mean quat
mean_quat = SF9DOF_UKF.recover_quat(est_mean)
inverse_mean_quat = tf_math.quaternion_inverse(mean_quat.flatten())
for i in range(0,self.n*2+1):
#Calculate the error for the quaternion
sig_quat = SF9DOF_UKF.recover_quat(transformed_sigmas[i])
error_quat = tf_math.quaternion_multiply(sig_quat, \
inverse_mean_quat)
diff[0:3,0] = error_quat[0:3,0]
#Calculate the error for the rest of the state
diff[3:,0] = transformed_sigmas[i][3:,0] - est_mean[3:,0]
prod = dot(diff, diff.T)
term = self.weight_covariance[i] * prod
est_covariance += term
return est_covariance
def get_pose_error(self, goal_pose, curr_pose):
dx = curr_pose.pose.position.x - goal_pose.pose.position.x
dy = curr_pose.pose.position.y - goal_pose.pose.position.y
dz = curr_pose.pose.position.z - goal_pose.pose.position.z
pos_err = np.linalg.norm((dx, dy, dz))
goal_q = (goal_pose.pose.orientation.x,
goal_pose.pose.orientation.y,
goal_pose.pose.orientation.z,
goal_pose.pose.orientation.w)
curr_q = (curr_pose.pose.orientation.x,
curr_pose.pose.orientation.y,
curr_pose.pose.orientation.z,
curr_pose.pose.orientation.w)
#goal_q = curr_q * delta_q -> delta_q = curr_q^-1 * goal_q
cq_inv = tft.quaternion_inverse(curr_q)
dq = tft.quaternion_multiply(cq_inv, goal_q)
#theta = 2*arctan2( ||X||, w)
q_vec_norm = np.linalg.norm(dq[:3])
rot_err = 2*np.arctan2(q_vec_norm, dq[3])
return pos_err, rot_err
def euler_from_matrix(parent_quat, child_quat):
q_relative = tr.quaternion_multiply(tr.quaternion_inverse(parent_quat), child_quat)
R = tr.quaternion_matrix(q_relative)
if not nearly_equal(abs(R[2][0]), 1.0):
p1 = -math.asin(R[2][0])
p2 = math.pi - p1
r1 = math.atan2(R[2][1]/math.cos(p1), R[2][2]/math.cos(p1))
r2 = math.atan2(R[2][1]/math.cos(p2), R[2][2]/math.cos(p2))
y1 = math.atan2(R[1][0]/math.cos(p1), R[1][0]/math.cos(p1))
y2 = math.atan2(R[1][0]/math.cos(p2), R[1][0]/math.cos(p2))
r = r2
p = p2
y = y2
else:
y = 0
if nearly_equal(R[2][0], -1.0):
p = math.pi/2
r = y + math.atan2(R[0][1], R[0][2])
else:
p = -math.pi/2
r = -y + math.atan2(-R[0][1], -R[0][2])
return [r,p,y]
def update_errors(self):
if not self.odom_is_init:
self._logger.warning('Odometry topic has not been update yet')
return
self._update_reference()
# Calculate error in the BODY frame
self._update_time_step()
# Rotation matrix from INERTIAL to BODY frame
rotItoB = self._vehicle_model.rotItoB
rotBtoI = self._vehicle_model.rotBtoI
if self._dt > 0:
# Update position error with respect to the the BODY frame
pos = self._vehicle_model.pos
vel = self._vehicle_model.vel
quat = self._vehicle_model.quat
self._errors['pos'] = np.dot(
rotItoB, self._reference['pos'] - pos)
# Update orientation error
self._errors['rot'] = quaternion_multiply(
quaternion_inverse(quat), self._reference['rot'])
# Velocity error with respect to the the BODY frame
self._errors['vel'] = np.hstack((
np.dot(rotItoB, self._reference['vel'][0:3]) - vel[0:3],
np.dot(rotItoB, self._reference['vel'][3:6]) - vel[3:6]))
if self._error_pub.get_num_connections() > 0:
stamp = rospy.Time.now()
msg = TrajectoryPoint()
msg.header.stamp = stamp
msg.header.frame_id = self._local_planner.inertial_frame_id
# Publish pose error
msg.pose.position = Vector3(*np.dot(rotBtoI, self._errors['pos']))
msg.pose.orientation = Quaternion(*self._errors['rot'])
# Publish velocity errors in INERTIAL frame
msg.velocity.linear = Vector3(*np.dot(rotBtoI, self._errors['vel'][0:3]))
msg.velocity.angular = Vector3(*np.dot(rotBtoI, self._errors['vel'][3:6]))
self._error_pub.publish(msg)
def qerror(self, q1, q2): """ Returns rotation vector representing the orientation error from quaternion q1 to q2. """ # Quaternion from q1 to q2 qdiff = trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)) # Renormalize for accuracy qdiff = trns.unit_vector(qdiff) # If "amount of rotation" is negative, flip quaternion if qdiff[3] < 0: qdiff = -qdiff # Extract axis if not np.allclose(qdiff[:3], 0): axis = trns.unit_vector(qdiff[:3]) else: axis = np.array([0, 0, 0]) # Extract angle angle = 2 * np.arccos(qdiff[3]) # Return rotvec return angle * axis
def get_euler_angles(parent_quat, child_quat):
q_relative = tr.quaternion_multiply(tr.quaternion_inverse(parent_quat), child_quat)
qw = q_relative[3]
qx = q_relative[0]
qy = q_relative[1]
qz = q_relative[2]
test = qx*qy + qz*qw
if (test > 0.499):
roll = 2.0 * math.atan2(qx,qw)
pitch = math.pi/2
yaw = 0
elif (test < -0.499):
roll = -2.0 * math.atan2(qx, qw)
pitch = -math.pi/2
yaw = 0
else:
sqx = qx**2;
sqy = qy**2;
sqz = qz**2;
roll = math.atan2(2*qy*qw-2*qx*qz , 1 - 2*sqy - 2*sqz)
pitch = math.asin(2*test)
yaw = math.atan2(2*qx*qw-2*qy*qz , 1 - 2*sqx - 2*sqz)
return [roll, pitch, yaw]
def _get_quaternion_error(self, q, target_q):
"""
Returns an angluar differnce between q and target_q in radians
"""
dq = trns.quaternion_multiply(np.array(target_q), trns.quaternion_inverse(np.array(q)))
return 2 * np.arccos(dq[3])
def get_command(self, msg):
'''
Publishes the wrench for this instant.
(Note: this is called get_command because it used to be used for
getting the actual thruster values, but now it is only being
used for getting the wrench which is then later mapped elsewhere).
'''
if self.p_ref is None:
return # C3 is killed
# Compute timestep from interval between this message's stamp and last's
if self.last_odom is None:
self.last_odom = msg
else:
self.timestep = (msg.header.stamp - self.last_odom.header.stamp).to_sec()
self.last_odom = msg
# ROS read-in
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
lin_vel_body = msg.twist.twist.linear
ang_vel_body = msg.twist.twist.angular
# ROS unpack
self.position = np.array([position.x, position.y])
self.orientation = np.array([orientation.x, orientation.y, orientation.z, orientation.w])
# Frame management quantities
R = np.eye(3)
R[:2, :2] = trns.quaternion_matrix(self.orientation)[:2, :2]
y = trns.euler_from_quaternion(self.orientation)[2]
# More ROS unpacking, converting body frame twist to world frame lin_vel and ang_vel
self.lin_vel = R.dot(np.array([lin_vel_body.x, lin_vel_body.y, lin_vel_body.z]))[:2]
self.ang_vel = R.dot(np.array([ang_vel_body.x, ang_vel_body.y, ang_vel_body.z]))[2]
# Convert body PD gains to world frame
kp = R.dot(self.kp_body).dot(R.T)
kd = R.dot(self.kd_body).dot(R.T)
# Compute error components (reference - true)
p_err = self.p_ref - self.position
y_err = trns.euler_from_quaternion(trns.quaternion_multiply(self.q_ref, trns.quaternion_inverse(self.orientation)))[2]
v_err = self.v_ref - self.lin_vel
w_err = self.w_ref - self.ang_vel
# Combine error components into error vectors
err = np.concatenate((p_err, [y_err]))
errdot = np.concatenate((v_err, [w_err]))
# Compute "anticipation" feedforward based on the boat's inertia
inertial_feedforward = np.concatenate((self.a_ref, [self.aa_ref])) * [self.mass_ref, self.mass_ref, self.inertia_ref]
# Compute the "learning" matrix
drag_regressor = np.array([[ self.lin_vel[0]*np.cos(y)**2 + self.lin_vel[1]*np.sin(y)*np.cos(y), self.lin_vel[0]/2 - (self.lin_vel[0]*np.cos(2*y))/2 - (self.lin_vel[1]*np.sin(2*y))/2, -self.ang_vel*np.sin(y), -self.ang_vel*np.cos(y), 0],
[self.lin_vel[1]/2 - (self.lin_vel[1]*np.cos(2*y))/2 + (self.lin_vel[0]*np.sin(2*y))/2, self.lin_vel[1]*np.cos(y)**2 - self.lin_vel[0]*np.cos(y)*np.sin(y), self.ang_vel*np.cos(y), -self.ang_vel*np.sin(y), 0],
[ 0, 0, self.lin_vel[1]*np.cos(y) - self.lin_vel[0]*np.sin(y), - self.lin_vel[0]*np.cos(y) - self.lin_vel[1]*np.sin(y), self.ang_vel]])
# wrench = PD + feedforward + I + adaptation
if self.only_PD:
wrench = (kp.dot(err)) + (kd.dot(errdot))
self.drag_effort = [0, 0, 0]
self.dist_est = [0, 0, 0]
else:
self.drag_effort = np.clip(drag_regressor.dot(self.drag_est), -self.drag_limit, self.drag_limit)
wrench = (kp.dot(err)) + (kd.dot(errdot)) + inertial_feedforward + self.dist_est + self.drag_effort
# Update disturbance estimate, drag estimates
if self.learn and (npl.norm(p_err) < self.learning_radius):
self.dist_est = np.clip(self.dist_est + (self.ki * err * self.timestep), -self.dist_limit, self.dist_limit)
self.drag_est = self.drag_est + (self.kg * (drag_regressor.T.dot(err + errdot)) * self.timestep)
# Update model reference for the next call
if not self.use_external_tgen:
self.increment_reference()
# convert wrench to body frame
wrench_body = R.T.dot(wrench)
# NOT NEEDED SINCE WE ARE USING A DIFFERENT NODE FOR ACTUAL THRUSTER MAPPING
# # Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
# B = np.concatenate((R.dot(self.thruster_directions.T), R.dot(self.lever_arms.T)))
# B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
# command = self.thruster_mapper(wrench, B_3dof)
# Give wrench to ROS
wrench_msg = WrenchStamped()
wrench_msg.header.frame_id = "/base_link"
wrench_msg.wrench.force.x = wrench_body[0]
wrench_msg.wrench.force.y = wrench_body[1]
wrench_msg.wrench.torque.z = wrench_body[2]
self.wrench_pub.publish(wrench_msg)
# Publish reference pose for examination
self.pose_ref_pub.publish(PoseStamped(
header=Header(
frame_id='/enu',
stamp=msg.header.stamp,
),
pose=Pose(
position=Point(self.p_ref[0], self.p_ref[1], 0),
orientation=Quaternion(*self.q_ref),
),
))
# Publish adaptation (Y*theta) for plotting
adaptation_msg = WrenchStamped()
adaptation_msg.header.frame_id = "/base_link"
adaptation_msg.wrench.force.x = (self.dist_est + self.drag_effort)[0]
adaptation_msg.wrench.force.y = (self.dist_est + self.drag_effort)[1]
adaptation_msg.wrench.torque.z = (self.dist_est + self.drag_effort)[2]
self.adaptation_pub.publish(adaptation_msg)
def inverse(self):
return Transform(
-self._q_mat.T.dot(self._p),
transformations.quaternion_inverse(self._q),
)
def _body_y_position(pos, traj_start):
R_nav2body = transformations.quaternion_matrix(
transformations.quaternion_inverse(traj_start.orientation))[:3, :3]
return R_nav2body.dot(pos)[1]
def rotation_from(self,*args,**kwargs): """Get the rotation (as tb_angles) which, when applied to the input rotation by right-multiplication, produces this rotation""" other = tb_angles(*args,**kwargs) out = tb_angles(tft.quaternion_multiply(tft.quaternion_inverse(other.quaternion), self.quaternion)) return out
def fill_points(tfl):
try:
global user
frame = "/openni_link" #"camera_link"
allFramesString = tfl.getFrameStrings()
onlyUsers = set([line for line in allFramesString if 'right_elbow_' in line])
n = len('right_elbow_')
userIDs = [el[n:] for el in onlyUsers]
user = ''
if len(userIDs) > 0:
mostRecentUID = userIDs[0]
mostRecentTime = tfl.getLatestCommonTime(frame, 'right_elbow_' + mostRecentUID).to_sec()
for uid in userIDs:
compTime = tfl.getLatestCommonTime(frame, 'right_elbow_' + uid).to_sec()
#rospy.loginfo("Diff time " + str(rospy.get_rostime().to_sec() - compTime))
if compTime >= mostRecentTime and rospy.get_rostime().to_sec() - compTime < 5:
user = uid
mostRecentTime = compTime
global left_arm_origin
global right_arm_origin
global head_origin
global head_point
global left_arm_point
global right_arm_point
global left_foot
global right_foot
(to_left_elbow,_) = tfl.lookupTransform(frame,"/left_elbow_" + user, rospy.Time(0))
(to_right_elbow,_) = tfl.lookupTransform(frame,"/right_elbow_" + user, rospy.Time(0))
(to_left_hand,_) = tfl.lookupTransform(frame,"/left_hand_" + user, rospy.Time(0))
(to_right_hand,_) = tfl.lookupTransform(frame,"/right_hand_" + user, rospy.Time(0))
(right_foot,_) = tfl.lookupTransform(frame, "/right_foot_" + user, rospy.Time(0))
(left_foot,_) = tfl.lookupTransform(frame, "/left_foot_" + user, rospy.Time(0))
(to_head,head_rot) = tfl.lookupTransform(frame,"/head_" + user, rospy.Time(0))
left_arm_origin = to_left_hand
left_arm_point = add_vec(to_left_hand, sub_vec(to_left_hand, to_left_elbow))
right_arm_origin = to_right_hand
right_arm_point = add_vec(to_right_hand, sub_vec(to_right_hand, to_right_elbow))
head_origin = to_head
head_temp = dot((0.0,0.0,-1.0,1.0), quaternion_matrix(quaternion_inverse(head_rot)))
head_point = (head_temp[0] + to_head[0], head_temp[1] + to_head[1], head_temp[2] + to_head[2])
#visualization for testing (verify head vector)
# marker = Marker()
# marker.header.frame_id = "camera_link"
# marker.header.stamp = rospy.Time(0)
# marker.type = marker.POINTS
# marker.action = marker.ADD
# marker.scale.x = 0.2
# marker.scale.y = 0.2
# marker.scale.z = 0.2
# marker.color.a = 1.0
# p1 = Point(right_arm_origin[0],right_arm_origin[1],right_arm_origin[2])
# p2 = Point(right_arm_point[0],right_arm_point[1],right_arm_point[2])
# p3 = Point(left_arm_origin[0],left_arm_origin[1],left_arm_origin[2])
# p4 = Point(left_arm_point[0],left_arm_point[1],left_arm_point[2])
# p5 = Point(head_origin[0],head_origin[1],head_origin[2])
# p6 = Point(head_point[0],head_point[1],head_point[2])
# marker.points += [p1, p2, p3, p4, p5, p6]
# pub.publish(marker)
return True
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
left_arm_point = None
right_arm_origin = None
left_arm_origin = None
right_arm_point = None
head_point = None
head_origin = None
left_foot = None
right_foot = None
return False
def odometry_callback(self, msg):
"""Handle odometry callback: The actual control loop."""
# Update local planner's vehicle position and orientation
pos = [msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z]
quat = [msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w]
self.local_planner.update_vehicle_pose(pos, quat)
# Compute the desired position
t = rospy.Time.now().to_sec()
des = self.local_planner.interpolate(t)
# Publish the reference
ref_msg = TrajectoryPoint()
ref_msg.header.stamp = rospy.Time.now()
ref_msg.header.frame_id = self.local_planner.inertial_frame_id
ref_msg.pose.position = geometry_msgs.Vector3(*des.p)
ref_msg.pose.orientation = geometry_msgs.Quaternion(*des.q)
ref_msg.velocity.linear = geometry_msgs.Vector3(*des.vel[0:3])
ref_msg.velocity.angular = geometry_msgs.Vector3(*des.vel[3::])
self.reference_pub.publish(ref_msg)
p = self.vector_to_np(msg.pose.pose.position)
forward_vel = self.vector_to_np(msg.twist.twist.linear)
ref_vel = des.vel[0:3]
q = self.quaternion_to_np(msg.pose.pose.orientation)
rpy = trans.euler_from_quaternion(q, axes='sxyz')
# Compute tracking errors wrt world frame:
e_p = des.p - p
abs_pos_error = np.linalg.norm(e_p)
abs_vel_error = np.linalg.norm(ref_vel - forward_vel)
# Generate error message
error_msg = TrajectoryPoint()
error_msg.header.stamp = rospy.Time.now()
error_msg.header.frame_id = self.local_planner.inertial_frame_id
error_msg.pose.position = geometry_msgs.Vector3(*e_p)
error_msg.pose.orientation = geometry_msgs.Quaternion(
*trans.quaternion_multiply(trans.quaternion_inverse(q), des.q))
error_msg.velocity.linear = geometry_msgs.Vector3(*(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear)))
error_msg.velocity.angular = geometry_msgs.Vector3(*(des.vel[3::] - self.vector_to_np(msg.twist.twist.angular)))
# Based on position tracking error: Compute desired orientation
pitch_des = -math.atan2(e_p[2], np.linalg.norm(e_p[0:2]))
# Limit desired pitch angle:
pitch_des = max(-self.desired_pitch_limit, min(pitch_des, self.desired_pitch_limit))
yaw_des = math.atan2(e_p[1], e_p[0])
yaw_err = self.unwrap_angle(yaw_des - rpy[2])
# Limit yaw effort
yaw_err = min(self.yaw_error_limit, max(-self.yaw_error_limit, yaw_err))
# Roll: P controller to keep roll == 0
roll_control = self.p_roll * rpy[0]
# Pitch: P controller to reach desired pitch angle
pitch_control = self.p_pitch * self.unwrap_angle(pitch_des - rpy[1]) + self.d_pitch * (des.vel[4] - msg.twist.twist.angular.y)
# Yaw: P controller to reach desired yaw angle
yaw_control = self.p_yaw * yaw_err + self.d_yaw * (des.vel[5] - msg.twist.twist.angular.z)
# Limit thrust
thrust = min(self.max_thrust, self.p_gain_thrust * np.linalg.norm(abs_pos_error) + self.d_gain_thrust * abs_vel_error)
thrust = max(self.min_thrust, thrust)
rpy = np.array([roll_control, pitch_control, yaw_control])
# In case the world_ned reference frame is used, the convert it back
# to the ENU convention to generate the reference fin angles
rtf = deepcopy(self.rpy_to_fins)
if self.local_planner.inertial_frame_id == 'world_ned':
rtf[:, 1] *= -1
rtf[:, 2] *= -1
# Transform orientation command into fin angle set points
fins = rtf.dot(rpy)
# Check for saturation
max_angle = max(np.abs(fins))
if max_angle >= self.max_fin_angle:
fins = fins * self.max_fin_angle / max_angle
thrust_force = self.thruster.tam_column * thrust
self.thruster.publish_command(thrust_force[0])
cmd = FloatStamped()
for i in range(self.n_fins):
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = '%s/fin%d' % (self.namespace, i)
cmd.data = min(fins[i], self.max_fin_angle)
cmd.data = max(cmd.data, -self.max_fin_angle)
self.pub_cmd[i].publish(cmd)
self.error_pub.publish(error_msg)