adding robots for Analytical Kinematics

[Coroush]

Feature Request

It would be nice to allow using custom robots with Analytical Kinematics solver.

Details

Is your feature request related to a problem? Please describe.
Currently there are only a very limited robots available to use with Analytical Kinematics solver.

Describe the solution you'd like
for example:

# based on http://archive.fabacademy.org/archives/2017/fablabcept/students/184/assets/lobster_ik.pdf

points = [Point(0.320, 0.000, 0.78),
                Point(0.320, 0.000, 2.06),
                Point(1.9125, 0.000, 2.26),
                Point(2.1125, 0.000, 2.26)]

irb6700_150 = { 'name' : 'ABB_IRB6700_150_320' , 'brand' : 'ABB',  'type' : 'spherical_wrist', 'points' : points }
# brand is used for classifying pre_processing and post_processing functions

ik.inverse_kinematics(robot, frame_WCF, options={'solver': irb6700_150})

Describe alternatives you've considered
Manually modified the local python files in compas (forking):

class ABB_IRB6700_150_320Kinematics(SphericalWristKinematics):

    """Analytical IK solver for the ABB IRB6700 150/320 robot."""

    def __init__(self):

        points = [Point(0.320, 0.000, 0.780),
                  Point(0.320, 0.000, 2.060),
                  Point(1.9125, 0.000, 2.26),
                  Point(2.1125, 0.000, 2.26)]

        super(ABB_IRB6700_150_320Kinematics, self).__init__(points)
  .
  .
  .

[yck011522]

Reminds me of something related to IKFast?
Their implementation is aimed to have extremely high speed, generalizable to 6 DOF arms. Unfortunately not 7DOF and not combination with linear tracks.

Just 2 cents.

http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html

[Coroush]

hi @yck011522, thanks a lot for your comment and the reference. Its very relevant and helpful! I'll look into it further.
do you see any advantage of using IKFast compared to Analytical Kinematics backend? i found this more lightweight since it's not using ros in the backend. would you have any suggestions for Fast IK solver for 6 DOF arm combined with linear tracks?

thank you

[yck011522]

hi @Coroush . IKFast AFAIK is an implementation of analytical kinematic solver. Pretty much it is a bunch of close form sine cosine functions. The closed form solution only works for certain kinematics chains, in particularly, not more than 6 DOF. For more than 6DOF, you have a continuous solution instead of a discrete Configuration. That is no longer a close form equation.

