def _getCollisionJacobian(self, col, res):
'''Compute the jacobian for one collision only. '''
contact = res.getContact(0)
g1 = self.gmodel.geometryObjects[col.first]
g2 = self.gmodel.geometryObjects[col.second]
oMc = pio.SE3(
pio.Quaternion.FromTwoVectors(
np.matrix([0, 0, 1]).T, contact.normal).matrix(), contact.pos)
joint1 = g1.parentJoint
joint2 = g2.parentJoint
oMj1 = self.rdata.oMi[joint1]
oMj2 = self.rdata.oMi[joint2]
cMj1 = oMc.inverse() * oMj1
cMj2 = oMc.inverse() * oMj2
J1 = pio.getJointJacobian(self.rmodel, self.rdata, joint1,
pio.ReferenceFrame.LOCAL)
J2 = pio.getJointJacobian(self.rmodel, self.rdata, joint2,
pio.ReferenceFrame.LOCAL)
Jc1 = cMj1.action * J1
Jc2 = cMj2.action * J2
J = (Jc1 - Jc2)[2, :]
return J
def Kid(q_, J_=None):
pinocchio.computeJointJacobians(model, data, q_)
J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
if J_ is not None:
J_[:, :] = J
M = pinocchio.crba(model, data, q_).copy()
return np.bmat([[M, J.T], [J, zero([6, 6])]])
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
# THIS FUNCTION CALLS THE FORWARD KINEMATICS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.WORLD)
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
# Print out the placement of each joint of the kinematic tree
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.WORLD)
def comp_combined_force(self, q, des_force, des_torque, in_touch):
if (np.sum(in_touch) == 0):
return np.zeros((9)), np.zeros((9))
q = self.trafo_q(q)
# TODO: I think it is sufficient if this is calculated only once:
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
J_trans_inv_mat = np.zeros((3, 3 * 12))
for i in range(3):
if (in_touch[i] == 1):
idx = i * 4 + 4
idx_low = idx - 4
J_NEW = pin.getJointJacobian(
self.model, self.data, idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J_trans = J_NEW.T
J_trans_inv = np.matmul(
np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T)
J_trans_inv_mat[:, idx_low * 3:idx * 3] = J_trans_inv
add_torque_force = np.matmul(
np.matmul(
np.linalg.pinv(np.matmul(J_trans_inv_mat.T, J_trans_inv_mat)),
J_trans_inv_mat.T), des_force)
add_torque_force1 = self.inv_trafo_q(add_torque_force[:12])
add_torque_force2 = self.inv_trafo_q(add_torque_force[12:24])
add_torque_force3 = self.inv_trafo_q(add_torque_force[24:36])
return (add_torque_force1 + add_torque_force2 +
add_torque_force3), np.zeros((9))
def dresidualWorld(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
rMi = Mref.inverse() * rdata.oMi[jid]
return np.dot(
pinocchio.Jlog6(rMi),
pinocchio.getJointJacobian(rmodel, rdata, jid,
pinocchio.ReferenceFrame.WORLD))
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel)
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_linear_vel = joint_velocity.linear
joint_angular_vel = joint_velocity.angular
joint_6v_vel = joint_velocity.vector
err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_linear_vel = frame_velocity.linear
frame_angular_vel = frame_velocity.angular
frame_6v_vel = frame_velocity.vector
err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def cid(q_, v_, tau_):
pinocchio.computeJointJacobians(model, data, q_)
J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL).copy()
J = J6[:3, :]
b = pinocchio.rnea(model, data, q_, v_, zero(model.nv)).copy()
M = pinocchio.crba(model, data, q_).copy()
pinocchio.forwardKinematics(model, data, q_, v_, zero(model.nv))
gamma = data.a[-1].linear + cross(data.v[-1].angular, data.v[-1].linear)
K = np.bmat([[M, J.T], [J, zero([3, 3])]])
r = np.concatenate([tau_ - b, -gamma])
return inv(K) * r
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
# IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = joint_velocity.vector - \
joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = frame_velocity.vector - \
frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def imp_ctrl(self, q, force_dir, joint_id, vel, pos, v, K_xy, K_z):
q = self.trafo_q(q)
v = self.trafo_q(v)
m = 1.0
d_xy = np.sqrt(4 * m * 500)
d_z = np.sqrt(4 * m * 500)
M = np.eye(3) * m
M_inv = np.linalg.pinv(M)
D = np.eye(3)
D[0, 0] = D[1, 1] = d_xy
D[2, 2] = d_z
K = np.eye(3)
K[0, 0] = K_xy
K[1, 1] = K_xy
K[2, 2] = K_z
F = 0.01 #0.05
M_joint = pin.crba(self.model, self.data, q)
pos_gain = np.matmul(K, pos)
damp_gain = np.matmul(D, vel)
# damp_gain = 0.0
force_gain = F * force_dir
acc = np.matmul(M_inv, (force_gain - pos_gain - damp_gain))
# TODO: I think it is sufficient if this is calculated only once:
full_J_var = pin.computeJointJacobiansTimeVariation(
self.model, self.data, q, v)
J_var_frame = pin.getJointJacobianTimeVariation(
self.model, self.data, joint_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
dyn_comp = np.matmul(J_var_frame, v)
acc = acc - dyn_comp
# TODO: I think it is sufficient if this is calculated only once:
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
J_NEW = pin.getJointJacobian(
self.model, self.data, joint_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J = J_NEW
J_inv = np.matmul(np.linalg.pinv(np.matmul(J.T, J)), J.T)
# print (J_inv)
torque = np.matmul(M_joint, np.matmul(J_inv, acc))
return self.inv_trafo_q(torque)
def comp_combined_torque(self, q, des_force, des_torque, in_touch, center,
p1, p2, p3):
P = [p1, p2, p3]
# only rotate when completely in touch!
if (np.sum(in_touch) != 3):
return np.zeros((9)), np.zeros((9))
q = self.trafo_q(q)
# TODO: I think it is sufficient if this is calculated only once:
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
J_trans_inv_mat = np.zeros((3, 3 * 12))
for i in range(3):
if (in_touch[i] == 1):
idx = i * 4 + 4
idx_low = idx - 4
J_NEW = pin.getJointJacobian(
self.model, self.data, idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J_trans = J_NEW.T
J_trans_inv = np.matmul(
np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T)
# print (np.shape(J_trans_inv))
J_trans_inv = np.matmul(self.skew(P[i] - center), J_trans_inv)
# print(np.shape(J_trans_inv))
# input("STOOP")
J_trans_inv_mat[:, idx_low * 3:idx * 3] = J_trans_inv
add_torque_force = np.matmul(
np.matmul(
np.linalg.pinv(np.matmul(J_trans_inv_mat.T, J_trans_inv_mat)),
J_trans_inv_mat.T), des_torque)
add_torque_force1 = self.inv_trafo_q(add_torque_force[:12])
add_torque_force2 = self.inv_trafo_q(add_torque_force[12:24])
add_torque_force3 = self.inv_trafo_q(add_torque_force[24:36])
# print (J_trans_inv_mat)
# input ("WAIT....")
# net_force = net_force + np.matmul(J_trans_inv, torque)
# print (net_force)
# input ("NET FORCE")
return np.zeros(
(9)), (add_torque_force1 + add_torque_force2 + add_torque_force3)
def cartesian_position_control(self, model, data, q, v, location,
threshold):
q = self.kinematics.trafo_q(q)
v = self.kinematics.trafo_q(v)
## Compute Jacobian ##
_JAC = pin.computeJointJacobians(self.model, self.data, q)
jacobian_list = []
for idx in self.end_effectors_id:
#pin.forwardKinematics(self.model, self.data, self.trafo_q(q))
jacobian = pin.getJointJacobian(
self.model,
self.data,
idx,
pin.ReferenceFrame.WORLD,
)[:3, :]
jacobian_list.append(jacobian)
#print('index is ',idx,'with jacobian: ',jacobian)
######################
## Get the Cartesian fingers Pose ##
# print('state robot is : ', self.robot_state[2])
# print('stacked robot state:', np.stack(self.robot_state[2], axis=1))
# print('goal state is:', self.goal_target)
fingers_xyz = np.stack(self.robot_state[2], axis=1)
xyz_error = self.goal_target - fingers_xyz
#print('Error in XYZ is:', xyz_error)
####################################
ac = np.zeros((9, ))
JOINTS = [[0, 3], [4, 7], [8, 11]]
for i in range(3):
jac = jacobian_list[i]
jac = np.linalg.pinv(jac)
er = xyz_error[:, i]
q_er = jac @ er
ac[i * 3:(i + 1) * 3] = q_er[JOINTS[i][0]:JOINTS[i][1]] - np.array(
[0.1, 0.1, 0.1]) * v[self.end_effectors_id[i] -
4:self.end_effectors_id[i] - 1]
ac = np.clip(ac, -0.36, 0.36)
return ac
def comp_effective_force(self, q, torque, in_touch):
q = self.trafo_q(q)
torque = self.trafo_q(torque)
net_force = np.zeros((3))
full_JAC = pin.computeJointJacobians(self.model, self.data, q)
for i in range(3):
if (in_touch[i] == 1):
idx = i * 4 + 4
J_NEW = pin.getJointJacobian(
self.model, self.data, idx,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[:3, :]
J_trans = J_NEW.T
J_trans_inv = np.matmul(
np.linalg.pinv(np.matmul(J_trans.T, J_trans)), J_trans.T)
net_force = net_force + np.matmul(J_trans_inv, torque)
# print (net_force)
# input ("NET FORCE")
return net_force
da_dq, da_dqn, NUMDIFF_MODIFIER *
h) # threshold was 3e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check ABA versus RNEA derivatives (with forces)
assertNumDiff(
inv(data.M) * dtau_dq, -da_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check ABA versus RNEA + forces (no derivatives)
del a
for i, f in enumerate(fs[:-1]):
fs[i] = f * 0
f = fs[-1].vector
M = pinocchio.crba(model, data, q).copy()
pinocchio.computeJointJacobians(model, data, q)
J = pinocchio.getJointJacobian(model, data, model.joints[-1].id,
pinocchio.ReferenceFrame.LOCAL).copy()
b = pinocchio.rnea(model, data, q, v, zero(model.nv)).copy()
a = pinocchio.aba(model, data, q, v, tau, fs).copy()
assert (absmax(a - (inv(M) * (tau - b + J.T * f))) < 1e-6)
tau = pinocchio.rnea(model, data, q, v, a, fs)
assert (absmax(tau - (M * a + b - J.T * f)) < 1e-6)
# ------------------------------------------------
# ------------------------------------------------
# ------------------------------------------------
# Checking linear acceleration (i.e. ahat = a.linear + w x v = [a + v x (vlinear,0)].linear )
a = rand(model.nv) * 2 - 1
dt = 1e-6
def compute(self,
trajectory,
trajectory_derivative,
duration,
initial_angles,
dt=0.1,
lam=10.0):
"""
trajectory: A lambda, the trajectory to follow
duration: The total duration of the movement
dt: Frequence of configuration update
lam: Hyper parameter for the optimisation problem to solve at each iteration
"""
# Use model.getJointId to get the id of joints that we want to control
# TODO get the correct joint
id_LH = self.model.getJointId("LHand")
data = self.model.createData()
# Initial configuration of the robot to adjust in function of the position of the system
q = self.model.neutralConfiguration
for i in range(len(self.joint_names)):
q[self.joint_ids[i]] = initial_angles[i]
nb_step = int(duration / dt)
t = 0
q_s_result = []
for step in range(nb_step):
pin.forwardKinematics(self.model, data, q)
# Update all jacobians of the robot
pin.computeJointJacobians(self.model, data, q)
# Get the Jacobians of the joint to control in the world repair
J = pin.getJointJacobian(self.model, data, id_LH,
pin.ReferenceFrame.LOCAL)[:3, 7:]
# Solve the inverse kinematics
# 1. calculer l'erreur courante en fonction de la position des organes terminaux
# data.oMi[id_du_joint] contient la position et l'orientation de l'organe par rapport au repere monde
X = data.oMi[id_LH].translation
R = data.oMi[id_LH].rotation
#print(trajectory(t)).T
Xdes = np.matrix(trajectory(t)).T
error = R.T * (X - Xdes)
error_norm = np.linalg.norm(error)
#print(error_norm)
deriv = np.matrix(trajectory_derivative(t)).T
v = -np.dot(np.linalg.pinv(J), deriv + lam * error)
z = np.matrix(np.zeros((6, 1)))
v = np.concatenate((z, v))
# v_sol is the solution of the least square problem
# t_deriv = trajectory_derivative(t)
# update current configurqtion
q = pin.integrate(self.model, q, v * dt)
#print(q.T)
#print(q[15:20].T)
q_s_result.append([float(q[15])] + [float(q[16])] +
[float(q[17])] + [float(q[18])] +
[float(q[19])] + [float(q[32])])
t += dt
# TODO Check returns type and convert to appropriate format for naoqi functions
return np.array(q_s_result)
def computeValueAndJacobian(self):
model = self.robot.model; data = self.robot.data
nj = len(self.joints)
for im, measurement in enumerate(self.measurements):
# ^
# compute configuration q = q + q
# i i off
qi = neutral(model)
for i in range(nj):
imeas = self.measurementIndices[i]
imodel = self.modelQIndices[i]
qi[imodel] = measurement['joint_states'][imeas] + \
self.variable.q_off[i,0]
forwardKinematics(model, data, qi)
computeJointJacobians(model, data)
# position of the head
Th = data.oMi[self.headId]
hTc = self.variable.hTc
if measurement.has_key('left_gripper'):
meas_cTs = measurement['left_gripper']
Tw = data.oMi[self.lwId]
wTs = self.variable.lwTls
Jw_full = getJointJacobian(model, data, self.lwId, \
ReferenceFrame.LOCAL)
left = True
else:
meas_cTs = measurement['right_gripper']
Tw = data.oMi[self.rwId]
wTs = self.variable.rwTrs
Jw_full = getJointJacobian(model, data, self.rwId, \
ReferenceFrame.LOCAL)
left = False
valueSE3 = meas_cTs.inverse()*hTc.inverse()*Th.inverse()*Tw*wTs
value = log6(valueSE3)
Jlog = Jlog6(valueSE3)
self.value[6*im+0:6*im+6] = value.vector.reshape(6,1)
# Compute Jacobian
Xh = Th.toActionMatrix()
Xw_inv = Tw.toActionMatrixInverse()
wXs_inv = wTs.toActionMatrixInverse()
hXc = hTc.toActionMatrix()
sTc = wTs.inverse()*Tw.inverse()*Th*hTc;
sXc = sTc.toActionMatrix()
# Fill reduced Jacobian matrices
Jh_full = getJointJacobian(model, data, self.headId, \
ReferenceFrame.LOCAL)
# columns relative to q_off
for i,c in enumerate (self.modelVIndices):
self.Jh[0:6,i:i+1] = Jh_full[0:6,c:c+1]
self.Jw[0:6,i:i+1] = Jw_full[0:6,c:c+1]
self.jacobian[6*im+0:6*im+6,0:nj] = \
Jlog.dot(wXs_inv).dot(-Xw_inv.dot(Xh).dot(self.Jh) + self.Jw)
# columns relative to hTc
self.jacobian[6*im+0:6*im+6,nj:nj+6] = -Jlog.dot(sXc)
# columns relative to lwTls and rwTrs
if left:
self.jacobian[6*im+0:6*im+6,nj+6: nj+12] = Jlog
self.jacobian[6*im+0:6*im+6,nj+12:nj+18] = np.zeros((6,6))
else:
self.jacobian[6*im+0:6*im+6,nj+6: nj+12] = np.zeros((6,6))
self.jacobian[6*im+0:6*im+6,nj+12:nj+18] = Jlog
