I am looking for a way of computing the forward kinematics. I.e. go from joint angles to TCP pose. I see that there used to be a function get_forward_kin() but it just gives the current TCP position. I need to compute the TCP pose corresponding to a given set of joint angles q.
p = get_forward_kin(q)
This seems like very basic functionality and would be incredibly useful to have. This has shown up as a question but since it doesn’t seem to exist, as far as I can tell, I’m putting this a feature request.
Can you please elaborate on the use case.
I.e. what is the application where you need the conversion from joint vector to Cartesian pose?
Others are welcome to contribute their use cases.
Hi @jbm,
My immediate need is partially as an elaborate workaround for the fact that you cannot choose specific IK solutions when calling movel or movep and as a result wrist 3 tends to get wrapped up and occasionally other wrong IK solutions get picked.
I have a specific pose that I want to move to as a joint vector. I define it as a joint vector rather than a TCP pose because only one specific kinematic configuration is acceptable due to installation constraints (i.e. elbow up, wrist 1 down, whist 3 not wrapped around). I also need to move to this arm configuration linearly in tool space but with various tools installed without their various TCP offsets effecting things. As a result, I’m forced to convert that joint vector to a TCP pose then do IK on it using the current robot joint vector as qnear to figure out what solution the IK solver would pick if I did a movel or movep to my target pose from my current pose. If it doesn’t pick the correct one I do some other things involving small movej commands to get into a configuration where I’m confident movel will pick the solution I want.
A little hard to explain but hopefully that makes sense.
I guess another feature request I should put in is being able to force specific IK solutions with movel or movep even if they generate tool space paths that don’t necessarily monotonically approach p_target along all axes.
Suppose I have an algorithme that manipulate the tcp position base on sensor input in respect with joint and cartesian position constraints.
If the joint positions of target tcp is out of range for one axis, I want to modify that joint position and recalculate the target tcp position to verify cartesian constraint before applying the command.
Is there a function “p = get_forward_kin (q)” as described by @gabe available? I would also be interested in this functionality.
To my application: I receive coordinates p[x, y, z, rx, ry, rz] from a vision system. However, I always want to keep the joint 6 in the same orientation. For me, the simplest way would be to convert my coordinates transmitted by the vision system into joint angles, compute joint 6 to the desired angle, and then carry out a forward kinematics. The background is that I would like to calculate further positions in the Cartesian coordinates based on the transmitted ones and around the joint 6 rotated position.
Unfortunately, get_forward_kin(q) is currently not available neither using URScript nor the API.
But thank you for also showing your interest in this feature, and hence improving the use case.
I would like it as well to synchronise an external motor with the robot movement during linear movements in Cartesian space specified using Q’s.
It is quite necessary in such cases because in order to find the time it takes from point A to B with velocity and acceleration given in m/s and m/s^2 I also need the distance between A and B in meters.
Having to implement the fwd kinematics by ourself is of course a solution, but it’s such a simple feature one would expect it to be part of the command set.
@jbm I second this.
We have a use case where a host system needs to be able to do a forward kinematic calculation for a path verification and planning application.
This feature request is now considered implemented!
Check out the release of PolyScope 5.0 and PolyScope 3.6.
get_forward_kin() and get_inverse_kin() now accept arguments, and will give you the result based on the given argument.
If no argument is given, it will behave as before. Check out the Script Manual for PolyScope 3.6 or 5.0.