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]))