The link reference to Lobster Plugin (http://archive.fabacademy.org/archives/2017/fablabcept/students/184/assets/lobster_ik.pdf) is interesting. I remember back then, 4 to 5 years ago, I read a primer on how Lobster work, and instead of equations, it uses Rhino to perform projections and circle intersection. If I remember correctly, there are only a few intersections to solve and you can get the whole IK. The first thing to solve is the wrist point, it is just the TCP moved backwards along Z. The next is projection of the wrist point to the robot base plane, that point's angle relative to the base zero direction will decide J0 angle. J0 can be forward or backward (+-180deg). For each case, a circle intersection from shoulder center and wrist center solves for the elbow center, typically it will be two intersections, resulting in elbow up and elbow down config. J4 is typically a continuous in-line joint and has a positive and negative solution. So in total 8 possible config, without considering joint limits.

[yck011522]

Oh actually I'm completely missing your point of the Feature Request.

I guess you want ABB_IRB6700_150_320Kinematics ,and I am guessing you can do that without forking compas_fab, just need to extend the SphericalWristKinematics class in your user code similar to the ABB_IRB4600_40_255Kinematics(SphericalWristKinematics): https://github.com/compas-dev/compas_fab/blob/main/src/compas_fab/backends/kinematics/solvers/spherical_wrist_kinematics.py#L58

[yck011522]

If you want to combine it with a linear track, a easy way is to fix your linear track at some value, and then do 6DOF IK. Depends on your application, you can sample multiple Linear Track values and see if you get any good IK results.

[Coroush]

thanks a lot @yck011522 for explaining how the lobster and IKFast solver works! super interesting and helpful 🙌

yes the request i had was about having this extension from your side if that's possible (or coming with a more general solution of adding custom robots) so it can be easier when I update compas_fab or pass my code to other people. I have already extended the SphericalWristKinematics in my local files and it is working. The parameters that I have written in the request are the correct ones that I calculated based on Lobster pdf reference using Rhino. Sorry if that wasn't clear in the initial request.

Thanks again

[yck011522]

@Coroush Cool. Can you show me how you get the 4 points for the ABB_IRB6700_150_320 robot?

I believe if @romanarust or @gonzalocasas can verify that your values are correct, we can add that robot to the main branch too?

By the way, when you say custom robots, do you mean (1) other 6DOF robots belonging to the SphericalWrist and the OffsetWrist type? or do you mean by (2) any RobotModel that can be described with an URDF?

I believe (1) is definitely possible. For example how you are already creating it but you did it in spherical_wrist_kinematics.py. You can also create this in a file in your own codebase and distributed with your code.

I believe it is also possible (but not yet) to parse an URDF or RobotModel class of a 6 DOF robot corresponding to the SphericalWrist and the OffsetWrist type and interogate that 4 points out.

However, (2) is rather impossible because of the lack of closed form solution available.

[Coroush]

awesome! thank you @yck011522. here is how I extract the points:
I'm using the robot components plugin in grasshopper to load the robots, and use the axis planes to read the 4 points; when the robot is in home position, the origin of axis planes is similar to the intersection of XZ plane with the axis line.
IRB6700-150/3.2

If possible to add the robots, would you please also consider IRB6700-235/2.65 :
these are the points:

points = [Point(0.320, 0.000, 0.78),
                Point(0.320, 0.000, 1.915),
                Point(1.5025, 0.000, 2.115),
                Point(1.7025, 0.000, 2.115)]

"(1) other 6DOF robots belonging to the SphericalWrist and the OffsetWrist"

yes i meant this option ! maybe it will be quite easy to read these points (axis planes origins), similar to the pictures i've attached, from the URDF files? there is even no need for calculating intersections.

[yck011522]

Cool. I'm not the maintainer of the repo so you will have to see what they think regarding the addition.

Are you sure the config is read in the home config, not the zero config? The IK solver is rather undocumented on how the 4 points are defined (@romanarust) And yes, if we can be sure how the 4 points are extracted. I think it is rather simple to read out the 4 points just from parsing the URDF or RobotModel object. For example, we can set the config of a RobotModel and just read out the joint origins. I think it corresponds to J2, J3, J5, J6 in Link 0 (robot base) coordinates.

[Coroush]

yep you're right. in this case zero and home config is the same. I just double checked;

I will also keep you posted in-case i find more on extracting these points from URDF or RobotModel.

thanks

[gonzalocasas]

I'm a bit late to the party, but very cool conversation! 👍
Indeed, the spherical wrist and offset write kinematic solvers are general as @yck011522 pointed out earlier, and adding a new robot is only a matter of adding the correct 4-points (or DH params in the case of the offset wrist ik solver).

Here's an example script for how to use a custom-defined one: https://github.com/compas-teaching/COMPAS-II-FS2022/blob/main/lecture_03/218_inverse_kinematics_dh_params.py (and in lecture_06 folder of that repo, you'll find a bunch of examples of how to use that in combination with pybullet).

Originally, we were not aiming at adding all robots + parameters for these analytical solver to compas_fab, but if this would be valuable, we can totally do it, because -as mentioned earlier in this thread- it is a very trivial addition.

[Coroush]

Oh amazing! this example is what I was looking for :) thanks a lot!
using this it's quite easy to add a custom robot in the code! so works great.

[Coroush]

@gonzalocasas if I follow based on your example, would I need to manually do the pre_processing and post_processing for joint values in case of SphericalWristKinematics?
similar to here:
https://github.com/compas-dev/compas_fab/blob/main/src/compas_fab/backends/kinematics/solvers/spherical_wrist_kinematics.py#L67