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:])
pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
data.tauq[:] = self.actuation.calc(data.actuation, x, u)
self.contact.calc(data.contact, x)
data.K[:nv, :nv] = data.pinocchio.M
if hasattr(self.pinocchio, 'armature'):
data.K[range(nv), range(nv)] += self.pinocchio.armature.flat
data.K[nv:, :nv] = data.contact.J
data.K.T[nv:, :nv] = data.contact.J
data.Kinv = np.linalg.inv(data.K)
data.r[:nv] = data.tauq - m2a(data.pinocchio.nle)
data.r[nv:] = -data.contact.a0
data.af[:] = np.dot(data.Kinv, data.r)
data.f[:] *= -1.
# Convert force array to vector of spatial forces.
self.contact.setForces(data.contact, data.f)
data.cost = self.costs.calc(data.costs, x, u)
return data.xout, data.cost
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 log_state(self, logger, prefix):
'''Log current state: the values logged are defined in CLAPTRAP_STATE_SUFFIXES
@param logger Logger object
@param prefix Prefix to add before each suffix.
'''
rpy = se3.rpy.matrixToRpy(
se3.Quaternion(self.q[6, 0], self.q[3, 0], self.q[4, 0],
self.q[5, 0]).matrix())
logger.set(prefix + "roll", rpy[0, 0])
logger.set(prefix + "pitch", rpy[1, 0])
logger.set(prefix + "yaw", rpy[2, 0])
logger.set_vector(prefix + "omega", self.v[3:6, 0])
logger.set(
prefix + "wheelVelocity",
self.v[self.robot.model.joints[self.robot.model.
getJointId("WheelJoint")].idx_v, 0])
se3.computeAllTerms(self.robot.model, self.robot.data, self.q, self.v)
energy = self.robot.data.kinetic_energy + self.robot.data.potential_energy
logger.set(prefix + "energy", energy)
logger.set(prefix + "wheelTorque", self.tau[0, 0])
w_M_base = self.robot.framePosition(
self.q, self.robot.model.getFrameId("Body"), False)
logger.set_vector(prefix + "base", w_M_base.translation)
def update_state(self, q, v):
nf, ny = self.nf, self.ny
se3.computeAllTerms(self.robot.model, self.robot.data, q, v)
se3.framesForwardKinematics(self.robot.model, self.robot.data, q) # replace with updateFramePlacements(model, data)
M = self.robot.data.M #(7,7)
Mj_diag = np.matrix(np.diag(np.diag(M[ny:,ny:])))
Jl,Jr = self.robot.get_Jl_Jr_world(q, False)
J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7)
Mlf, Mrf = self.robot.get_Mlf_Mrf(q, False)
pyl, pzl = Mlf.translation[1:].A1
pyr, pzr = Mrf.translation[1:].A1
com, com_vel = self.robot.get_com_and_derivatives(q, v)
cy, cz = com.A1
X_am = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)])
# X_com = np.hstack([np.eye(2)/M[0,0],np.eye(2)/M[0,0]])
X_com = np.hstack([np.eye(2),np.eye(2)])
self.X = np.vstack([X_com, X_am])
self.X_pinv = np.linalg.pinv(self.X)
self.A[ ny:2*ny, 2*ny:2*ny+nf] = self.X
self.H[ ny:2*ny, 2*ny:2*ny+nf] = self.X
self.XT_Mb_inv = self.X.T * np.linalg.inv(M[:ny,:ny])
Minv = np.linalg.inv(M)
Upsilon = J*Minv*J.T
S = matlib.zeros((self.na, self.nv))
S[:,self.nv-self.na:] = matlib.eye(self.na)
JSTpinv = np.linalg.pinv(J*S.T)
A = J*Minv*S.T*Mj_diag*JSTpinv
self.K_A = self.K*A
self.K_Upsilon = self.K*Upsilon
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 __call__(self, q, vq):
robot = self.robot
NV = robot.model.nv
NJ = robot.model.njoints
C = self.C
YS = self.YS
Sxv = self.Sxv
H = hessian(robot, q)
se3.computeAllTerms(robot.model, robot.data, q, vq)
J = robot.data.J
oMi = robot.data.oMi
v = [(oMi[i] * robot.data.v[i]).vector for i in range(NJ)]
Y = [(oMi[i] * robot.data.Ycrb[i]).matrix() for i in range(NJ)]
# --- Precomputations
# Centroidal matrix
for j in range(NJ):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
YS[:, j0:j1] = Y[j] * J[:, j0:j1]
# velocity-jacobian cross products.
for j in range(NJ):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
vj = v[j]
Sxv[:, j0:j1] = adj(vj) * J[:, j0:j1]
# --- Main loop
for i in range(NJ - 1, 0, -1):
i0, i1 = iv(i)[0], iv(i)[-1] + 1
Yi = Y[i]
Si = J[:, i0:i1]
vp = v[robot.model.parents[i]]
SxvY = Sxv[:, i0:i1].T * Yi
for j in ancestors(i):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
Sj = J[:, j0:j1]
# COR ===> Si' Yi Sj x vj
C[i0:i1, j0:j1] = YS[:, i0:i1].T * Sxv[:, j0:j1]
# CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
C[i0:i1, j0:j1] -= SxvY * Sj
vxYS = adjdual(v[i]) * Yi * Si
YSxv = Yi * Sxv[:, i0:i1]
Yv = Yi * vp
for ii in ancestors(i)[:-1]:
ii0, ii1 = iv(ii)[0], iv(ii)[-1] + 1
Sii = J[:, ii0:ii1]
# COR ===> Sii' Yi Si x vi
C[ii0:ii1, i0:i1] = Sii.T * YSxv
# CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
C[ii0:ii1, i0:i1] += Sii.T * vxYS
# CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
C[ii0:ii1, i0:i1] += np.matrix(
td(H[:, i0:i1, ii0:ii1], Yv, [0, 0])[:, :, 0]).T
return C
def __call__(self,q,vq):
robot = self.robot
NV = robot.model.nv
NJ = robot.model.njoints
C = self.C
YS = self.YS
Sxv = self.Sxv
H = hessian(robot,q)
pin.computeAllTerms(robot.model,robot.data,q,vq)
J = robot.data.J
oMi=robot.data.oMi
v = [ (oMi[i]*robot.data.v[i]).vector for i in range(NJ) ]
Y = [ (oMi[i]*robot.data.Ycrb[i]).matrix() for i in range(NJ) ]
# --- Precomputations
# Centroidal matrix
for j in range(NJ):
j0,j1 = iv(j)[0],iv(j)[-1]+1
YS[:,j0:j1] = Y[j]*J[:,j0:j1]
# velocity-jacobian cross products.
for j in range(NJ):
j0,j1 = iv(j)[0],iv(j)[-1]+1
vj = v[j]
Sxv[:,j0:j1] = adj(vj)*J[:,j0:j1]
# --- Main loop
for i in range(NJ-1,0,-1):
i0,i1 = iv(i)[0],iv(i)[-1]+1
Yi = Y[i]
Si = J[:,i0:i1]
vp = v[robot.model.parents[i]]
SxvY = Sxv[:,i0:i1].T*Yi
for j in ancestors(i):
j0,j1 = iv(j)[0],iv(j)[-1]+1
Sj = J[:,j0: j1 ]
# COR ===> Si' Yi Sj x vj
C[i0:i1,j0:j1] = YS[:,i0:i1].T*Sxv[:,j0:j1]
# CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
C[i0:i1,j0:j1] -= SxvY*Sj
vxYS = adjdual(v[i])*Yi*Si
YSxv = Yi*Sxv[:,i0:i1]
Yv = Yi*vp
for ii in ancestors(i)[:-1]:
ii0,ii1 = iv(ii)[0],iv(ii)[-1]+1
Sii = J[:,ii0:ii1]
# COR ===> Sii' Yi Si x vi
C[ii0:ii1,i0:i1] = Sii.T*YSxv
# CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
C[ii0:ii1,i0:i1] += Sii.T*vxYS
# CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
C[ii0:ii1,i0:i1] += np.matrix(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0]).T
return C
def simulate(self, x0, simulation_duration, dt, motor_control_law, output_name = "/tmp/claptrap.csv", verbose = True):
''' Run a simulation of given controller motor_control_law, log the results, and return the state.
@param x0 Initial state (position + velocity)
@param simulation_duration Length of the simulation
@param dt Timestep for logger - note that the controller is simulated in a continuous manner
@param motor_control_law Motor callback law, with signature motor_control_law(t, q, v) -> torque
@param output_name Optional, name of the output log file.
@param verbose Optional, whether or not to display a progress bar during simulation.
'''
self.controller = motor_control_law
# Create integrator.
solver = scipy.integrate.ode(self._dynamics)
solver.set_integrator('dopri5')
solver.set_initial_value(x0)
# Create logger
logged_values = ["Claptrap.q" + str(i) for i in range(self.robot.model.nq)] + \
["Claptrap.v" + str(i) for i in range(self.robot.model.nv)] + \
["Claptrap.energy"]
logger = Logger(logged_values)
if verbose:
pbar = tqdm.tqdm(total=simulation_duration, bar_format="{percentage:3.0f}%|{bar}| {n:.2f}/{total_fmt} [{elapsed}<{remaining}]")
t = 0
result_x = []
while solver.successful() and t < simulation_duration:
# Integrate, skip first iteration, we only want to log in this case
if t > 0:
solver.integrate(t)
if verbose:
pbar.update(dt)
result_x.append(solver.y)
q = solver.y[:self.robot.nq]
v = solver.y[self.robot.nq:]
# Log
for i in range(self.robot.model.nq):
logger.set("Claptrap.q" + str(i), q[i])
for i in range(self.robot.model.nv):
logger.set("Claptrap.v" + str(i), v[i])
pnc.computeAllTerms(self.robot.model, self.robot.data, q, v)
energy = self.robot.data.kinetic_energy + self.robot.data.potential_energy
logger.set("Claptrap.energy", energy)
logger.set("time", t)
logger.new_line()
t += dt
logger.save(output_name)
# Return time and x
return logger.data["time"][:-1], np.array(result_x)
def cost(self, x):
tauq, fr, fl = self.x2vars(x)
pinocchio.computeAllTerms(self.rmodel, self.rdata, self.q, self.vq)
b = self.rdata.nle
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
Jr = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
LOCAL)
Jl = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idL,
LOCAL)
self.residuals = m2a(b - tauq - Jr.T * fr - Jl.T * fl)
return sum(self.residuals**2)
def test_forwardDynamics_rcoeff(self):
data_no_q = self.model.createData()
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
self.tau0, self.J, self.gamma, r_coeff)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
self.J, self.gamma, r_coeff)
self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
self.assertApprox(ddq, ddq_no_q)
def updateRobotConfig(self, q, robotName, osimref=True):
#self.robots[robotName].display(q, robotName)
se3.computeAllTerms(self.robots[robotName].model,
self.robots[robotName].data, q,
self.robots[robotName].v)
#se3.forwardKinematics(self.robots[robotName].model,
# self.robots[robotName].data,
# q)
#se3.framesKinematics(self.robots[robotName].model,
# self.robots[robotName].data,
# q)
self.display(q, robotName)
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = robot.q # actuation joint position
qa_dot = robot.v # actuation joint velocity
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(qa,
ID_EE) ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.vector
v_vec = v_error.vector
for i in range(0, 3): # for position only control
p_vec[i + 3] = 0.0
v_vec[i + 3] = 0.0
f_star = Lam * (-Kp * p_vec - 2.0 * np.sqrt(Kp) * v_vec)
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
return torque
def update(self, q):
se3.computeAllTerms(self.model, self.data, self.q, self.v)
#se3.forwardKinematics(self.model, self.data, q, self.v, self.a)
#- se3::forwardKinematics
#- se3::crba
#- se3::nonLinearEffects
#- se3::computeJacobians
#- se3::centerOfMass
#- se3::jacobianCenterOfMass
#- se3::kineticEnergy
#- se3::potentialEnergy
se3.framesKinematics(self.model, self.data, q)
se3.computeJacobians(self.model, self.data, q)
se3.rnea(self.model, self.data, q, self.v, self.a)
self.biais(self.q, self.v)
self.q = q.copy()
def compute_closed_loop_transition_matrix(gains_array, robot, q, v, update=True):
gains = Gains(gains_array)
ny=3
H = matlib.zeros((3*nf+2*ny, 3*nf+2*ny))
H[:ny, ny:2*ny] = matlib.eye(ny)
H_f = matlib.zeros((3*nf,3*nf))
H_f[ :nf, nf:2*nf] = matlib.eye(nf)
H_f[nf:2*nf, 2*nf:3*nf] = matlib.eye(nf)
H_f[2*nf:, 2*nf:3*nf] = -gains.kd_bar*matlib.eye(nf)
if(update):
se3.computeAllTerms(robot.model, robot.data, q, v)
se3.framesForwardKinematics(robot.model, robot.data, q)
M = robot.data.M #(7,7)
Jl,Jr = robot.get_Jl_Jr_world(q, False)
J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7)
Mlf, Mrf = robot.get_Mlf_Mrf(q, False)
pyl, pzl = Mlf.translation[1:].A1
pyr, pzr = Mrf.translation[1:].A1
com, com_vel = robot.get_com_and_derivatives(q, v)
cy, cz = com.A1
X_am = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)])
X_com = np.hstack([np.eye(2),np.eye(2)])
X = np.vstack([X_com, X_am])
X_pinv = np.linalg.pinv(X)
Minv = np.linalg.inv(M)
Upsilon = J*Minv*J.T
S = matlib.zeros((na,nv))
S[:,nv-na:] = matlib.eye(na)
JSTpinv = np.linalg.pinv(J*S.T)
A = J*Minv*S.T*Mj_diag*JSTpinv
# compute closed-loop transition matrix
K1 = gains.kp_bar*K*A*gains.Kf
K2 = K*Upsilon + gains.kp_bar*matlib.eye(nf)
H_f[2*nf:, 1*nf:2*nf] = - K2
H_f[2*nf:, 0*nf:1*nf] = - K1
H[2*ny:, 2*ny:] = H_f
H[ ny:2*ny, 2*ny:2*ny+nf] = X
H[ -nf:, :ny] = -K1*X_pinv*gains.Kp_com
H[ -nf:, ny:2*ny] = -K1*X_pinv*gains.Kd_com
return H
def calc(self, data, x, u=None):
'''
M(vnext-v) - J^T f = 0
J vnext = 0
[MJ^T][vnext] = [Mv]
[J ][ -f ] [0 ]
[vnext] = K^-1[Mv], with K = [MJ^T;J0]
[ -f ] [0 ]
'''
nq, nv = self.nq, self.nv
q = a2m(x[:nq])
v = a2m(x[-nv:])
pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
self.impulse.calc(data.impulse, x)
data.K[:nv, :nv] = data.pinocchio.M
if hasattr(self.pinocchio, 'armature'):
data.K[range(nv), range(nv)] += self.pinocchio.armature.flat
data.K[nv:, :nv] = data.impulse.J
data.K.T[nv:, :nv] = data.impulse.J
data.Kinv = inv(data.K)
data.r[:nv] = (data.K[:nv, :nv] * v).flat
data.r[nv:] = 0
data.af[:] = np.dot(data.Kinv, data.r)
data.f[:] *= -1.
# Convert force array to vector of spatial forces.
self.impulse.setForces(data.impulse, data.f)
data.xnext[:nq] = q.flat
data.xnext[nq:] = data.vnext
if isinstance(self.costs, CostModelImpactBase):
self.costs.setImpactData(data.costs, data.vnext)
if isinstance(self.costs, CostModelSum):
for cmodel, cdata in zip(self.costs.costs.values(),
data.costs.costs.values()):
if isinstance(cmodel.cost, CostModelImpactBase):
cmodel.cost.setImpactData(cdata, data.vnext)
data.cost = self.costs.calc(data.costs, x, u=None)
return data.xnext, data.cost
def test_numdiff(self):
rmodel,rdata = self.rmodel,self.rdata
q,vq,aq,dq= self.q,self.vq,self.aq,self.dq
#### Compute d/dq VCOM with the algo.
pin.computeAllTerms(rmodel,rdata,q,vq)
pin.computeForwardKinematicsDerivatives(rmodel,rdata,q,vq,aq)
dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel,rdata)
#### Approximate d/dq VCOM by finite diff.
def calc_vc(q,vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel,rdata,q,vq)
return rdata.vcom[0]
dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q)
self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision)))
def test_numdiff(self):
rmodel, rdata = self.rmodel, self.rdata
rdata_fd = self.rdata_fd
q, vq = self.q, self.vq
#### Compute d/dq VCOM with the algo.
pin.computeAllTerms(rmodel, rdata, q, vq)
dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel, rdata)
#### Approximate d/dq VCOM by finite diff.
def calc_vc(q, vq):
""" Compute COM velocity """
pin.centerOfMass(rmodel, rdata_fd, q, vq)
return rdata_fd.vcom[0].copy()
dvc_dqn = df_dq(rmodel, lambda _q: calc_vc(_q, vq), q)
self.assertTrue(
np.allclose(dvc_dq, dvc_dqn, atol=np.sqrt(self.precision)))
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 computeProblemData(self, time, q, v):
se3.computeAllTerms(self.model, self.data, q, v)
if self.freeflyer:
M_a = self.data.M[self.u:, :]
h_a = self.robot.nle(q, v)[self.u:]
M_u = self.data.M[:self.u, :]
h_u = self.robot.nle(q, v)[:self.u]
# self.baseDynamics.setMatrix(np.hstack((M_u, -J_u)))
# self.baseDynamics.setVector(-h_u)
else:
self.M_a = self.data.M
self.h_a = self.robot.nle(q, v)
for i in range(0, len(self.taskMotions)):
c = self.taskMotions[i].task.compute(time, q, v)
self.solutionDecoded = False
return self.hqpData
def step(self, q, v, tauq, dt=None):
if dt is None: dt = self.dt
self.tauq = tauq.copy()
robot = self.robot
NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
if self.first_iter:
self.compute_f_df(q, v)
self.lpf = FirstOrderLowPassFilter(
dt, self.joint_torques_cut_frequency, tauq.A1)
self.first_iter = False
# filter desired joint torques
if (self.joint_torques_cut_frequency > 0):
self.tauq = np.matrix(self.lpf.filter_data(self.tauq.A1)).T
# add Coulomb friction to joint torques
for i in range(3, 7):
if v[i] < 0:
self.tauq[i] += self.tauc[i - 4]
elif v[i] > 0:
self.tauq[i] -= self.tauc[i - 4]
#~ dv = se3.aba(robot.model,robot.data,q,v,tauq,ForceDict(self.forces,NB))
#~ #simulazione mano! (Forces are directly in the world frame, and aba wants them in the end effector frame)
se3.forwardKinematics(robot.model, robot.data, q, v)
se3.computeAllTerms(robot.model, robot.data, q, v)
se3.updateFramePlacements(robot.model, robot.data)
se3.computeJointJacobians(robot.model, robot.data, q)
M = robot.data.M #(7,7)
h = robot.data.nle #(7,1)
Jl, Jr = robot.get_Jl_Jr_world(q)
Jc = np.vstack([Jl[1:3], Jr[1:3]]) # (4, 7)
dv = np.linalg.inv(M) * (self.tauq - h + Jc.T * self.f
) #use last forces
v_mean = v + 0.5 * dt * dv
v += dv * dt
q = se3.integrate(robot.model, q, v_mean * dt)
# WARNING: using the previous dv to compute df is an approximation!
self.compute_f_df(q, v, dv, False)
self.dv = dv
self.t += dt
return q, v
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 test_forwardDynamics_default(self):
data_no_q = self.model.createData()
self.model.gravity = pin.Motion.Zero()
ddq = pin.forwardDynamics(self.model, self.data, self.q, self.v0,
self.tau0, self.J, self.gamma)
self.assertLess(np.linalg.norm(ddq), self.tolerance)
KKT_inverse = pin.getKKTContactDynamicMatrixInverse(
self.model, self.data, self.J)
M = pin.crba(self.model, self.model.createData(), self.q)
self.assertApprox(
M,
np.linalg.inv(KKT_inverse)[:self.model.nv, :self.model.nv])
pin.computeAllTerms(self.model, data_no_q, self.q, self.v0)
ddq_no_q = pin.forwardDynamics(self.model, data_no_q, self.tau0,
self.J, self.gamma)
self.assertLess(np.linalg.norm(ddq_no_q), self.tolerance)
self.assertApprox(ddq, ddq_no_q)
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 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
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):
se3.computeAllTerms(self.model,self.data,q,v)
M = self.data.M
b = self.data.nle
#tau = u-self.Kf*v
a = inv(M)*(u-self.Kf*v-b)
v += a*DT
q += v*DT
cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT
if display:
self.display(q)
time.sleep(1e-4)
x[:self.nq] = modulePi(q)
x[self.nq:] = np.clip(v,-self.vmax,self.vmax)
return x,-cost
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
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):
pin.computeAllTerms(self.model, self.data, q, v)
M = self.data.M
b = self.data.nle
#tau = u-self.Kf*v
a = inv(M) * (u - self.Kf * v - b)
v += a * DT
q += v * DT
cost += (sumsq(q) + 1e-1 * sumsq(v) + 1e-3 * sumsq(u)) * DT
if display:
self.display(q)
time.sleep(1e-4)
x[:self.nq] = modulePi(q)
x[self.nq:] = np.clip(v, -self.vmax, self.vmax)
return x, -cost
def test_forwardDynamics_no_q(self):
data5 = self.data
data6 = self.model.createData()
data9_deprecated = self.model.createData()
pin.computeAllTerms(self.model, data5, self.q, self.v0)
pin.computeAllTerms(self.model, data6, self.q, self.v0)
pin.computeAllTerms(self.model, data9_deprecated, self.q, self.v0)
ddq5 = pin.forwardDynamics(self.model, data5, self.tau, self.J,
self.gamma)
ddq6 = pin.forwardDynamics(self.model, data6, self.tau, self.J,
self.gamma, r_coeff)
with warnings.catch_warnings(record=True) as warning_list:
ddq9_deprecated = pin.forwardDynamics(self.model, data9_deprecated,
self.q, self.v, self.tau,
self.J, self.gamma, r_coeff,
False)
self.assertTrue(
any(item.category == pin.DeprecatedWarning
for item in warning_list))
self.assertTrue((ddq5 == ddq6).all())
self.assertTrue((ddq5 == ddq9_deprecated).all())
def update_quantities(robot: jiminy.Model,
position: np.ndarray,
velocity: Optional[np.ndarray] = None,
acceleration: Optional[np.ndarray] = None,
update_physics: bool = True,
update_com: bool = True,
update_energy: bool = True,
update_jacobian: bool = False,
update_collisions: bool = True,
use_theoretical_model: bool = True) -> None:
"""Compute all quantities using position, velocity and acceleration
configurations.
Run multiple algorithms to compute all quantities which can be known with
the model position, velocity and acceleration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
- center-of-mass jacobian,
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
- collisions and distances
.. note::
Computation results are stored internally in the robot, and can
be retrieved with associated getters.
.. warning::
This function modifies the internal robot data.
.. warning::
It does not called overloaded pinocchio methods provided by
`jiminy_py.core` but the original pinocchio methods instead. As a
result, it does not take into account the rotor inertias / armatures.
One is responsible of calling the appropriate methods manually instead
of this one if necessary. This behavior is expected to change in the
future.
:param robot: Jiminy robot.
:param position: Robot position vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param update_physics: Whether or not to compute the non-linear effects and
internal/external forces.
Optional: True by default.
:param update_com: Whether or not to compute the COM of the robot AND each
link individually. The global COM is the first index.
Optional: False by default.
:param update_energy: Whether or not to compute the energy of the robot.
Optional: False by default
:param update_jacobian: Whether or not to compute the jacobians.
Optional: False by default.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
"""
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
if (update_physics and update_com and
update_energy and update_jacobian and
velocity is not None and acceleration is None):
pin.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if velocity is None:
pin.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(
pnc_model, pnc_data, position, velocity, acceleration)
if update_com:
if velocity is None:
pin.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pin.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pin.centerOfMass(
pnc_model, pnc_data, position, velocity, acceleration)
if update_jacobian:
if update_com:
pin.jacobianCenterOfMass(pnc_model, pnc_data)
pin.computeJointJacobians(pnc_model, pnc_data)
if update_physics:
if velocity is not None:
pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pin.crba(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pin.computeKineticEnergy(pnc_model, pnc_data)
pin.computePotentialEnergy(pnc_model, pnc_data)
pin.updateFramePlacements(pnc_model, pnc_data)
if update_collisions:
pin.updateGeometryPlacements(
pnc_model, pnc_data, robot.collision_model, robot.collision_data)
pin.computeCollisions(
robot.collision_model, robot.collision_data,
stop_at_first_collision=False)
pin.computeDistances(robot.collision_model, robot.collision_data)
for dist_req in robot.collision_data.distanceResults:
if np.linalg.norm(dist_req.normal) < 1e-6:
pin.computeDistances(
robot.collision_model, robot.collision_data)
break
# --- HESSIAN ---
# --- HESSIAN ---
# --- HESSIAN ---
H = hessian(robot,q)
# Compute the Hessian matrix H using RNEA, so that acor = v*H*v
vq1 = vq*0
Htrue = np.zeros([6,robot.model.nv,robot.model.nv])
for joint_i in range(1,robot.model.njoints):
for joint_j in ancestors(joint_i)[:-1]:
for i in iv(joint_i):
for j in iv(joint_j):
vq1 *= 0
vq1[i] = vq1[j] = 1.0
pin.computeAllTerms(robot.model,robot.data,q,vq1)
Htrue[:,i,j] = (robot.data.oMi[joint_i]*robot.data.a[joint_i]).vector.T
print('Check hessian = \t\t', norm(H-Htrue))
# --- dCRBA ---
# --- dCRBA ---
# --- dCRBA ---
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
# --- Validate dM/dq by finite diff
dM = np.zeros([robot.model.nv,]*3)
eps = 1e-6
def step(self, u, dt=None):
if dt is None:
dt = self.dt
# compute all quantities needed for simulation
se3.computeAllTerms(self.model, self.data, self.q, self.v)
se3.updateFramePlacements(self.model, self.data)
M = self.data.M # mass matrix
h = self.data.nle # nonlinear effects (gravity, Coriolis, centrifugal)
if (self.simulate_coulomb_friction
and self.simulation_type == 'timestepping'):
# minimize kinetic energy using time stepping
from quadprog import solve_qp
'''
Solve a strictly convex quadratic program
Minimize 1/2 x^T G x - a^T x
Subject to C.T x >= b
Input Parameters:
G : array, shape=(n, n)
a : array, shape=(n,)
C : array, shape=(n, m) matrix defining the constraints
b : array, shape=(m), default=None, vector defining the constraints
meq : int, default=0
the first meq constraints are treated as equality constraints,
all further as inequality constraints
Output: a tuple, where the first element is the optimal x.
'''
# M (v' - v) = dt*S^T*(tau - tau_c) - dt*h + dt*J^T*f
# M v' = M*v + dt*(S^T*tau - h + J^T*f) - dt*S^T*tau_c
# M v' = b + B*tau_c
# v' = Minv*(b + B*tau_c)
b = M.dot(self.v) + dt * (self.S.T.dot(u) - h)
B = -dt * self.S.T
# Minimize kinetic energy:
# min v'.T * M * v'
# min (b+B*tau_c).T*Minv*(b+B*tau_c)
# min tau_c.T * B.T*Minv*B* tau_C + 2*b.T*Minv*B*tau_c
Minv = np.linalg.inv(M)
G = B.T.dot(Minv.dot(B))
a = -b.T.dot(Minv.dot(B))
C = np.vstack((np.eye(self.na), -np.eye(self.na)))
c = np.concatenate((-self.tau_coulomb_max, -self.tau_coulomb_max))
solution = solve_qp(G, a, C.T, c, 0)
self.tau_c = solution[0]
self.v = Minv.dot(b + B.dot(self.tau_c))
self.q = se3.integrate(self.model, self.q, self.v * dt)
elif (self.simulation_type == 'euler'
or self.simulate_coulomb_friction == False):
self.tau_c = self.tau_coulomb_max * np.sign(self.v[-self.na:])
self.dv = np.linalg.solve(M, self.S.T.dot(u - self.tau_c) - h)
v_mean = self.v + 0.5 * dt * self.dv
self.v += self.dv * dt
self.q = se3.integrate(self.model, self.q, v_mean * dt)
else:
print("[ERROR] Unknown simulation type:", self.simulation_type)
self.t += dt
return self.q, self.v
# --- HESSIAN ---
# --- HESSIAN ---
# --- HESSIAN ---
H = hessian(robot, q)
# Compute the Hessian matrix H using RNEA, so that acor = v*H*v
vq1 = vq * 0
Htrue = np.zeros([6, robot.model.nv, robot.model.nv])
for joint_i in range(1, robot.model.njoints):
for joint_j in ancestors(joint_i)[:-1]:
for i in iv(joint_i):
for j in iv(joint_j):
vq1 *= 0
vq1[i] = vq1[j] = 1.0
se3.computeAllTerms(robot.model, robot.data, q, vq1)
Htrue[:, i, j] = (robot.data.oMi[joint_i] *
robot.data.a[joint_i]).vector.T
print('Check hessian = \t\t', norm(H - Htrue))
# --- dCRBA ---
# --- dCRBA ---
# --- dCRBA ---
dcrba = DCRBA(robot)
dcrba.pre(q)
Mp = dcrba()
# --- Validate dM/dq by finite diff
dM = np.zeros([
u = pin.utils.zero(m.nv)
# 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)
def c_control(dt, robot, sample, t_simu):
eigenpy.switchToNumpyMatrix()
qa = np.matrix(np.zeros((7, 1)))
qa_dot = np.matrix(np.zeros((7, 1)))
for i in range(7):
qa[i, 0] = robot.q[i]
qa_dot[i, 0] = robot.v[i]
# Method : Joint Control A q_ddot = torque
Kp = 400. # Convergence gain
ID_EE = robot.model.getFrameId(
"LWrMot3") # Control target (End effector) ID
# Compute/update all joints and frames
se3.computeAllTerms(robot.model, robot.data, qa, qa_dot)
# Get kinematics information
oMi = robot.framePlacement(qa, ID_EE) ## EE's placement
v_frame = robot.frameVelocity(qa, qa_dot, ID_EE) # EE's velocity
J = robot.computeFrameJacobian(
qa, ID_EE)[:3, :] ## EE's Jacobian w.r.t Local Frame
# Get dynamics information
M = robot.mass(qa) # Mass Matrix
NLE = robot.nle(qa, qa_dot) #gravity and Coriolis
Lam = np.linalg.inv(J * np.linalg.inv(M) * J.transpose()) # Lambda Matrix
# Update Target oMi position
wMl = copy.deepcopy(sample.pos)
wMl.rotation = copy.deepcopy(oMi.rotation)
v_frame_ref = se3.Motion() # target velocity (v, w)
v_frame_ref.setZero()
# Get placement Error
p_error = se3.log(sample.pos.inverse() * oMi)
v_error = v_frame - wMl.actInv(v_frame_ref)
# Task Force
p_vec = p_error.linear
v_vec = v_error.linear
xddotstar = (-Kp * p_vec - 2 * np.sqrt(Kp) * v_vec)
f_star = Lam * xddotstar
# Torque from Joint error
torque = J.transpose() * f_star + NLE
torque[0] = 0.0
# Selection Matrix
Id7 = np.identity(7)
fault_joint_num = 0
S = np.delete(Id7, (fault_joint_num),
axis=0) #delete fault joint corresponding row
# Selection Matrix Inverse - dynamically consistent inverse
Minv = np.linalg.inv(M)
ST = S.transpose()
SbarT = np.linalg.pinv(S * Minv * ST) * S * Minv
# Jacobian Matrix Inverse - dynamically consistent inverse
JbarT = Lam * J * np.linalg.inv(M)
#Weighting matrix
W = S * Minv * ST
Winv = np.linalg.inv(W)
#Weighted pseudo inverse of JbarT*ST
JtildeT = Winv * (JbarT * ST).transpose() * np.linalg.pinv(
JbarT * ST * Winv * (JbarT * ST).transpose())
#Null-space projection matrix
Id6 = np.identity(6)
NtildeT = Id6 - JtildeT * JbarT * ST
null_torque = 0.0 * qa_dot # joint damping
#Torque
torque = ST * (JtildeT * f_star + NtildeT * SbarT * null_torque +
SbarT * NLE)
return torque
def update_quantities(jiminy_model,
position,
velocity=None,
acceleration=None,
update_physics=True,
update_com=False,
update_energy=False,
update_jacobian=False,
use_theoretical_model=True):
"""
@brief Compute all quantities using position, velocity and acceleration
configurations.
@details Run multiple algorithms to compute all quantities
which can be known with the model position, velocity
and acceleration configuration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
(- center-of-mass jacobian : No Python binding available so far),
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
Computation results are stored in internal
data and can be retrieved with associated getters.
@note This function modifies internal data.
@param jiminy_model The Jiminy model
@param position Joint position vector
@param velocity Joint velocity vector
@param acceleration Joint acceleration vector
@pre model_.nq == positionIn.size()
@pre model_.nv == velocityIn.size()
@pre model_.nv == accelerationIn.size()
"""
if use_theoretical_model:
pnc_model = jiminy_model.pinocchio_model_th
pnc_data = jiminy_model.pinocchio_data_th
else:
pnc_model = jiminy_model.pinocchio_model
pnc_data = jiminy_model.pinocchio_data
if (update_physics and update_com and \
update_energy and update_jacobian and \
velocity is not None):
pnc.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if update_physics:
if velocity is not None:
pnc.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pnc.crba(pnc_model, pnc_data, position)
if update_jacobian:
# if update_com:
# pnc.getJacobianComFromCrba(pnc_model, pnc_data)
pnc.computeJointJacobians(pnc_model, pnc_data)
if update_com:
if velocity is None:
pnc.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity,
acceleration)
else:
if velocity is None:
pnc.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pnc.kineticEnergy(pnc_model, pnc_data, position, velocity,
False)
pnc.potentialEnergy(pnc_model, pnc_data, position, False)
def solve(self, t, q, v, f_meas, df_meas=None):
robot = self.robot
NQ, NV, NB, RF, LF, RK, LK = self.NQ, self.NV, self.NB, self.RF, self.LF, self.RK, self.LK
w_post = self.w_post
Kp_post, Kd_post = self.Kp_post, self.Kd_post
se3.computeAllTerms(robot.model, robot.data, q, v)
se3.updateFramePlacements(robot.model, robot.data)
se3.rnea(robot.model, robot.data, q, v, 0 * v)
M = robot.data.M #(7,7)
h = robot.data.nle #(7,1)
Jl, Jr = robot.get_Jl_Jr_world(q, False)
Mlf, Mrf = robot.get_Mlf_Mrf(q, False)
#4th order CoM via feet acceleration task **********************
'''com_s_des -> ddf_des -> feet_a_des'''
# Estimate center of mass and derivatives
if self.estimator is None:
#take the measurement state as estimate, assume jerk is measured by df
com_mes, com_v_mes, com_a_mes, com_j_mes = robot.get_com_and_derivatives(
q, v, f_meas, df_meas)
am_est = robot.get_angularMomentum(q, v)
com_est, com_v_est, com_a_est, com_j_est, f_est, df_est = com_mes, com_v_mes, com_a_mes, com_j_mes, f_meas, df_meas
else:
com_mes, com_v_mes, com_a_mes = robot.get_com_and_derivatives(
q, v, f_meas)
am = robot.get_angularMomentum(q, v)
p = np.hstack((Mlf.translation[1:].A1, Mrf.translation[1:].A1))
self.estimator.predict_update(com_mes.A1, com_v_mes.A1,
np.array([am]), f_meas.A1, p,
self.data.ddf_des.A1)
(com_est, com_v_est, am_est, f_est,
df_est) = self.estimator.get_state(True)
dummy1, dummy2, com_a_est, com_j_est = robot.get_com_and_derivatives(
q, v, f_est, df_est)
# DEBUG
# com_mes, com_v_mes, com_a_mes, com_j_mes = robot.get_com_and_derivatives(q, v, f_meas, df_meas)
# am_est = robot.get_angularMomentum(q, v)
# com_est, com_v_est, com_a_est, com_j_est = com_mes, com_v_mes, com_a_mes, com_j_mes
# f_est, df_est = f_meas, df_meas
# f_est = f_meas
# Formulate dynamic constrains (ne sert a rien...)************* todo remove and make the problem smaller
# |M -Jc.T -S.T| * [dv,f,tau].T = -h
Jc = np.vstack([Jl[1:3], Jr[1:3]]) # (4, 7)
driftLF, driftRF = robot.get_driftLF_driftRF_world(q, v, False)
dJcdq = np.vstack([driftLF.vector[1:3], driftRF.vector[1:3]])
S = np.hstack([matlib.zeros([4, 3]), matlib.eye(4)]) # (4,7)
Aec = np.vstack([
np.hstack([M, -Jc.T, -S.T]),
np.hstack(
[matlib.zeros([4, 7]),
matlib.eye(4),
matlib.zeros([4, 4])])
])
# bec = np.vstack([-h, f_est])
# bec = np.vstack([-h, f_meas])
# use f+dt/2*df instead of f
bec = np.vstack([-h, f_meas + 0.5 * self.dt * df_est])
# CoM TASK COMPUTATIONS
com_s_des, Xc, Nc = self._compute_com_task(t, com_est, com_v_est,
com_a_est, com_j_est)
# ANGULAR MOMENTUM TASK COMPUTATIONS
dddam_des, Xl, Nl = self._compute_ang_mom_task(q, v, am_est, com_est,
com_v_est, com_a_est,
f_est, df_est)
u_des = np.vstack([com_s_des, dddam_des]) # CoM + Angular Momentum
X = np.vstack([Xc, Xl]) # CoM + Angular Momentum
N = np.vstack([Nc, Nl]) # CoM + Angular Momentum
b_momentum = u_des - N - X * self.Kspring * dJcdq
A_momentum = np.hstack([
X * self.Kspring * Jc,
matlib.zeros([X.shape[0], 4 + 4])
]) # zeros for columns corresponding to forces and torques
#posture task *************************************************
post_p_err = matlib.zeros(7).T
post_p_err[:2] = q[:2] - self.post_p_ref[:2] # Y-Z error
post_p_err[2] = np.arcsin(q[3]) - np.arcsin(
self.post_p_ref[3]) # Angular error
post_p_err[3:] = q[4:] - self.post_p_ref[4:] # Actuation error
post_v_err = v - self.post_v_ref
post_a_des = self.post_a_ref - Kp_post * post_p_err - Kd_post * post_v_err
b_post = w_post * post_a_des
#stack all tasks
A = np.vstack([A_momentum, self.A_post])
b = np.vstack([b_momentum, b_post])
# Friction cone inequality constraints Aic * x >= bic
dt_f = 1.1 * self.dt
# Aic = -dt_f*B_f*self.Kspring*Jc
# viab_marg = -B_f*df_est + np.sqrt(-2*self.ddf_max*(B_f*f_est-b_f))
# if (viab_marg<=0.0).any():
# print "%.3f Warning: negative viability margins!"%(t), viab_marg.T
#
# margin = b_f - B_f*(f_est+dt_f*df_est)
# if (margin<=0.0).any():
# margin[margin<=0.0] = 0.0
# print "%.3f Warning: negative predicted margins!"%(t), margin.T
# bic = -np.sqrt(2*self.ddf_max*margin) + B_f*df_est + dt_f*B_f*self.Kspring*dJcdq
b1 = np.matlib.ones(self.b_f.shape[0]).T
B_ddf_max = np.abs(self.B_f * np.matlib.ones((4, 1)) * self.ddf_max)
pos = self.B_f * f_est
vel = self.B_f * df_est
pos_min = -1e100 * b1
pos_max = self.b_f.copy()
vel_max = 1e100 * b1
# viabViol = areStatesViable(pos, vel, pos_min, pos_max, vel_max, B_ddf_max);
# if(np.sum(viabViol)>0):
# print "Some constraints are not viable", np.where(viabViol)[0]
ind_q = (pos >= pos_max).A1
if (np.sum(ind_q) > 0):
new_pos_max = compute_min_q_upper_bound(pos[ind_q], vel[ind_q],
B_ddf_max[ind_q])
# print "TSID-FLEX WARNING: some forces violate friction cones:", np.where(ind_q)[0], (pos[ind_q]-pos_max[ind_q]).T,
# print " old UB", pos_max[ind_q].T, "new UB", new_pos_max.T
# print "TSID-FLEX WARNING: update B_f_max", np.where(ind_q)[0], "from", pos_max[ind_q].T, "to", new_pos_max.T
pos_max[ind_q] = new_pos_max + 1e-6
# pos_max[ind_q] = pos[ind_q] + 0.1
B_ddf_stop = compute_min_ddq_stop(pos, vel, pos_min, pos_max)
ind = (B_ddf_stop > B_ddf_max).A1
if (np.sum(ind) > 0):
self.ind = ind
# print "TSID-FLEX WARNING: update B_ddf_max", np.where(ind)[0], "from", B_ddf_max[ind].T, "to", B_ddf_stop[ind].T
B_ddf_max[ind] = B_ddf_stop[ind]
(B_ddf_LB,
B_ddf_UB) = computeAccLimits(pos,
vel,
pos_min,
pos_max,
vel_max,
B_ddf_max,
dt_f,
verbose=True,
IMPOSE_ACCELERATION_BOUNDS=False)
# B * ddf <= B_ddf_max
# B * K * ddp <= B_ddf_max
# B * K * J * dv <= B_ddf_max - B * K * dJdq
# -B * K * J * dv >= -B_ddf_max + B * K * dJdq
Aic = -self.B_f * self.Kspring * Jc
bic = -B_ddf_UB + self.B_f * self.Kspring * dJcdq
Aic = np.hstack([Aic, matlib.zeros((Aic.shape[0], 8))])
#stack equality and inequality constrains
Ac = np.vstack([Aec, Aic])
bc = np.vstack([bec, bic])
# Ac=Aec
# bc=bec
#formulate the least squares as a quadratic problem *************
H = (A.T * A).T + self.HESSIAN_REGULARIZATION * np.eye(A.shape[1])
g = (A.T * b).T
y_QP = solve_qp(H.A, g.A1, Ac.A.T, bc.A1, bec.shape[0])[0]
# compute active inequalities
margins = Aic * np.matrix(y_QP).T - bic
if (margins <= 1e-5).any():
# print "%.4f Active inequalities:"%(t), np.where(margins<=1e-5)[0] #margins.T
self.ind = (margins <= 1e-5).A1
# Solve with inverse *******************************************
if 0:
Mu = robot.data.M[:3]
JuT = Jc.T[:3]
hu = h[:3]
A_ = np.vstack([Mu, Jc, w_post * Jpost])
b_ = np.vstack(
[JuT * fc - hu, feet_a_des - dJcdq, w_post * post_a_des])
#~ A_ = np.vstack([ Mu,Jc])
#~ b_ = np.vstack([ JuT*fc - hu, feet_a_des - dJcdq ])
dv_PINV = np.linalg.pinv(A_) * b_
tau_PINV = S * (
M * dv_PINV + h - Jc.T * fc
) #get tau_des from dv_des and measured contact forces:
f_PINV = fc
y_PINV = np.vstack([dv_PINV, f_PINV, tau_PINV]).A1
assert isapprox(y_PINV, y_QP)
dv = np.matrix(y_QP[:7]).T
f = np.matrix(y_QP[7:7 + 4]).T
tau = np.matrix(y_QP[7 + 4:]).T
# compute ddf_des for next loop EKF computations
feet_a_des = Jc * dv + dJcdq
self.data.ddf_des = self.Kspring * feet_a_des
# store data
self.data.lf_a_des = feet_a_des[:2]
self.data.rf_a_des = feet_a_des[2:]
self.data.com_p_mes = com_mes.A1
self.data.com_v_mes = com_v_mes.A1
self.data.com_a_mes = com_a_mes.A1
#~ self.data.com_j_mes = com_j_mes.A1 # should not be able to measure jerk
self.data.com_p_est = com_est.A1
self.data.com_v_est = com_v_est.A1
self.data.com_a_est = com_a_est.A1
self.data.com_j_est = com_j_est.A1
self.data.com_s_des = com_s_des.A1
self.data.com_s_exp = (X * self.data.ddf_des).A1
self.data.lkf = f[:2].A1
self.data.rkf = f[2:].A1
self.data.tau = tau
self.data.dv = dv
self.data.f = f
self.data.B_ddf_max = -B_ddf_max
self.data.B_ddf_UB = -B_ddf_UB
self.data.B_ddf_des = -self.B_f * self.data.ddf_des
self.data.B_df = -vel
(B_df_min, B_df_max) = computeVelViabBounds(pos, pos_min, pos_max,
B_ddf_max)
self.data.B_df_max = -B_df_max
self.data.B_f = pos_max - pos
DEBUG_VIAB = 0
if DEBUG_VIAB:
dt = self.dt
pos_next = pos + dt * vel + 0.5 * dt * dt * self.data.B_ddf_des
vel_next = vel + dt * self.data.B_ddf_des
max_pos_next = pos + dt * vel + 0.5 * dt * dt * B_ddf_UB
max_vel_next = vel + dt * B_ddf_UB
viabViol = areStatesViable(max_pos_next, max_vel_next, pos_min,
pos_max, vel_max, B_ddf_max)
if (np.sum(viabViol) > 0):
print "%.4f ERROR Some constraints will not be viable at next time step" % (
t), np.where(viabViol)[0]
# if(t>0.16):
# print " %.4f Current value of B_f "%(t), pos.T
# print " %.4f Future value of B_f "%(t), pos_next.T
# print " %.4f Max future value of B_f "%(t), max_pos_next.T
# print " %.4f Current value of B_df"%(t), vel.T
# print " %.4f Future value of B_df"%(t), vel_next.T
# print " %.4f Max future value of B_df"%(t), max_vel_next.T
if (np.sum(ind) > 0):
B_ddf_max = np.abs(self.B_f * np.matlib.ones(
(4, 1)) * self.ddf_max)
# print "%.4f WARNING: increase max constr acc"%(t), np.where(ind)[0], 'from', B_ddf_max[ind].T, 'to', B_ddf_stop[ind].T, \
# 'B_f', (pos_max[ind]-pos[ind]).T
# print "%.4f WARNING: increase max constr acc"%(t), np.where(ind)[0], 'from', B_ddf_max[ind].T, 'to', B_ddf_stop[ind].T, \
# 'B_ddf_UB', B_ddf_UB[ind].T, 'B_ddf_des', self.data.B_ddf_des[ind].T, 'B_df', vel[ind].T, 'B_f', (pos_max[ind]-pos[ind]).T
# elif(np.sum(self.ind)>0):
# ind = self.ind
# print "%.4f DEBUG: "%(t), np.where(ind)[0], 'B_ddf_max', B_ddf_max[ind].T, 'B_ddf_stop', B_ddf_stop[ind].T, \
# 'B_ddf_UB', B_ddf_UB[ind].T, 'B_ddf_des', self.data.B_ddf_des[ind].T, 'B_df', vel[ind].T, 'B_f', (pos_max[ind]-pos[ind]).T
if w_post == 0 and 0:
#Test that feet acceleration are satisfied
try:
assert (isapprox(feet_a_des, Jc * dv + dJcdq))
except AssertionError:
print(
"Solution violate the desired feet acceleration and so the CoM snap {}"
).format(np.linalg.norm(feet_a_des - (Jc * dv + dJcdq)))
#external forces should be zero
assert (isapprox((M * dv + h - Jc.T * fc)[:3], np.zeros([3, 1])))
return np.vstack([zero(3), tau])