moveit: IkParameterizationType translationyaxisangle4d is not implemented

I have same topic (running moveit on yaw-yaw-z-yaw SCARA robot) and the discussion here was so helpful. I have to use Translation3D and then manually set the eef rotation, becauses of ikfast issue. For the record, here is what I confirmed;

1. The SCARA robot description https://github.com/k-okada/SCORE/tree/master/src/scara_description need small modification to work with ikfast (set rpy to zero https://github.com/k-okada/SCORE/commit/f15ed3e4b629ad48568efab016368d4d195f95e5) and since ikfast reqrues tool direction [1 0 0](https://github.com/rdiankov/openrave/issues/596#issuecomment-403673192), so we have to add dummy link for that (https://github.com/k-okada/SCORE/commit/d9df9044ba22046d571ca95174890654087ef253)
2. ik fast with TranslationZAxisAngle4D did not work, see https://github.com/rdiankov/openrave/issues/596 for detail.
3. running ikfast with ‘Translation3D’ and extend code for last yaw axis https://github.com/k-okada/SCORE/commit/549779a137110cfb1d2d2b6a2d2b550851926f7b#diff-25d902c24283ab8cfbac54dfa101ad31, see https://github.com/k-okada/SCORE/commit/2253b14d2761ab9652a780097ea352f26db7252d for the script
4. running ikfast with 'TranslateionXAxisAngle4Norm4D` works fine, see https://github.com/k-okada/SCORE/commit/00ea2404d6123d98b94e30cdd19a275cc03a264f for detail
5. fix ikfast_moveit_plugin.cpp based on https://github.com/ros-planning/moveit/issues/548#issuecomment-316298918 comment. see https://github.com/ros-planning/moveit/pull/973 for this modification

I can confirm this works with moveit_commander sample code

#!/usr/bin/env python

import sys
import math
import rospy
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState, Constraints
from geometry_msgs.msg import Pose


if __name__ == '__main__':
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)

    robot = RobotCommander()
    rospy.sleep(1)
    
    m = robot.manipulator

    # move init
    m.set_pose_target([0.5, 0.0, 0.15,  math.pi, 0, 0])
    m.go()
    
    # move y
    m.set_pose_target([0.3,-0.5, 0.15,  math.pi, 0, 0])
    m.go()

    # move z
    m.set_pose_target([0.3,-0.5, 0.05,  math.pi, 0, 0])    
    m.go()
 
    # rotate z
    m.set_pose_target([0.3,-0.5, 0.05,  math.pi, 0, math.pi/2])
    m.go()
    

I’ve added a PR that adds the missing IK types: #1823

The comment here also tries to explain this:

https://github.com/ros-planning/moveit/blob/d8678597530a8cbdb4af5075cd230fdaf1af6aae/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp#L96-L98

I’ve posted this snippet from the openrave documentation before:

TranslationXAxisAngle4D, TranslationYAxisAngle4D, TranslationZAxisAngle4D - end effector origin reaches desired 3D translation, manipulator direction makes a specific angle with x/y/z-axis (defined in the manipulator base link’s coordinate system)

That explains why TranslationYAxisAngle4D doesn’t work, but TranslationZAxisAngle4D does: The “YAxis” or “ZAxis” here is the manipulator’s base link coordinate frame (“world” in your picture), not the end effector frame. Also, it’s not a rotation around that axis, but an angle with that axis - in other words, exactly the pitch angle that you want.

It would be great if you could open a pull request with your change!

What it means is that when you ignore the translation, the rotation of the eef can be expressed as a rotation around one of the robot arm base frame axes.

For example, a SCARA robot has 4 degrees of freedom. The endeffector can rotate. The z axis of the robot’s base frame and eef always point up, since the eef misses the degrees of freedom to rotate the z axis out of that plane. Since the two z axes are always parallel, the eef pose can always be expressed as translation (x,y,z) and yaw angle wrt. the base frame, and TranslationZAxisAngle4D would be appropriate.

It would help if you could provide the URDF and the generated ikfast solver of your robot.

You should be able to get the rotation angle like this:

    case IKP_TranslationZAxisAngle4D:
        double roll, pitch, yaw;
        pose_frame.M.GetRPY(roll, pitch, yaw);
        ComputeIk(trans, *yaw, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
        return solutions.GetNumSolutions();

If that doesn’t work, try pose_frame.M.GetEulerZYZ or pose_frame.M.GetEulerZYX; see here.

I might be making a mistake in calculating the angle above?

You do not calculate it at all. M is the 3x3 rotation matrix and you have to extract the Roll/Pitch/Yaw angle yourself. For example the tf package provides methods to extract them.