def _motion_regression_6d(self, pnts, qt, t):
        """
        Compute translational and rotational velocities and accelerations in
        the inertial frame at the target time t.

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

    def align_poses(self, ps1, ps2):

        self.aul.update_frame(ps1)
        ps2.header.stamp = rospy.Time.now()
        self.tfl.waitForTransform(ps2.header.frame_id, 'lh_utility_frame',
                                  rospy.Time.now(), rospy.Duration(3.0))
        ps2_in_ps1 = self.tfl.transformPose('lh_utility_frame', ps2)

        ang = math.atan2(-ps2_in_ps1.pose.position.z,
                         -ps2_in_ps1.pose.position.y) + (math.pi / 2)
        q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0))
        q_st_new = transformations.quaternion_multiply([
            ps1.pose.orientation.x, ps1.pose.orientation.y,
            ps1.pose.orientation.z, ps1.pose.orientation.w
        ], q_st_rot)
        ps1.pose.orientation = Quaternion(*q_st_new)

        self.aul.update_frame(ps2)
        ps1.header.stamp = rospy.Time.now()
        self.tfl.waitForTransform(ps1.header.frame_id, 'lh_utility_frame',
                                  rospy.Time.now(), rospy.Duration(3.0))
        ps1_in_ps2 = self.tfl.transformPose('lh_utility_frame', ps1)
        ang = math.atan2(ps1_in_ps2.pose.position.z,
                         ps1_in_ps2.pose.position.y) + (math.pi / 2)

        q_st_rot = transformations.quaternion_about_axis(ang, (1, 0, 0))
        q_st_new = transformations.quaternion_multiply([
            ps2.pose.orientation.x, ps2.pose.orientation.y,
            ps2.pose.orientation.z, ps2.pose.orientation.w
        ], q_st_rot)
        ps2.pose.orientation = Quaternion(*q_st_new)
        return ps1, ps2

 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 generate_sigma_points(self, mean, covariance):
     sigmas = []
     sigmas.append(mean)
     if not check_symmetry(covariance):
         pdb.set_trace()
     temp = pos_sem_def_sqrt(covariance)
     if any(isnan(temp)):
         rospy.logerr("Sqrt matrix contained a NaN. Matrix was %s",
                 covariance)
     temp = temp * sqrt(self.n + self.kf_lambda)
     for i in range(0,self.n):
         #Must use columns in order to get the write thing out of the
         #Cholesky decomposition
         #(http://en.wikipedia.org/wiki/Cholesky_decomposition#Kalman_filters)
         column = temp[:,i].reshape((self.n,1))
         #Build the noise quaternion
         noise_vec = column[0:3,0]
         noise_quat = SF9DOF_UKF.build_noise_quat(noise_vec)
         original_quat = SF9DOF_UKF.recover_quat(mean)
         #Do the additive sample
         perturbed_quat = tf_math.quaternion_multiply(noise_quat,original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean + column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
         #Do the subtractive sample
         conj_noise_quat = tf_math.quaternion_conjugate(noise_quat)
         perturbed_quat = tf_math.quaternion_multiply(conj_noise_quat, original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean - column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
     return sigmas

 def generate_sigma_points(self, mean, covariance):
     sigmas = []
     sigmas.append(mean)
     temp = numpy.linalg.cholesky(covariance)
     temp = temp * sqrt(self.n + self.kf_lambda)
     for i in range(0,self.n):
         #Must use columns in order to get the write thing out of the
         #Cholesky decomposition
         #(http://en.wikipedia.org/wiki/Cholesky_decomposition#Kalman_filters)
         column = temp[:,i].reshape((self.n,1))
         #Build the noise quaternion
         noise_vec = column[0:3,0]
         noise_quat = SF9DOF_UKF.build_noise_quat(noise_vec)
         original_quat = SF9DOF_UKF.recover_quat(mean)
         #Do the additive sample
         perturbed_quat = tf_math.quaternion_multiply(noise_quat,original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean + column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
         #Do the subtractive sample
         conj_noise_quat = tf_math.quaternion_conjugate(noise_quat)
         perturbed_quat = tf_math.quaternion_multiply(conj_noise_quat, original_quat)
         #perturbed_quat = tf_math.unit_vector(perturbed_quat)
         new_mean = mean - column
         new_mean[0:3,0] = perturbed_quat[0:3,0]
         sigmas.append(new_mean)
     return sigmas

def rotate(v, q):
    """
    Rotate vector v according to quaternion q
    """
    q2 = np.r_[v[0:3], 0.0]
    return transformations.quaternion_multiply(
        transformations.quaternion_multiply(q, q2),
        transformations.quaternion_conjugate(q))[0:3]

        def process_touch_pose(ud):
            ######################################################################### 
            # tranformation logic for manipulation
            # put touch pose in torso frame
            frame_B_touch = util.pose_msg_to_mat(ud.touch_click_pose)
            torso_B_frame = self.get_transform("torso_lift_link", 
                                               ud.touch_click_pose.header.frame_id)
            if torso_B_frame is None:
                return 'tf_failure'
            torso_B_touch_bad = torso_B_frame * frame_B_touch

            # rotate pixel23d the right way
            t_pos, t_quat = util.pose_mat_to_pq(torso_B_touch_bad)
            # rotate so x axis pointing out
            quat_flip_rot = tf_trans.quaternion_from_euler(0.0, np.pi/2.0, 0.0)
            quat_flipped = tf_trans.quaternion_multiply(t_quat, quat_flip_rot)
            # rotate around x axis so the y axis is flat
            mat_flipped = tf_trans.quaternion_matrix(quat_flipped)
            rot_angle = np.arctan(-mat_flipped[2,1] / mat_flipped[2,2])
            quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
            quat_flipped_ortho = tf_trans.quaternion_multiply(quat_flipped, quat_ortho_rot)

            torso_B_touch = util.pose_pq_to_mat(t_pos, quat_flipped_ortho)

            # offset the touch location by the approach tranform
            appr_B_tool = self.get_transform(self.tool_approach_frame, self.tool_frame) 
            if appr_B_tool is None:
                return 'tf_failure'
            torso_B_touch_appr = torso_B_touch * appr_B_tool

            # project the approach back into the wrist
            appr_B_wrist = self.get_transform(self.tool_frame, 
                                              self.arm + "_wrist_roll_link") 
            if appr_B_wrist is None:
                return 'tf_failure'

            torso_B_wrist = torso_B_touch_appr * appr_B_wrist
            ######################################################################### 
            # create PoseStamped
            appr_wrist_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
                                                         torso_B_wrist)
            appr_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link', 
                                                        torso_B_touch_appr)
            touch_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link', 
                                                         torso_B_touch)

            # visualization
            self.wrist_pub.publish(appr_wrist_ps)
            self.appr_pub.publish(appr_tool_ps)
            self.touch_pub.publish(touch_tool_ps)

            # set return values
            ud.appr_wrist_mat = torso_B_wrist
            ud.appr_tool_ps = appr_tool_ps
            ud.touch_tool_ps = touch_tool_ps

            
            return 'succeeded'

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 rotate_vect_by_quat(v, q):
    '''
    Rotate a vector by a quaterion.
    v' = q*vq

    q should be [x,y,z,w] (standard ros convention)
    '''
    cq = np.array([-q[0], -q[1], -q[2], q[3]])
    cq_v = trans.quaternion_multiply(cq, v)
    v = trans.quaternion_multiply(cq_v, q)
    v[1:] *= -1  # Only seemed to work when I flipped this around, is there a problem with the math here?
    return np.array(v)[:3]

    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 euler_to_quat_deg(roll, pitch, yaw):
    roll = roll * (math.pi / 180.0)
    pitch = pitch * (math.pi / 180.0)
    yaw = yaw * (math.pi / 180.0)
    xaxis = (1, 0, 0)
    yaxis = (0, 1, 0)
    zaxis = (0, 0, 1)
    qx = transformations.quaternion_about_axis(roll, xaxis)
    qy = transformations.quaternion_about_axis(pitch, yaxis)
    qz = transformations.quaternion_about_axis(yaw, zaxis)
    q = transformations.quaternion_multiply(qx, qy)
    q = transformations.quaternion_multiply(q, qz)
    print q

    def _getPokePose(self, pos, orient):

        print orient
        if numpy.dot(tft.quaternion_matrix(orient)[0:3,2], (0,0,1)) < 0:
            print "fliiping arm pose"
            orient = tft.quaternion_multiply(orient, (1,0,0,0))
        tilt_adjust = tft.quaternion_about_axis(self.tilt_angle[self.arm_using], (0,1,0))
        new_orient = tft.quaternion_multiply(orient, tilt_adjust)
        pos[2] += self.above #push above cm above table 
#DEBUG
        #print new_orient

        return (pos, orient, new_orient)

 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 listen_imu_quaternion(self, imu):
        """Doesn't work b/c desired yaw is too far from possible yaw"""
        current_orientation = np.array([imu.orientation.x,
                                        imu.orientation.y,
                                        imu.orientation.z,
                                        imu.orientation.w])
        # current_angular_speed = np.array([imu.angular_velocity.x,
        #                                   imu.angular_velocity.y,
        #                                   imu.angular_velocity.z])
        diff_orientation = transformations.quaternion_multiply(
            self.desired_orientation,
            # Unit quaternion => inverse = conjugate / norm = congugate
            transformations.quaternion_conjugate(current_orientation))

        assert np.allclose(
            transformations.quaternion_multiply(diff_orientation,
                                                current_orientation),
            self.desired_orientation)

        # diff_r, diff_p, diff_y = transformations.euler_from_quaternion(
        #     diff_orientation)
        # rospy.loginfo("Orientation error (quaternion): {}".format(diff_orientation))
        # rospy.loginfo(
        #     "Orientation error (deg): {}".format(
        #         np.rad2deg([diff_r, diff_p, diff_y]))
        # )
        # out = self.pitch_controller(diff_p)

        corrected_orientation = transformations.quaternion_multiply(
            quaternion_power(diff_orientation, 1.5),
            self.desired_orientation)
        rospy.loginfo(
            "Desired orientation (deg): {}".format(
                np.rad2deg(transformations.euler_from_quaternion(
                    self.desired_orientation))))
        rospy.loginfo(
            "Corrected orientation (deg): {}".format(
                np.rad2deg(transformations.euler_from_quaternion(
                    corrected_orientation))))

        rospy.loginfo("Desired position: {}".format(
            self.desired_position))

        setpoint_pose = Pose(self.desired_position, corrected_orientation)

        servo_angles = self.platform.ik(setpoint_pose)
        rospy.logdebug(
            "Servo angles (deg): {}".format(np.rad2deg(servo_angles)))
        self.publish_servo_angles(servo_angles)

    def odom_transform_2d(self, data, new_frame, trans, rot):
        # NOTES: only in 2d rotation
        # also, it removes covariance, etc information
        odom_new = Odometry()
        odom_new.header = data.header
        odom_new.header.frame_id = new_frame

        odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
        odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
        odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
        # rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
        q = data.pose.pose.orientation
        q_tuple = (q.x, q.y, q.z, q.w,)
        # Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
        odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))

        heading_change = quaternion_to_heading(rot)

        odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
        odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
        odom_new.twist.twist.linear.z = 0

        odom_new.twist.twist.angular.x = 0
        odom_new.twist.twist.angular.y = 0
        odom_new.twist.twist.angular.z = data.twist.twist.angular.z

        return odom_new

 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 aim_pose_to(ps, pts, point_dir=(1,0,0)):
    if not (ps.header.frame_id.lstrip('/') == pts.header.frame_id.lstrip('/')):
        rospy.logerr("[Pose_Utils.aim_pose_to]:"+
                     "Pose and point must be in same frame: %s, %s"
                    %(ps.header.frame_id, pt2.header.frame_id))
    target_pt = np.array((pts.point.x, pts.point.y, pts.point.z))
    base_pt = np.array((ps.pose.position.x,
                        ps.pose.position.y,
                        ps.pose.position.z)) 
    base_quat = np.array((ps.pose.orientation.x, ps.pose.orientation.y,
                          ps.pose.orientation.z, ps.pose.orientation.w))

    b_to_t_vec = np.array((target_pt[0]-base_pt[0],
                           target_pt[1]-base_pt[1],
                           target_pt[2]-base_pt[2]))
    b_to_t_norm = np.divide(b_to_t_vec, np.linalg.norm(b_to_t_vec))

    point_dir_hom = (point_dir[0], point_dir[1], point_dir[2], 1)
    base_rot_mat = tft.quaternion_matrix(base_quat)
    point_dir_hom = np.dot(point_dir_hom, base_rot_mat.T)
    point_dir = np.array((point_dir_hom[0]/point_dir_hom[3],
                         point_dir_hom[1]/point_dir_hom[3],
                         point_dir_hom[2]/point_dir_hom[3]))
    point_dir_norm = np.divide(point_dir, np.linalg.norm(point_dir))
    axis = np.cross(point_dir_norm, b_to_t_norm)
    angle = np.arccos(np.vdot(point_dir_norm, b_to_t_norm))
    quat = tft.quaternion_about_axis(angle, axis)
    new_quat = tft.quaternion_multiply(quat, base_quat)
    ps.pose.orientation = Quaternion(*new_quat)

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