示例#1
0
    def q_tilde_quat(self, q, target):
        """Compute the error signal when there are quaternions in the state
        space and target signals. If there are quaternions in the state space,
        calculate the error and then transform them to 3D space for the control
        signal.

        NOTE: Assumes that for every quaternion there are 3 motors, that effect
        movement along the x, y, and z axes, applied in that order.

        Parameters
        ----------
        q : float numpy.array
            mix of joint angles and quaternions [quaternions & radians]
        target : float numpy.array
            mix of target joint angles and quaternions [quaternions & radians]
        """
        # for each quaternion in the list, the output size is reduced by 1
        q_tilde = np.zeros(len(q) - np.sum(self.quaternions))

        q_index = 0
        q_tilde_index = 0

        for quat_bool in self.quaternions:
            if quat_bool:

                # if the joint position is a quaternion, calculate error
                joint_q = q[q_index:q_index + 4]
                error = quaternion_multiply(
                    target[q_index:q_index + 4],
                    quaternion_conjugate(joint_q),
                )

                # NOTE: we need to rotate this back to undo the current angle
                # because the actuators don't rotate with the ball joint
                u = quaternion_multiply(
                    quaternion_conjugate(joint_q),
                    quaternion_multiply(
                        error,
                        joint_q,
                    ),
                )

                # convert from quaternion to 3D angular forces to apply
                # https://studywolf.wordpress.com/2018/12/03/force-control-of-task-space-orientation/
                q_tilde[q_tilde_index:q_tilde_index +
                        3] = u[1:] * np.sign(u[0])

                q_index += 4
                q_tilde_index += 3
            else:
                q_tilde[q_tilde_index] = self.q_tilde_angle(
                    q[q_index], target[q_index])

                q_index += 1
                q_tilde_index += 1

        return q_tilde
示例#2
0
    def _calc_orientation_forces(self, target_abg, q):
        """Calculate the desired Euler angle forces to apply to the arm to
        move the end-effector to the target orientation

        Parameters
        ----------
        target_abg: np.array
            the target Euler angles orientation for the end-effector:
            alpha, beta, gamma
        q: np.array
            the joint angles of the arm
        """
        u_task_orientation = np.zeros(3)
        if self.orientation_algorithm == 0:
            # transform the orientation target into a quaternion
            q_d = transformations.unit_vector(
                transformations.quaternion_from_euler(
                    target_abg[0], target_abg[1], target_abg[2], axes="rxyz"
                )
            )
            # get the quaternion for the end effector
            q_e = self.robot_config.quaternion("EE", q=q)
            q_r = transformations.quaternion_multiply(
                q_d, transformations.quaternion_conjugate(q_e)
            )
            u_task_orientation = -q_r[1:] * np.sign(q_r[0])

        elif self.orientation_algorithm == 1:
            # From (Caccavale et al, 1997) Section IV Quaternion feedback
            # get rotation matrix for the end effector orientation
            R_e = self.robot_config.R("EE", q)
            # get rotation matrix for the target orientation
            R_d = transformations.euler_matrix(
                target_abg[0], target_abg[1], target_abg[2], axes="rxyz"
            )[:3, :3]
            R_ed = np.dot(R_e.T, R_d)  # eq 24
            q_ed = transformations.unit_vector(
                transformations.quaternion_from_matrix(R_ed)
            )
            u_task_orientation = -1 * np.dot(R_e, q_ed[1:])  # eq 34

        else:
            raise Exception(
                "Invalid algorithm number %i for calculating "
                % self.orientation_algorithm
                + "orientation error"
            )

        return u_task_orientation
        u_task[:3] = -kp * (hand_xyz - target[:3])

        # Method 1 ------------------------------------------------------------
        #
        # transform the orientation target into a quaternion
        q_d = transformations.unit_vector(
            transformations.quaternion_from_euler(
                target[3], target[4], target[5], axes='rxyz'))

        # get the quaternion for the end effector
        q_e = transformations.unit_vector(
            transformations.quaternion_from_matrix(
                robot_config.R('EE', feedback['q'])))
        # calculate the rotation from the end-effector to target orientation
        q_r = transformations.quaternion_multiply(
            q_d, transformations.quaternion_conjugate(q_e))
        u_task[3:] = q_r[1:] * np.sign(q_r[0])


        # Method 2 ------------------------------------------------------------
        # From (Caccavale et al, 1997) Section IV - Quaternion feedback -------
        #
        # get rotation matrix for the end effector orientation
        # R_EE = robot_config.R('EE', feedback['q'])
        # # get rotation matrix for the target orientation
        # R_d = transformations.euler_matrix(
        #     target[3], target[4], target[5], axes='rxyz')[:3, :3]
        # R_ed = np.dot(R_EE.T, R_target)  # eq 24
        # q_e = transformations.quaternion_from_matrix(R_ed)
        # q_e = transformations.unit_vector(q_e)
        # u_task[3:] = np.dot(R_EE, q_e[1:])  # eq 34
            q=feedback["q"],
            dq=feedback["dq"],
            target=target,
        )

        # send forces into Mujoco, step the sim forward
        interface.send_forces(u)

        # track data
        q_track.append(np.copy(feedback["q"]))
        target_track.append(np.copy(target))

        # calculate the distance between quaternions
        error = quaternion_multiply(
            target,
            quaternion_conjugate(feedback["q"]),
        )
        error = 2 * np.arctan2(
            np.linalg.norm(error[1:]) * -np.sign(error[0]), error[0])
        # quaternion distance for same angles can be 0 or 2*pi, so use a sine
        # wave here so 0 and 2*np.pi = 0
        error = np.sin(error / 2)
        error_track.append(np.copy(error))

        if abs(error) < threshold:
            interface.sim.model.geom_rgba[target_geom_id] = green
            count += 1
        else:
            count = 0
            interface.sim.model.geom_rgba[target_geom_id] = red
