def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
p = msg.pose.pose.position
q = msg.pose.pose.orientation
p = numpy.array([p.x, p.y, p.z])
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.pos_des = p
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Position error
e_pos_world = self.pos_des - p
e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot(
e_pos_world)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
# special case if we are far away from goal:
# ignore desired heading, look towards goal position
heading = math.atan2(e_pos_world[1], e_pos_world[0])
quat_des = numpy.array(
[0, 0, math.sin(0.5 * heading),
math.cos(0.5 * heading)])
e_rot_quat = transf.quaternion_multiply(
transf.quaternion_conjugate(q), quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_linear = self.pid_pos.regulate(e_pos_body, t)
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0],
y=v_linear[1],
z=v_linear[2])
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def odometry_callback(self, msg):
"""Handle updated measured velocity callback."""
if not bool(self.config):
return
q = msg.orientation
q = numpy.array([q.x, q.y, q.z, q.w])
if not self.initialized:
# If this is the first callback: Store and hold latest pose.
self.quat_des = q
self.initialized = True
# Compute control output:
t = time_in_float_sec_from_msg(msg.header.stamp)
# Error quaternion wrt body frame
e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q),
self.quat_des)
# Error angles
e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat))
v_angular = self.pid_rot.regulate(e_rot, t)
# Convert and publish vel. command:
cmd_vel = geometry_msgs.Twist()
cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0],
y=v_angular[1],
z=v_angular[2])
self.pub_cmd_vel.publish(cmd_vel)
def _motion_regression_6d(self, pnts, qt, t):
"""
Compute translational and rotational velocities and accelerations in
the inertial frame at the target time t.
!!! note
[1] Sittel, Florian, Joerg Mueller, and Wolfram Burgard. Computing
velocities and accelerations from a pose time sequence in
three-dimensional space. Technical Report 272, University of
Freiburg, Department of Computer Science, 2013.
"""
lin_vel = np.zeros(3)
lin_acc = np.zeros(3)
q_d = np.zeros(4)
q_dd = np.zeros(4)
for i in range(3):
v, a = self._motion_regression_1d([(pnt['pos'][i], pnt['t'])
for pnt in pnts], t)
lin_vel[i] = v
lin_acc[i] = a
for i in range(4):
v, a = self._motion_regression_1d([(pnt['rot'][i], pnt['t'])
for pnt in pnts], t)
q_d[i] = v
q_dd[i] = a
# Keeping all velocities and accelerations in the inertial frame
ang_vel = 2 * quaternion_multiply(q_d, quaternion_conjugate(qt))
ang_acc = 2 * quaternion_multiply(q_dd, quaternion_conjugate(qt))
return np.hstack((lin_vel, ang_vel[0:3])), np.hstack(
(lin_acc, ang_acc[0:3]))