Driving a reduced coordinate articulation link towards a global pose

[NatiElgavi]

I built a reduced coordinate articulation and I can drive it through the joints (one DOF at a time), but I wonder if it possible to set a target world pose for one of its links and have the articulation figure out the kinematics for it. Essentially setting the pose of an end effector of a robotic arm and have all the links leading to be calculated by the articulation. What's the best way to go about doing this?

[kstorey-nvidia]

You can do this with the forward dynamics and the joint solver by attaching a soft constraint to the end effector that is constrained to either a static or kinematic body at the desired end effector pose. By soft, I mean a D6 with spring/damping terms that are not set infinitely stiff to ensure that it does not fight aggressively if the target is unreachable/far away. Depending on how far away the link is from its target, this approach may not get you exactly to the target in a single frame, but should converge on the target over a few frames (the stiffer the joint, the faster it will get you there but the potential for instabilities with unreachable configurations will increase).

In addition, set the joint drives on the articulation to have quite a bit of joint damping to reduce sway on the arm and you should get something that produces results quite similar to inverse kinematics.

PhysX doesn't currently provide IK but you do have access to Jacobians, kinematic state and mass information, so it would be theoretically possible to implement IK using this information yourself. You could also leverage other IK solutions, e.g. moveit and use the joint angles produced by those as either inputs for joint drives or to compute your own joint forces to drive the robot to the desired angles.

[NatiElgavi]

Thank you. A couple of follow up questions:

1. Should this D6 driving joint be added as a loop joint? It doesn't seem to work, so I'm guessing not, but I'm not sure.
2. How stiff should the articulation joints be? If i'm not directly changing their target drive, the D6 driving joint you suggested will actually work against them in a way. Meaning any pose other than the initial pose will not be a "rest" pose and it might be more sensitive to shake during collisions and so on.

Thanks, Nati.

[kstorey-nvidia]

(1) It shouldn't be necessary to add it as a loop joint. That API is specifically used to tell inverse dynamics which constraints you want to factor into its calculations.

(2) If you are going to attempt approximating IK using a joint attached to the end effector, then the joint drives should have 0 stiffness to avoid the drives fighting against the constraint. However, the joint drives do want to have quite a lot of damping, otherwise the arm will look completely limp as it is dragged around. Ideally, when the end effector has reached its target, the arm will cease any motion.

The most appropriate constraint for you to attach to the end effector would be a PxD6Joint, where all 6 motion axes are set to be "free" and a drive is applied to each drive axis. You'll probably need to experiment a bit to find a suitable set of values for the drives - I'd recommend starting off with low stiffness and increasing it. You will also probably need to pick a maxForce value for these joints so that, if the target is not reachable, the drive does not apply an overpowering amount of force that causes the simulation to become unstable.

[NatiElgavi]

Thank you for the help. I've been experimenting for a while and still didn't manage to get a stable behaviour. Here's my articulation:

• The green dot is the fixed root.
• The cyan capsule is connected to the root using a spherical joint.
• The blue capsule is connected to the cyan capsule using a prismatic joint (along its main axis).
• The yellow capsule is connected to the blue capsule using a spherical joint.
• The purple capsule (the end effector) is connected to the yellow capsule using a spherical joint.

All the joints are eFree, damped and have no stiffness. The red capsule is a kinematic actor I move around (different collision group than the articulation). I created a PxD6Joint to connect it to the end effector as you suggested. I'm working with this joint's damping, stiffness and force limit but the end effector is either following the kinematic target sluggishly or shakes like mad. Even if there are some magic numbers I can reach to make it work, the whole thing seems highly unstable. I wonder if there is anything I'm doing wrong. I'm starting to think using IK and setting drives directly is the only stable solution. Still, I rather not go to another 3rd party library if I don't have to. Any other tips?

[NatiElgavi]

I also uploaded a simpler example to here - https://github.com/NatiElgavi/PhysX/tree/DrivingArticulationsUsingKinematicActorsAndD6Joints

In this branch (DrivingArticulationsUsingKinematicActorsAndD6Joints), I took the "long chain" articulation snippet and added a "pulling joint" connecting the heavy box at the end of the chain to a nearby kinematic actor. The distance is short so there is no strain in the pull and I have followed your recommendations with regards to no stiffness, damped, force limited articulation joints and very slowly increasing the stiffness of the pulling joint. Still, the only results I was able to get are a very loose joint or and unstable one. Mind taking a look?

[kstorey-nvidia]

Thanks. I can potentially take a look towards the end of the week. I took a quick look and can make the following comments:

When I've used this technique, its generally been to implement basic IK-like functionality on a real-world robot model, e.g. a fetch, jaco, kinova arm. These have usually only a small number of degrees of freedom (e.g. ~10) between base and end effector rather than 100+ DOFs as with this long chain.

That example is more like a cooked strand of spaghetti lifting a meatball to a target :). The simulation should be able to do this with the technique I suggested (because you are essentially just using a magic joint to drag the meatball to the location in space).