Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

IkParameterizationType translationyaxisangle4d is not implemented #548

Open
luminize opened this issue Jul 14, 2017 · 13 comments

Comments

Projects
None yet
4 participants
@luminize
Copy link

commented Jul 14, 2017

TranslationYaxis4D is not implemented in moveit_kinematics

With the help of @gavanderhoorn his answer on http://answers.ros.org/question/263925/generating-an-ikfast-solution-for-4-dof-arm/ I have made an IkFast plugin for my robot.
When dragging the handles in Moveit the robot goal pose doesn't move, and while trying to drag I get the message:

[ERROR] [1500016582.397196842]: IK for this IkParameterizationType not implemented yet.

I've looked this up and came to the following answer on ros.answers.org http://answers.ros.org/question/191884/ik-for-this-ikparameterizationtype-not-implemented-yet/

I would be greatly helped to know which actions I should take to (similarly as in the answer above) get these kinematics implemented.

Environment

  • ROS Distro: Kinetic
  • OS Version: Ubuntu 16.04
@v4hn

This comment has been minimized.

Copy link
Member

commented Jul 17, 2017

I have not seen this IK type before but the basic steps to follow are:

  • figure out which parameters you have to pass to ComputeIk

  • add the appropriate code to compute "the right" parameters and pass them to the function here

  • File a pull-request! :)

@luminize

This comment has been minimized.

Copy link
Author

commented Jul 17, 2017

regarding a PR, no problem. But figuring out the first 2 points I have a question if you don't mind:

There's the instruction that the first value represents the angle. Looking at my IKFast cpp file I guess the first value are we talking about is the first element of eerot

So I should be able to use the same ComputeIK as in IKP_Transform6D and IKP_Translation3D like here ?

@v4hn

This comment has been minimized.

Copy link
Member

commented Jul 17, 2017

Looking at my IKFast cpp file I guess the first value are we talking about is the first element of eerot

I would guess the same thing.

So I should be able to use the same ComputeIK as in IKP_Transform6D and IKP_Translation3D like here ?

Possibly? I guess you tried it by now? :)
I would pass the address of one double (representing the angle) as vals.

Ping @mintar . You fought with these a lot in the past.
Could you drop a quick hint here?

@mintar

This comment has been minimized.

Copy link
Contributor

commented Jul 18, 2017

I would pass the address of one double (representing the angle) as vals.

I think so too. In the case of TranslationXaxis4D,TranslationYaxis4D and TranslationZaxis4D, eerot is probably only a single IkReal*. The best way to find out is to look at the ComputeIk() function in your generated *_ikfast_solver.cpp.

For example, when I implemented the support for IKP_TranslationDirection5D, I looked at these lines:

https://github.com/uos/katana_driver/blob/843b59619e42de37aa4b31cd0505188590cb7fcb/katana_moveit_ikfast_plugin/src/katana_450_6m90a_ikfast_solver.cpp#L271-L273

There you can see that it tries to read a 3-dimensional vector (the direction vector) from eerot.

So I computed the direction vector from pose_frame and put it in there:

case IKP_TranslationDirection5D:
// For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target
// direction.
direction = pose_frame.M * KDL::Vector(0, 0, 1);
ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
return solutions.GetNumSolutions();

So you need to do something similar for the three Translation*axis4D IK types: compute an angle around the X/Y/Z axis from pose_frame, put that angle as a double* into eerot.

FYI, from the openrave documentation:

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)

For testing, I found it easier to generate some test poses that I know are reachable by the arm (for example in RViz, generate a new random goal pose), and then call MoveIt's IK service with those poses. Dragging the handle adds further complexity IMO, so I did that as a second step.

If you fix it and could provide a PR, that would be great! I didn't do it back then because I didn't have a Translation*axis4D solver file to test with.

@luminize

This comment has been minimized.

Copy link
Author

commented Jul 18, 2017

@v4hn @mintar Thanks, I'm walking the learning curve here, So apologies for that.