示例#5
0
        u_task[:3] = -kp * (hand_xyz - target[:3])

        # Method 1 ------------------------------------------------------------
        #
        # transform the orientation target into a quaternion
        q_d = transformations.unit_vector(
            transformations.quaternion_from_euler(
                target[3], target[4], target[5], axes='rxyz'))

        # get the quaternion for the end effector
        q_e = transformations.unit_vector(
            transformations.quaternion_from_matrix(
                robot_config.R('EE', feedback['q'])))
        # calculate the rotation from the end-effector to target orientation
        q_r = transformations.quaternion_multiply(
            q_d, transformations.quaternion_conjugate(q_e))
        u_task[3:] = q_r[1:] * np.sign(q_r[0])


        # Method 2 ------------------------------------------------------------
        # From (Caccavale et al, 1997) Section IV - Quaternion feedback -------
        #
        # get rotation matrix for the end effector orientation
        # R_EE = robot_config.R('EE', feedback['q'])
        # # get rotation matrix for the target orientation
        # R_d = transformations.euler_matrix(
        #     target[3], target[4], target[5], axes='rxyz')[:3, :3]
        # R_ed = np.dot(R_EE.T, R_target)  # eq 24
        # q_e = transformations.quaternion_from_matrix(R_ed)
        # q_e = transformations.unit_vector(q_e)
        # u_task[3:] = np.dot(R_EE, q_e[1:])  # eq 34
示例#6
0
            target=target,
        )

        # send forces into Mujoco, step the sim forward
        interface.send_forces(u)

        # track data
        q_track.append(np.copy(feedback["q"]))
        target_track.append(np.copy(target))

        # calculate the distance between quaternions
        error = 0.0
        for ii in range(2):
            temp = quaternion_multiply(
                target[ii * 4:(ii * 4) + 4],
                quaternion_conjugate(feedback["q"][ii * 4:(ii * 4) + 4]),
            )
            temp = 2 * np.arctan2(
                np.linalg.norm(temp[1:]) * -np.sign(temp[0]), temp[0])
            # quaternion distance for same angles can be 0 or 2*pi, so use a sine
            # wave here so 0 and 2*np.pi = 0
            error += np.sin(temp / 2)
        error_track.append(np.copy(error))

        if abs(error) < threshold:
            interface.sim.model.geom_rgba[target_geom_id] = green
            count += 1
        else:
            count = 0
            interface.sim.model.geom_rgba[target_geom_id] = red