Esempio n. 1
0
    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])
Esempio n. 2
0
 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()
Esempio n. 4
0
    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
Esempio n. 6
0
    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)
Esempio n. 7
0
 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)
Esempio n. 9
0
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)
Esempio n. 11
0
    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)
Esempio n. 12
0
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]
Esempio n. 14
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))
Esempio n. 15
0
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])]
Esempio n. 16
0
    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
Esempio n. 18
0
    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
Esempio n. 19
0
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
Esempio n. 20
0
    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
Esempio n. 21
0
File: tf.py Progetto: sentree/txros
 def __sub__(self, other):
     return numpy.concatenate([
         self._p - other._p,
         _rotvec_from_quat(transformations.quaternion_multiply(
             self._q,
             transformations.quaternion_inverse(other._q),
         )),
     ])
Esempio n. 22
0
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()
Esempio n. 23
0
    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
Esempio n. 24
0
  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))
Esempio n. 25
0
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()
Esempio n. 27
0
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))
Esempio n. 28
0
    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)
Esempio n. 31
0
 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])
Esempio n. 32
0
 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)
Esempio n. 34
0
    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()
Esempio n. 38
0
    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'
Esempio n. 40
0
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)
Esempio n. 43
0
 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
Esempio n. 44
0
 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
Esempio n. 46
0
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)
Esempio n. 48
0
	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
Esempio n. 49
0
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]
Esempio n. 50
0
 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])
Esempio n. 51
0
    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)
Esempio n. 52
0
File: tf.py Progetto: txros/txros
 def inverse(self):
     return Transform(
         -self._q_mat.T.dot(self._p),
         transformations.quaternion_inverse(self._q),
     )
Esempio n. 53
0
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]
Esempio n. 54
0
	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
Esempio n. 55
0
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)