ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

What method can be used to interpolate the orientation of a robotic arm?

asked 2019-07-03 06:13:46 -0600

691TReilly gravatar image

updated 2019-07-03 09:03:31 -0600

I am currently writing a program that when a 6 DOF robotic arm is provided with an initial pose (which is comprised of a position and orientation), a series of intermediate poses and a target pose, a trajectory is calculated that passes through these points. I have successfully implemented a cubic spline interpolation algorithm that calculates a trajectory for the positions of the robotic arm, however I am unsure how to interpolate the orientation of the arm. The orientation matrix has four variables: x, y, z and w. From what I understand from the research I have conducted, quaternions appear to be involved? However I am unsure of what approach/method I ought to implement?

I've written the following function that implements the SLERP method, as advised:

def slerp(self, start_O, target_O, t_array):
        t_array = np.array(t_array)
        start_O = np.array(start_O)
        target_O = np.array(target_O)
        dot = np.sum(start_O*target_O)

        if (dot < 0.0):
            target_O = -target_O
            dot = -dot

        DOT_THRESHOLD = 0.9995
        if (dot > DOT_THRESHOLD):
            result = target_O[np.newaxis,:] + t_array[:,np.newaxis]*(target_O - start_0)[np.newaxis,:]
            return (result.T / np.linalg.norm(result, axis=1)).T

        theta_0 = np.arrcos(dot)
        sin_theta_0 = np.sin(theta_0)
        theta = theat_0 * t_array
        sin_theta = np.sign(theta)

        s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
        s1 = sin_theta / sin_theta_0
        return (s0[:,np.newaxis] * v0[np.newaxis,:]) + (s1[:,np.newaxis] * v1[np.newaxis,:])

However, how do I implement this function in the function trajectoryMover, shown below, such that for each intermediate position of the arm, the orientation of the arm is simultaneously interpolated using the SLERP method? Currently, the x,y,z and w orientations are set as constants, however these ought to become variables corresponding to the current interpolated orientation values, if I understand correctly?

def trajectoryMover(self):
        newPose = Pose()
        arr1 = []
        arr2 = []
        arr3 = []
        x_axis = [0.001, 0.0039, 0.0160, 0.0334]
        y_axis = [0.009, 0.0239, 0.0121, 0.0034]
        z_axis = [0.009, 0.0199, 0.0821, 0.1034]
        start_orientation = [0.707106781172, 0.707106781191, 2.59734823723e-06, 0]
        end_orientation = [0.151231412, 0.5112315134, 0.0051534141, 0.5
        self.cubicSplineInterpolate(x_axis,y_axis,z_axis)
        arr1 = self.xLinespace
        arr2 = self.yLinespace
        arr3 = self.zLinespace

        for x, y, z in zip(arr1, arr2, arr3):

            newPose.position.x = x
            newPose.position.y = y
            newPose.position.z = z
            newPose.orientation.x = 0.707106781172
            newPose.orientation.y = 0.707106781191
            newPose.orientation.z = 2.59734823723e-06
            newPose.orientation.w = 0
            self.set_position_cartesian.publish(newPose)
            rospy.loginfo(newPose)
            rospy.sleep(0.05)

Thanks in advance.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-07-03 07:49:41 -0600

Interpolating between two rotation quaternions is a non-trivial bit of mathematics. Luckily this function is built into the TF2 quaternion class in ROS.

The function you want is slightly cryptically called SLERP (Spherical Linear Interpolation). This method is described here, it is a method to interpolate between this quaternion and other given quaternion at any ratio.

This will do all the hard work for you.

edit flag offensive delete link more

Comments

Thank you for your help! I've implemented the SLERP method as you recommended (see amended question above), however how do I implement this function in the function trajectoryMover, shown below, such that for each intermediate position of the arm, the orientation of the arm is simultaneously interpolated using the SLERP method? Currently, the x,y,z and w orientations are set as constants, however these ought to become variables corresponding to the current interpolated orientation values, if I understand correctly?

691TReilly gravatar image 691TReilly  ( 2019-07-03 08:59:40 -0600 )edit

I don't believe the suggestion was to implement slerp yourself, rather to use the implementation provided by TF2.

But I have the impression your question is not really ROS related, hence the reimplementation.

Can you please edit your original question text to make it clear why this is posted on ROS Answers?

gvdhoorn gravatar image gvdhoorn  ( 2019-07-03 10:39:08 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2019-07-03 06:13:46 -0600

Seen: 1,424 times

Last updated: Jul 03 '19