def retarget(self, target_se3_in_world_frame, return_cost=False, close_joints_coefficient=1e-4):
"""
r_h: vector of fingertip targets
"""
# print(r_h)
res = minimize(self.fast_c,
self.start_angles, method="SLSQP", args=(target_se3_in_world_frame, self.prev_angles,
close_joints_coefficient),
jac=self.grad_c, tol=1e-10, options={'maxiter': 6000}, bounds=self.bounds,
)
self.prev_angles = self.start_angles
self.start_angles = res.x
print("arm retargeter")
print("optimal value")
print(res.fun)
print("\n")
if return_cost:
return res.x, res.fun # , [fk1[:3,-1], fk2[:3,-1], fk3[:3,-1], fk4[:3,-1]]
else:
return res.x
self.fk = kinDyn.forward_kinematics_fun('tool0')
def compute_fk(self, angles):
return self.fk(jnp.eye(4), angles)
We are using jax.
The FK is 100x slower than IK. I understand there is probably some symbolic computation happening for this FK, but is there a faster version somewhere in this library?