def calc(self, data, x, u=None):
self.costsData.shareMemory(data)
if u is None:
u = self.unone
q, v = x[:self.state.nq], x[-self.state.nv:]
self.actuation.calc(self.actuationData, x, u)
tau = self.actuationData.tau
# Computing the dynamics using ABA or manually for armature case
if self.enable_force:
data.xout[:] = pinocchio.aba(self.state.pinocchio,
self.pinocchioData, q, v, tau)
else:
pinocchio.computeAllTerms(self.state.pinocchio, self.pinocchioData,
q, v)
data.M = self.pinocchioData.M
if self.armature.size == self.state.nv:
data.M[range(self.state.nv),
range(self.state.nv)] += self.armature
self.Minv = np.linalg.inv(data.M)
data.xout = self.Minv * (tau - self.pinocchioData.nle)
# Computing the cost value and residuals
pinocchio.forwardKinematics(self.state.pinocchio, self.pinocchioData,
q, v)
pinocchio.updateFramePlacements(self.state.pinocchio,
self.pinocchioData)
self.costs.calc(self.costsData, x, u)
data.cost = self.costsData.cost
def f(self, x, u, t, jacobian=False):
nq = self.robot.nq
nv = self.robot.nv
model = self.robot.model
data = self.robot.data
q = x[:nq]
v = x[nq:]
if (nv == 1):
# for 1 DoF systems pin.aba does not work (I don't know why)
pin.computeAllTerms(model, data, q, v)
ddq = (u - data.nle) / data.M[0]
else:
ddq = pin.aba(model, data, q, v, u)
self.dx[:nv] = v
self.dx[nv:] = ddq
if (jacobian):
pin.computeABADerivatives(model, data, q, v, u)
self.Fx[:nv, :nv] = 0.0
self.Fx[:nv, nv:] = np.identity(nv)
self.Fx[nv:, :nv] = data.ddq_dq
self.Fx[nv:, nv:] = data.ddq_dv
self.Fu[nv:, :] = data.Minv
return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu))
return np.copy(self.dx)
def f(x, u):
q = x[:6]
dq = x[6:]
a = pinocchio.aba(robot.model, robot.data, q, dq, u)
f = np.zeros(12)
f[:6] = dq.copy()
f[6:] = a
return f
def test_aba_derivatives(self):
res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau)
self.assertTrue(len(res) == 3)
data2 = self.model.createData()
pin.aba(self.model,data2,self.q,self.v,self.tau)
self.assertApprox(self.data.ddq,data2.ddq)
# With external forces
res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau,self.fext)
self.assertTrue(len(res) == 3)
pin.aba(self.model,data2,self.q,self.v,self.tau,self.fext)
self.assertApprox(self.data.ddq,data2.ddq)
def test_aba(self):
model = self.model
ddq = pin.aba(self.model, self.data, self.q, self.v, self.ddq)
null_fext = pin.StdVec_Force()
for _ in range(model.njoints):
null_fext.append(pin.Force.Zero())
ddq_null_fext = pin.aba(self.model, self.data, self.q, self.v,
self.ddq, null_fext)
self.assertApprox(ddq_null_fext, ddq)
null_fext_list = []
for f in null_fext:
null_fext_list.append(f)
print('size:', len(null_fext_list))
ddq_null_fext_list = pin.aba(self.model, self.data, self.q, self.v,
self.ddq, null_fext_list)
self.assertApprox(ddq_null_fext_list, ddq)
def calc(self, data, x, u=None):
if u is None:
u = self.unone
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
tauq = a2m(self.actuation.calc(data.actuation, x, u))
data.xout[:] = pinocchio.aba(self.pinocchio, data.pinocchio, q, v, tauq).flat
pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
data.cost = self.costs.calc(data.costs, x, u)
return data.xout, data.cost
def forward_dyn(self, tau):
# self.fwd_dyn_method can be either Cholesky, aba, or pinMinv
if self.fwd_dyn_method == 'Cholesky':
return np.linalg.solve(self.data.M, tau - self.data.nle)
if self.fwd_dyn_method == 'aba':
return se3.aba(self.model, self.data, self.q, self.v, tau)
if self.fwd_dyn_method == 'pinMinv':
# se3.cholesky.decompose(self.model, self.data, self.q)
# se3.cholesky.computeMinv(self.model, self.data)
se3.computeMinverse(self.model, self.data, self.q)
return self.data.Minv @ (tau - self.data.nle)
raise Exception("Unknown forward dynamics method " +
self.fwd_dyn_method)
def step(self, q, vq, tauq, dt=None):
if dt is None: dt = self.dt
robot = self.robot
NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
# --- Simu
se3.forwardKinematics(robot.model, robot.data, q, v)
se3.framesKinematics(robot.model, robot.data)
Mrf = self.Mrf0.inverse() * robot.data.oMf[RF]
vrf = robot.model.frames[RF].placement.inverse() * robot.data.v[RK]
Mlf = self.Mlf0.inverse() * robot.data.oMf[LF]
vlf = robot.model.frames[LF].placement.inverse() * robot.data.v[LK]
qrf = np.vstack(
[Mrf.translation[1:],
se3.rpy.matrixToRpy(Mrf.rotation)[0]])
qlf = np.vstack(
[Mlf.translation[1:],
se3.rpy.matrixToRpy(Mlf.rotation)[0]])
frf = self.frf # Buffer where to store right force
frf[[1, 2, 3]] = self.Krf * qrf # Right force in effector frame
rf0_frf = se3.Force(frf) # Force in rf0 frame
rk_frf = (robot.data.oMi[RK].inverse() * self.Mrf0).act(
rf0_frf) # Spring force in R-knee frame.
flf = self.flf # Buffer where to store left force
flf[[1, 2, 3]] = self.Klf * qlf # Left force in effector frame
lf0_flf = se3.Force(flf) # Force in lf0 frame
lk_flf = (robot.data.oMi[LK].inverse() * self.Mlf0).act(
lf0_flf) # Spring force in L-knee frame.
self.forces = ForceDict({
RK: rk_frf,
LK: lk_flf
}, NB) # Argument to ABA
aq = se3.aba(robot.model, robot.data, q, vq, tauq, self.forces)
vq += aq * dt
q = se3.integrate(robot.model, q, vq * dt)
self.aq = aq
return q, vq
def dynamics(self,x,u,display=False):
'''
Dynamic function: x,u -> xnext=f(x,y).
Put the result in x (the initial value is destroyed).
Also compute the cost of making this step.
Return x for convenience along with the cost.
'''
modulePi = (lambda th: (th+np.pi)%(2*np.pi)-np.pi) if self.modulo else (lambda th: th)
sumsq = lambda x : np.sum(np.square(x))
cost = 0.0
q = modulePi(x[:self.nq])
v = x[self.nq:]
u = np.clip(np.reshape(np.matrix(u),[self.nu,1]),-self.umax,self.umax)
DT = self.DT/self.NDT
for i in range(self.NDT):
tau = u-self.Kf*v
if self.armature is None:
a = se3.aba(self.model,self.data,q,v,tau)
else:
se3.computeAllTerms(self.model,self.data,q,v)
M = self.data.M
#M.flat[::self.nv+1] += self.armature
b = self.data.nle
a = inv(M+self.armature*eye(self.nv))*(tau-b)
v += a*DT # TODO
q += v*DT
cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT
if display:
self.display(q)
time.sleep(self.DT/20)
x[:self.nq] = modulePi(q)
x[self.nq:] = np.clip(v,-self.vmax,self.vmax)
return x,-cost
def dynAndCost(self, x, u, verbose=False):
x = x.copy()
q, v = x[:self.nq], x[-self.nv:]
u = np.clip(u, -self.umax, self.umax)
cost = 0.
dt = self.DT / self.NDT
for t in range(self.NDT):
# Add friction
if self.Kf > 0.0: tau = u - self.Kf * v
# Evaluate cost
cost += self.cost(np.concatenate([q, v]), u) / self.NDT
#print(self,self.cost,t,x,u,cost)
# Evaluate dynamics
a = pio.aba(self.rmodel, self.rdata, q, v, tau)
if verbose: print(q, v, tau, a)
v += a * dt
v = np.clip(v, self.xmin[self.nq:], self.xmax[self.nq:])
q = pio.integrate(self.rmodel, q, v * dt)
xnext = np.concatenate([q, v])
return xnext, cost
def calc(self, data, x, u=None):
if u is None:
u = self.unone
q, v = x[:self.state.nq], x[-self.state.nv:]
# Computing the dynamics using ABA or manually for armature case
if self.forceAba:
data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q,
v, u)
else:
pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q,
v)
data.M = data.pinocchio.M
if self.armature.size == self.state.nv:
data.M[range(self.state.nv),
range(self.state.nv)] += self.armature
data.Minv = np.linalg.inv(data.M)
data.xout = data.Minv * (u - data.pinocchio.nle)
# Computing the cost value and residuals
pinocchio.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)
self.costs.calc(data.costs, x, u)
data.cost = data.costs.cost
def calc(self, data, x, u=None):
if u is None:
u = self.unone
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
tauq = a2m(u)
# --- Dynamics
if self.forceAba:
data.xout[:] = pinocchio.aba(self.pinocchio, data.pinocchio, q, v,
tauq).flat
else:
pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
data.M = data.pinocchio.M
if hasattr(self.pinocchio, 'armature'):
data.M[range(nv), range(nv)] += self.pinocchio.armature.flat
data.Minv = np.linalg.inv(data.M)
data.xout[:] = data.Minv * (tauq - data.pinocchio.nle).flat
# --- Cost
pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
data.cost = self.costs.calc(data.costs, x, u)
return data.xout, data.cost
def PnCFwdDyn(robot, q, qdot, tau):
"""
q: np.array, qdot: np.array, tau: np.array
"""
mass, mass_inv, b, g, lambda_i, jac_i_bar, null_i = UpdateRobotSystem(
robot, {
'jp': q[0],
'jd': q[1]
}, {
'jp': qdot[0],
'jd': qdot[1]
})
qddot = np.dot(mass_inv, tau - b - g)
qddot_pin = pin.aba(robot._model, robot._data, q, qdot, tau)
# print("==========================")
# print("qddot: ", qddot)
# print("qddot_pin: ", qddot_pin)
return qddot
d = m.createData()
# Initialize external forces in joint space
fext = pin.StdVec_Force()
for k in range(m.njoints):
fext.append(pin.Force.Zero())
for i in range(0, 15):
print("t: ", end="")
print(t[i])
# Set external force -- WRONG ASSUMPTION
# for j in range(1, m.njoints):
# fext[j].angular = np.array([0.,0.,qext[i+1][j-1]])
# Evaluate forward dynamics
pin.computeAllTerms(m, d, qpos[i], qvel[i])
aunc = pin.aba(m, d, qpos[i], qvel[i], ctrl[i + 1])
acon = pin.aba(m, d, qpos[i], qvel[i], ctrl[i + 1] + qext[i + 1])
# acon_ext = pin.aba(m, d, qpos[i], qvel[i], ctrl[i+1], fext)
# Print variables
print(" qext: ", end="")
print(qext[i])
print(" qfrc: ", end="")
print(qfrc[i])
print(" ctrl: ", end="")
print(ctrl[i + 1])
print(" qpos: ", end="")
print(qpos[i])
print(" qvel: ", end="")
print(qvel[i])
print(" aunc: ", end="")
print(aunc)
The AB algorithm compute the forward dynamics ie $\dot{v}_q = aba(q,v_q,\tau_q)$
"""
q = pinocchio.randomConfiguration(rmodel)
vq = rand(rmodel.nv) * 2 - 1
aq = rand(rmodel.nv) * 2 - 1
# We compute Tau_q through the RNE algorithm
tauq = pinocchio.rnea(rmodel, rdata, q, vq, aq)
print(tauq.T)
# We compute forward dynamics through ABA algorithm
aq_check = pinocchio.aba(rmodel, rdata, q, vq, tauq)
print(norm(aq - aq_check))
# The mass matrix, M, is computed through the CRBA algorithm ( Composite Rigid Body Algorithm)
M = pinocchio.crba(rmodel, rdata, q)
print(eig(M)[0])
# When you need M, you also typically need $b(q,v_q) = c(q,v_q)+g(q)$
# that we sometime name the bias, or drift, or affine term of the dynamics.
# You can compute both with:
pinocchio.computeAllTerms(rmodel, rdata, q, vq)
print(norm(M - rdata.M))
nle_check = pinocchio.rnea(rmodel, rdata, q, vq, 0 * aq)
print(norm(rdata.nle - nle_check))
frameAcceleration.vector) < 1e-9)
'''
(a,f) = K^-1 (tau-b,-gamma)
avec K = [ M J* ; J 0 ]
(a',f') = -K^-1 K' K^-1 (tau-b,-gamma) - K^-1 (b';gamma')
= -Ki K' (a,f) - Ki (b';g')
= -Ki ( K'(a,f) - (b',g') )
'''
# Define finite-diff routines.
# Check ABA derivatives (without forces)
da_dq = df_dq(model, lambda q_: pinocchio.aba(model, data, q_, v, tau), q)
da_dv = df_dx(lambda v_: pinocchio.aba(model, data, q, v_, tau), v)
pinocchio.computeABADerivatives(model, data, q, v, tau)
h = np.sqrt(2 * EPS)
assertNumDiff(
da_dq, data.ddq_dq, NUMDIFF_MODIFIER *
h) # threshold was 1e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
da_dv, data.ddq_dv, NUMDIFF_MODIFIER *
h) # threshold was 1e-2, is now 2.11e-4 (see assertNumDiff.__doc__)
# Check RNEA Derivatives (without forces)
a = pinocchio.aba(model, data, q, v, tau)
dtau_dq = df_dq(model, lambda q_: pinocchio.rnea(model, data, q_, v, a), q)
# Random states and control
q = pin.utils.rand(m.nq)
if use_free_jnt:
q[3:7] = q[3:7]/np.linalg.norm(q[3:7]) # normalize quaternion if free joint
v = pin.utils.rand(m.nv)
u = pin.utils.rand(m.nv)
# Set the external forces
fext = pin.StdVec_Force()
for k in range(m.njoints):
fext.append(pin.Force.Zero())
# Evaluate all terms and forward dynamics
pin.computeAllTerms(m, d, q, v)
a = pin.aba(m, d, q, v, u, fext)
# Print state and controls
print("pos =", q)
print("vel =", v)
print("tau =", u)
print("acc =", a)
print("nle =", d.nle)
# Evaluate the ABA derivatives
pin.computeABADerivatives(m, d, q, v, u, fext)
print("\nABA derivatives:")
print("ddq_dq:\n", d.ddq_dq)
print("ddq_dv:\n", d.ddq_dv)
print("M:\n", d.M)
print("Minv:\n", d.Minv)
def forward_dynamics(self, angle, velocity, actuator_torque):
joint_torque = actuator_torque + self.friction_torque(velocity)
return pinocchio.aba(self.model, self.data, angle, velocity,
joint_torque)
import numpy as np
model = pin.buildSampleModelHumanoid()
model.lowerPositionLimit[:7] = -np.ones(7)
model.upperPositionLimit[:7] = +np.ones(7)
pool = pin.ModelPool(model)
num_threads = pin.omp_get_max_threads()
batch_size = 128
q = np.empty((model.nq, batch_size))
for k in range(batch_size):
q[:, k] = pin.randomConfiguration(model)
v = np.zeros((model.nv, batch_size))
a = np.zeros((model.nv, batch_size))
tau = np.zeros((model.nv, batch_size))
print("num_threads: {}".format(num_threads))
print("batch_size: {}".format(batch_size))
# Call RNEA
res_rnea = np.empty((model.nv, batch_size))
pin.rnea(num_threads, pool, q, v, a, res_rnea) # Without allocation
res_rnea2 = pin.rnea(num_threads, pool, q, v, a) # With allocation
# Call ABA
res_aba = np.empty((model.nv, batch_size))
pin.aba(num_threads, pool, q, v, tau, res_aba) # Without allocation
res_aba2 = pin.aba(num_threads, pool, q, v, tau) # With allocation
def calc(self, q):
g = pin.computeGeneralizedGravity(self.rmodel, self.rdata, q)
taugrav = -pin.aba(self.rmodel, self.rdata, q, self.v0, self.v0)
return np.dot(taugrav, g)
def f(self, x, u, t, jacobian=False):
nq = self.robot.nq
nv = self.robot.nv
model = self.robot.model
data = self.robot.data
q = x[:nq]
v = x[nq:]
i = 0
self.robot.computeAllTerms(q, v)
for cs in self.contact_surfaces: # for each candidate contact surface
for cp in self.contact_points: # for each candidate contact point
# Contact point placement
H = self.robot.framePlacement(q, cp.frame_id, False)
p_c = H.translation
if (cs.check_collision(p_c)
): # check whether the point is colliding with the surface
if (cp.active == False
): # if the contact was not already active
cp.active = True
cp.p0 = np.copy(p_c) # anchor point
# Compute the contact force
self.fc[i:i + 3] = cs.compute_force(cp, q, v, self.robot)
# compute the jacobian
self.Jc[i:i + 3, :] = cp.get_jacobian()
i += 3
else: # if the point is not colliding more
if (cp.active): # if the contact was already active
cp.active = False
# Contact force equal to 0
self.fc[i:i + 3] = np.zeros(3)
# jacobian equl to zero
self.Jc[i:i + 3, :] = np.zeros((3, self.robot.model.nv))
i += 3
# compute JT*force from contact point
u_con = u + self.Jc.T.dot(self.fc)
if (nv == 1):
# for 1 DoF systems pin.aba does not work (I don't know why)
ddq = (u_con - data.nle) / data.M[0]
else:
ddq = pin.aba(model, data, q, v, u_con)
self.dx[:nv] = v
self.dx[nv:] = ddq
if (jacobian):
pin.computeABADerivatives(model, data, q, v, u_con)
self.Fx[:nv, :nv] = 0.0
self.Fx[:nv, nv:] = np.identity(nv)
self.Fx[nv:, :nv] = data.ddq_dq
self.Fx[nv:, nv:] = data.ddq_dv
self.Fu[nv:, :] = data.Minv
return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu))
return np.copy(self.dx)
sys.exit(0)
# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)
# Play a bit with the simulation
dt = 0.01
T = 5
N = math.floor(T / dt)
model.lowerPositionLimit.fill(-math.pi)
model.upperPositionLimit.fill(+math.pi)
q = pin.randomConfiguration(model)
v = np.zeros((model.nv))
t = 0.
data_sim = model.createData()
for k in range(N):
tau_control = np.zeros((model.nv))
a = pin.aba(model, data_sim, q, v, tau_control) # Forward dynamics
v += a * dt
#q += v*dt
q = pin.integrate(model, q, v * dt)
viz.display(q)
time.sleep(dt)
t += dt
import numpy as np
import scipy as sp
import time
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'ur_description/urdf/ur5_gripper.urdf')
robot = RobotWrapper(URDF, [PKG])
robot.initDisplay(loadModel=True)
#Initial Set up & Variables
q = rand(robot.nq)
vq = zero(robot.nv)
aq = zero(robot.nv)
se3.forwardKinematics(robot.model, robot.data, q)
robot.display(q)
input_specified = None
dt = 0.001
while 1:
if input_specified:
pass
else:
tau = np.full((1, robot.nv), 10).T
#tau = zero(robot.nv)
aq = se3.aba(robot.model, robot.data, q, vq, tau)
vq += aq * dt
q = se3.integrate(robot.model, q, vq * dt)
robot.display(q)
