def compute(self, x_des, M_des, q0):
self.frame_id = self.model.getFrameId('tool0')
q_i = q0
H_des = pin.SE3(M_des, x_des)
for i in range(self.n):
self.robot.computeAllTerms(q_i, np.zeros(6))
H = self.robot.framePlacement(q_i, self.frame_id, False)
dH = H_des.actInv(H)
error = pin.log(dH).vector
if (norm(error) < self.eps):
self.success = True
break
# J = self.robot.frameJacobian(q_i, self.frame_id, False)
J = pin.computeJointJacobian(self.model, self.data, q_i, 6)
v = -J.T.dot(solve(J.dot(J.T) + self.damp * np.eye(6), error))
q_i = pin.integrate(self.model, q_i, v * self.DT)
return q_i
def ik(robot,oMdes, JOINT_ID,eps = 1e-4):
q = robot.q0.copy()
IT_MAX = 500
DT = 1e-1
damp = 1e-12
i=0
success = False
while True:
pinocchio.forwardKinematics(robot.model,robot.data,q)
dMi = oMdes.actInv(robot.data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(robot.model,robot.data,q,JOINT_ID)
v = - J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(robot.model,q,v*DT)
i += 1
if not success:
#raise( Exception('ik did not converged') )
q[:] = np.nan
if ((q < robot.model.lowerPositionLimit).any() or
(q > robot.model.upperPositionLimit).any()):
#Violate joint limits
q[:] = np.nan
return (q)
def solve(self, M_des, robot, joint_id, q=None):
""" Inverse kinematics for specified joint (joint_id) and desired pose M_des (array 4x4)
NOTE The code below is adopted from here (01.03.2020):
https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html
"""
oMdes = pinocchio.SE3(M_des[:3, :3], M_des[:3, 3])
if np.all(q == None): q = pinocchio.neutral(robot.model)
i = 0
iter_resample = 0
while True:
# forward
pinocchio.forwardKinematics(robot.model, robot.data, q)
# error between desired pose and the current one
dMi = oMdes.actInv(robot.data.oMi[joint_id])
err = pinocchio.log(dMi).vector
# Termination criteria
if norm(err) < self.inv_kin_sol_params.eps:
success = True
break
if i >= self.inv_kin_sol_params.max_iter:
if iter_resample <= self.inv_kin_sol_params.max_resample:
q = pinocchio.randomConfiguration(robot.model)
iter_resample += 1
continue
else:
success = False
break
# Jacobian
J = pinocchio.computeJointJacobian(robot.model, robot.data, q,
joint_id)
# P controller (?)
# v* =~ A * e
v = -J.T.dot(
solve(
J.dot(J.T) + self.inv_kin_sol_params.damp * np.eye(6),
err))
# integrate (in this case only sum)
q = pinocchio.integrate(robot.model, q,
v * self.inv_kin_sol_params.dt)
i += 1
if not success: q = pinocchio.neutral(robot.model)
q_arr = np.squeeze(np.array(q))
q_arr = np.mod(np.abs(q_arr), 2 * np.pi) * np.sign(q_arr)
mask = np.abs(q_arr) > np.pi
# If angle abs(q) is larger than pi, represent angle as the shorter angle inv_sign(q) * (2*pi - abs(q))
# This is to avoid conflicting with angle limits.
q_arr[mask] = (-1) * np.sign(
q_arr[mask]) * (2 * np.pi - np.abs(q_arr[mask]))
return success, q_arr
def inverse_kinematics(self, x, y, z):
"""Calculate the joint values for a desired cartesian position"""
model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
"/home/gabriele/catkin_ws/src/abel_move/scripts/abel_arms_full.urdf"
)
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
JOINT_ID = 5
oMdes = pinocchio.SE3(np.eye(3), np.array([x, y, x]))
q = np.array(abel.sort_joints()[5:])
eps = 1e-1
IT_MAX = 1000
DT = 1e-4
damp = 1e-4
i = 0
while True:
pinocchio.forwardKinematics(model, data, q)
dMi = oMdes.actInv(data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(model, q, v * DT)
if not i % 10:
print('%d: error = %s' % (i, err.T))
i += 1
if success:
print("Convergence achieved!")
else:
print(
"\nWarning: the iterative algorithm has not reached convergence to the desired precision"
)
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)
point = JointTrajectoryPoint()
point.positions = [
0, 0, 0, 0, 0, q[0], q[1], q[2], q[3], q[4], q[5], q[6], q[7],
q[8], q[9]
]
abel.move_all_joints(point)
self.direct_kinematics()
def calcDiff(self, q):
Jp = pinv(
pin.computeJointJacobian(robot.model, robot.data, q,
self.jointIndex))
res = np.zeros(robot.model.nv)
v0 = np.zeros(robot.model.nv)
for k in range(6):
pin.computeForwardKinematicsDerivatives(robot.model, robot.data, q,
Jp[:, k], v0)
JqJpk = pin.getJointVelocityDerivatives(robot.model, robot.data,
self.jointIndex,
pin.LOCAL)[0]
res += JqJpk[k, :]
res *= self.calc(q)
return res
def inverse_kinematics(self, q, x_des, joint_id):
if (joint_id == 4):
joint_id = 0
elif (joint_id == 8):
joint_id = 1
elif (joint_id == 12):
joint_id = 2
q0 = self.env.pinocchio_utils.inverse_kinematics(joint_id, x_des, q)
if (q0 is not None):
return q0
q = self.trafo_q(q)
eps = 1e-3
DT = 0.04
damp = 1e-6
JOINT_ID = joint_id # 3
i = 0
pin.forwardKinematics(self.model, self.data, q)
while True:
pin.forwardKinematics(self.model, self.data, q)
x = self.data.oMi[JOINT_ID].translation
R = self.data.oMi[JOINT_ID].rotation
a = (np.reshape((x - x_des), (3, 1)))
err = np.matmul(R.T, a)
if norm(err) < eps:
success = True
return self.inv_trafo_q(q)
# return q
J = pin.computeJointJacobian(self.model, self.data, q,
JOINT_ID)[:3, :]
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(3), err))
q = pin.integrate(self.model, q, v * DT)
i += 1
if (i > 1000):
# return result latest after 1000 iterations:
return self.inv_trafo_q(q)
# return q
return q
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.full((self.model.nq, 1), np.pi)
self.q = pin.randomConfiguration(self.model, -qmax, qmax)
self.v = rand(self.model.nv)
self.tau = rand(self.model.nv)
self.v0 = zero(self.model.nv)
self.tau0 = zero(self.model.nv)
self.tolerance = 1e-9
# we compute J on a different self.data
self.J = pin.computeJointJacobian(self.model, self.model.createData(),
self.q,
self.model.getJointId('lleg6_joint'))
self.gamma = zero(6)
IT_MAX = 1000
DT = 1e-4
damp = 1e-12
i = 0
while True:
pinocchio.forwardKinematics(model, data, q)
dMi = oMdes.actInv(data.oMi[JOINT_ID])
err = pinocchio.log(dMi).vector
if norm(err) < eps:
success = True
break
if i >= IT_MAX:
success = False
break
J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID)
v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))
q = pinocchio.integrate(model, q, v * DT)
if not i % 10:
print('%d: error = %s' % (i, err.T))
i += 1
if success:
print("Convergence achieved!")
else:
print(
"\nWarning: the iterative algorithm has not reached convergence to the desired precision"
)
print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)
def calc(self, q):
J = self.J = pin.computeJointJacobian(robot.model, robot.data, q,
self.jointIndex)
return np.sqrt(det(J @ J.T))
q = robot.q0
#Compute max contact forces on the X-Z plan
#plt.style.use('_mpl-gallery-nogrid')
# make data with uneven sampling in x
N = 100
x = np.linspace(-0.65, 0.65, N)
z = np.linspace(-1.0, -0.4, N)
F = np.empty([6, N, N])
for ix in range(N):
print(f"{ix/N *100} %")
for iz in range(N):
oMdes = pinocchio.SE3(np.eye(3), np.array([x[ix], 0., z[iz]]))
q = ik(robot, oMdes, JOINT_ID)
f_max = np.ones(6) * np.nan
if not np.isnan(q).any():
J = pinocchio.computeJointJacobian(robot.model, robot.data, q,
JOINT_ID)
for axis in range(6):
c = np.array([0., 0., 0., 0., 0., 0.])
c[axis] = -1.0
A_ub = np.vstack([J.T, -J.T])
b_ub = np.concatenate([tau_max, tau_max])
f_max[axis] = linprog(c, A_ub=A_ub, b_ub=b_ub).x[axis]
#robot.display(q)
F[:, ix, iz] = f_max
np.savez("fmax_talos.npz", F=F, x=x, z=z, N=N)
embed()
