Only Truth Matters
0%

Inverse Kinematics

Posted on Edited on In , , , ,

1. Goal

Given a goal in world coordinates, should calculate the robot joint angle that end-effector would result in the desired goal

  • Position
  • Orientation

2. Method

Ref

2.1. Jacobian Transpose

Idea: Virtual force

How: Taking the transpose of Jacobian, multiplying by error, use the velocity to update the joint angle

Pro: fast, easy to implement

Con: lead to singularities, numerical instablity

2.2. Pseudo-inverse Jacobian

How: taking the pseudo-inverse of jacobian matrix, multiplying by error, use the velocity to update the joint angle

Pro: more robust than transpose

Con: result in larger joint velocity and longer convergence time

1
2
3
4
5
6
7
8
9
10
11
12
13
14
iteration=0

current_xyz=self.fk_lambda(current_angle)[:3,3]
error=np.linalg.norm(current_xyz-target_xyz)

while error>threshold and iteration<max_iteration:
J=self.Jac(current_angle) ## get the jacobian matrix
delta_xyz=target_xyz-current_xyz ## get the error

angle_delta=np.dot(J.T,delta_xyz) ## the key step
current_angle+=alpha*angle_delta ## update the joint angle
current_xyz=self.fk_lambda(current_angle)[:3,3]
error=np.linalg.norm(current_xyz-target_xyz)
iteration+=1

2.3. Damped Least Square

How: adding a damping factor to the pseudo-inverse method

Pro: improved stability and convergence

Con: more computationally intensive

2.4. Optimization Based

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
joint_limits=np.zeros((7,2))
for i in range(7):
joint_limits[i,0]=self.joint_list[i]['bounds'][0]
joint_limits[i,1]=self.joint_list[i]['bounds'][0]
def objective_function(joint_angle):
current_xyz=self.fk_lambda(joint_angle)[:3,3]
error=np.linalg.norm(current_xyz-target_xyz)
# print(error)
return error
def inequality_constraint(joint_angle):
ineq= np.concatenate((joint_angle[:6] -joint_limits[:6,0], joint_limits[:6, 1] - joint_angle[:6])).flatten()
# print(ineq.shape)
return ineq
def equality_constraint(joint_angle):
current_xyz=self.fk_lambda(joint_angle)[:3,3]
error=current_xyz-target_xyz
return error
cons = [
{'type': 'ineq', 'fun': inequality_constraint}]
# cons=[
#     NonlinearConstraint(equality_constraint,0,0)
# ]
result = minimize(objective_function, current_angle, method='SLSQP', tol=1e-6,constraints=None, options={'disp': True,'maxiter':1000})