This is what I did so far:

    case IKP_TranslationXAxisAngle4D:
    case IKP_TranslationYAxisAngle4D:
    case IKP_TranslationZAxisAngle4D:
      // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value
      // represents the angle.
      mult2 = pose_frame.M;

      double rot[1];
      rot[0] = mult2(0, 1);
      ComputeIk(trans, rot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
      return solutions.GetNumSolutions();

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

So I will first see that I get the IK to work.

Regarding the IK service I try to do the following:

parallels@ubuntu:~$ rosservice call /compute_ik
Usage: rosservice call /service [args...]

rosservice: error: Please specify service arguments
parallels@ubuntu:~$ rosservice type /compute_ik 
moveit_msgs/GetPositionIK
parallels@ubuntu:~$ rosservice call /moveit_msgs/GetPositionIK
ERROR: Service [/moveit_msgs/GetPositionIK] is not available.

I did some searching on this but I can't figure out how to set this up. Any pointers?

@v4hn

This comment has been minimized.

Copy link
Member

commented Jul 18, 2017

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.

@luminize

This comment has been minimized.

Copy link
Author

commented Jul 18, 2017

I think I have a problem here. I have a 4DOF robot, the eef can rotate, but not around the y axis of the base (which is fixed). But around its own (joint) y axis.

@mintar

This comment has been minimized.

Copy link
Contributor

commented Jul 19, 2017

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.

@luminize

This comment has been minimized.

Copy link
Author

commented Jul 19, 2017

@mintar I see your point with the example of the SCARA robot. My URDF is in the working branch "cleaned" of my robot support package:
https://github.com/luminize/matilda_support/tree/cleaned
Here's also a link of the prototype (non ik-fast):
https://www.youtube.com/watch?v=6Mzok9r9k88

@luminize

This comment has been minimized.

Copy link
Author

commented Jul 19, 2017

The reason I'm making an IKfast plugin in the first place is that I'd like to use python with moveit_commander to get the robot to a number of positions. The robot is driven by Machinekit, and that has a python API too.

I use ROS/Moveit as the off-line planning alternative to Machinekits G-code interpreter. So for making an application I'll use a python program which will take actions, depending on hardware inputs/outputs, the Machinetalk remote interface and other application specifics (like production automation machinery).

Because my robot is < 4DOF I've not succeeded in using moveit_commander (cartesian) with the "standard" KDL kinematics or trac_ik plugin.

So in the end I'd like to provide x,y and z (and possibly eef angle) and just let Moveit plan and execute the move.
What I guess will be the easiest/proper workflow is:

  • use Translation3D kinematics for cartesian position -> joint angles
  • manually set the 4th axis (eef) rotation
  • plan and execute
@mintar

This comment has been minimized.

Copy link
Contributor

commented Jul 19, 2017

Ok, your robot indeed doesn't have an axis that's parallel with the base frame, so you can't use TranslationZAxisAngle4D. Your approach to use Translation3D and then manually set the eef rotation sounds reasonable. An even better solution would be to hack your generated *_ikfast_moveit_plugin.cpp file (in the case Translation3D: block), compute the eef rotation there and put it into solutions[3] before returning.

@luminize

This comment has been minimized.

Copy link
Author

commented Jul 20, 2017

@k-okada

This comment has been minimized.

Copy link
Contributor

commented Jul 10, 2018

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 k-okada/SCORE@f15ed3e) and since ikfast reqrues tool direction [1 0 0](rdiankov/openrave#596 (comment)), so we have to add dummy link for that (k-okada/SCORE@d9df904)
  2. ik fast with TranslationZAxisAngle4D did not work, see rdiankov/openrave#596 for detail.
  3. running ikfast with 'Translation3D' and extend code for last yaw axis k-okada/SCORE@549779a#diff-25d902c24283ab8cfbac54dfa101ad31, see k-okada/SCORE@2253b14 for the script
  4. running ikfast with 'TranslateionXAxisAngle4Norm4D` works fine, see k-okada/SCORE@00ea240 for detail
  5. fix ikfast_moveit_plugin.cpp based on #548 (comment) comment. see #973 for this modification

screenshot from 2018-07-10 10-44-46

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.