search

UM-ARM-Lab / pytorch_kinematics

Robot kinematics implemented in pytorch
MIT License
443 stars 42 forks source link

Add IK Test Cases for Non serial chains #43

Closed StoneT2000 closed 2 months ago

StoneT2000 commented 2 months ago

Tests for #42

Visualized looks like this:

image

Not sure why some generated IK goals are under the floor, maybe the robot URDF has poorly set joint limits?

Otherwise seems to all solve anyway:

Testing kuka_iiwa IK
IK took 0.373344 seconds
IK converged number: 2673 / 10000
IK took 30 iterations
IK solved 912 / 1000 goals
Testing widowx IK
IK took 0.244910 seconds
IK converged number: 3472 / 10000
IK took 30 iterations
IK solved 977 / 1000 goals
LemonPi commented 2 months ago

The tests failed because of some pathing issues to the new ur5 robot I added for a test - could you rebase this branch onto the latest master?

The goals are generated by sampling joint values within the joint limits without considering obstacles, so the floor is actually cosmetic. Adding constraints to IK would be interesting. The naive way is probably to accept soft constraints as a differentiable external cost function on the joint values q. Another way would be to do gradient projection using the null space of the Jacobian to satisfy the constraints.