diff --git a/crocoddyl/contact.py b/crocoddyl/contact.py
index 5124348d2043b07b8593a68dd383ef4a4defbc1d..4e7f691ad223b9d0d3e11221675f5cb866081953 100644
--- a/crocoddyl/contact.py
+++ b/crocoddyl/contact.py
@@ -116,7 +116,7 @@ class ContactModel3D(ContactModelPinocchio):
if forcesVec is None:
forcesVec = data.forces
data.forces[data.joint] *= 0
- forcesVec[data.joint] += data.jMf*pinocchio.Force(-a2m(forcesArr), np.zeros((3,1)))
+ forcesVec[data.joint] += data.jMf*pinocchio.Force(a2m(forcesArr), np.zeros((3,1)))
return forcesVec
class ContactData3D(ContactDataPinocchio):
@@ -190,7 +190,7 @@ class ContactModel6D(ContactModelPinocchio):
if forcesVec is None:
forcesVec = data.forces
data.forces[data.joint] *= 0
- forcesVec[data.joint] += data.jMf*pinocchio.Force(-a2m(forcesArr))
+ forcesVec[data.joint] += data.jMf*pinocchio.Force(a2m(forcesArr))
return forcesVec
class ContactData6D(ContactDataPinocchio):
diff --git a/crocoddyl/floating_contact.py b/crocoddyl/floating_contact.py
index 43ce0edccbbd45ae2f66174a049ba56b01f5e5ac..465fb81696b8845e65415618ed4780f21e461930 100644
--- a/crocoddyl/floating_contact.py
+++ b/crocoddyl/floating_contact.py
@@ -37,11 +37,13 @@ class DifferentialActionModelFloatingInContact(DifferentialActionModelAbstract):
data.K[range(nv),range(nv)] += model.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(np.linalg.inv(data.K),data.r)
+ data.af[:] = np.dot(data.Kinv,data.r)
+ data.f[:] *= -1.
+
# Convert force array to vector of spatial forces.
fs = model.contact.setForces(data.contact,data.f)
@@ -61,8 +63,8 @@ class DifferentialActionModelFloatingInContact(DifferentialActionModelAbstract):
pinocchio.computeForwardKinematicsDerivatives(model.pinocchio,data.pinocchio,q,v,a)
pinocchio.updateFramePlacements(model.pinocchio,data.pinocchio)
- # [a;f] = K^-1 [ tau - b, -gamma ]
- # [a';f'] = -K^-1 [ K'a + b' ; J'a + gamma' ] = -K^-1 [ rnea'(q,v,a,fs) ; acc'(q,v,a) ]
+ # [a;-f] = K^-1 [ tau - b, -gamma ]
+ # [a';-f'] = -K^-1 [ K'a + b' ; J'a + gamma' ] = -K^-1 [ rnea'(q,v,a,fs) ; acc'(q,v,a) ]
# Derivative of the actuation model tau = tau(q,u)
# dtau_dq and dtau_dv are the rnea derivatives rnea'
@@ -79,14 +81,14 @@ class DifferentialActionModelFloatingInContact(DifferentialActionModelAbstract):
# We separate the Kinv into the a and f rows, and the actuation and acceleration columns
da_did = -data.Kinv[:nv,:nv]
- df_did = -data.Kinv[nv:,:nv]
+ df_did = data.Kinv[nv:,:nv]
da_dacc = -data.Kinv[:nv,nv:]
- df_dacc = -data.Kinv[nv:,nv:]
+ df_dacc = data.Kinv[nv:,nv:]
da_dq = np.dot(da_did,did_dq) + np.dot(da_dacc,dacc_dq)
da_dv = np.dot(da_did,did_dv) + np.dot(da_dacc,dacc_dv)
da_dtau = data.Kinv[:nv,:nv] # Add this alias just to make the code clearer
- df_dtau = data.Kinv[nv:,:nv] # Add this alias just to make the code clearer
+ df_dtau = -data.Kinv[nv:,:nv] # Add this alias just to make the code clearer
# tau is a function of x and u (typically trivial in x), whose derivatives are Ax and Au
dtau_dx = data.actuation.Ax
diff --git a/crocoddyl/impact.py b/crocoddyl/impact.py
index 36491bf1989034a263659985f820e13e8edcf932..5a85661214d4f17c4736e7d0fa4ebcda20a43129 100644
--- a/crocoddyl/impact.py
+++ b/crocoddyl/impact.py
@@ -73,7 +73,7 @@ class ImpulseModel6D(ImpulseModelPinocchio):
if forcesVec is None:
forcesVec = data.forces
data.forces[data.joint] *= 0
- forcesVec[data.joint] += data.jMf*pinocchio.Force(-a2m(forcesArr))
+ forcesVec[data.joint] += data.jMf*pinocchio.Force(a2m(forcesArr))
return forcesVec
class ImpulseData6D(ImpulseDataPinocchio):
@@ -114,7 +114,7 @@ class ImpulseModel3D(ImpulseModelPinocchio):
if forcesVec is None:
forcesVec = data.forces
data.forces[data.joint] *= 0
- forcesVec[data.joint] += data.jMf*pinocchio.Force(-a2m(forcesArr), np.zeros((3,1)))
+ forcesVec[data.joint] += data.jMf*pinocchio.Force(a2m(forcesArr), np.zeros((3,1)))
return forcesVec
class ImpulseData3D(ImpulseDataPinocchio):
@@ -328,14 +328,14 @@ class ActionModelImpact:
def createData(self): return ActionDataImpact(self)
def calc(model,data,x,u=None):
'''
- M(vnext-v) + J^T f = 0
+ M(vnext-v) - J^T f = 0
J vnext = 0
[MJ^T][vnext] = [Mv]
- [J ][ f ] [0 ]
+ [J ][ -f ] [0 ]
[vnext] = K^-1[Mv], with K = [MJ^T;J0]
- [ f ] [0 ]
+ [ -f ] [0 ]
'''
nx,nu,nq,nv,nout,nc = model.nx,model.nu,model.nq,model.nv,model.nout,model.nimpulse
q = a2m(x[:nq])
@@ -351,11 +351,12 @@ class ActionModelImpact:
data.K[range(nv),range(nv)] += model.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(inv(data.K),data.r)
+ data.af[:] = np.dot(data.Kinv,data.r)
+ data.f[:] *= -1.
# Convert force array to vector of spatial forces.
fs = model.impulse.setForces(data.impulse,data.f)
@@ -403,8 +404,6 @@ class ActionModelImpact:
model.impulse.calcDiff(data.impulse,x,recalc=False)
data.dv_dq = data.impulse.Vq
- data.Kinv = inv(data.K)
-
data.Fq[:nv,:] = 0
np.fill_diagonal(data.Fq[:nv,:],1) # dq/dq
data.Fv[:nv,:] = 0 # dq/dv
diff --git a/unittest/test_impact.py b/unittest/test_impact.py
index 8cc1422750d595532af0d19eb6b60bb67372251c..af90c68742efb53b063d09ba772d87c49b68bff3 100644
--- a/unittest/test_impact.py
+++ b/unittest/test_impact.py
@@ -80,7 +80,7 @@ rmodel.gravity = pinocchio.Motion.Zero()
pinocchio.computeRNEADerivatives(rmodel,rdata,q,zero(rmodel.nv),zero(rmodel.nv),data.impulse.forces)
d = rdata.dtau_dq.copy()
rmodel.gravity = g
-assert(norm(d-dn)<1e-5)
+assert(norm(d+dn)<1e-5)
### Check J.T f + M(vnext-v)
np.set_printoptions(precision=4, linewidth=200, suppress=True)
@@ -91,7 +91,7 @@ def MvJtf(q,vnext,v,f):
pinocchio.forwardKinematics(rmodel,rdata,q)
pinocchio.updateFramePlacements(rmodel,rdata)
J = pinocchio.getFrameJacobian(rmodel,rdata,CONTACTFRAME,pinocchio.ReferenceFrame.LOCAL)
- return M*(vnext-v)+J.T*f
+ return M*(vnext-v)-J.T*f
dn = df_dq(rmodel,lambda _q:MvJtf(_q,a2m(data.vnext),v,a2m(data.f)),q)
g = rmodel.gravity
@@ -171,7 +171,7 @@ dn = df_dq(rmodel,lambda _q:Jtf(_q,a2m(data.f)),q)
g = rmodel.gravity
rmodel.gravity = pinocchio.Motion.Zero()
pinocchio.computeRNEADerivatives(rmodel,rdata,q,zero(rmodel.nv),zero(rmodel.nv),data.impulse.forces)
-d = rdata.dtau_dq.copy()
+d = -rdata.dtau_dq.copy()
rmodel.gravity = g
assert(norm(d-dn)<1e-4)
@@ -184,7 +184,7 @@ def MvJtf(q,vnext,v,f):
pinocchio.forwardKinematics(rmodel,rdata,q)
pinocchio.updateFramePlacements(rmodel,rdata)
J = pinocchio.getFrameJacobian(rmodel,rdata,CONTACTFRAME,pinocchio.ReferenceFrame.LOCAL)
- return M*(vnext-v)+J.T*f
+ return M*(vnext-v)-J.T*f
dn = df_dq(rmodel,lambda _q:MvJtf(_q,a2m(data.vnext),v,a2m(data.f)),q)
g = rmodel.gravity
@@ -204,7 +204,7 @@ def Krk(q,vnext,v,f):
pinocchio.updateFramePlacements(rmodel,rdata)
J = pinocchio.getFrameJacobian(rmodel,rdata,CONTACTFRAME,pinocchio.ReferenceFrame.LOCAL)
- return np.vstack([ M*(vnext-v)+J.T*f, J*vnext ])
+ return np.vstack([ M*(vnext-v)-J.T*f, J*vnext ])
dn = df_dq(rmodel,lambda _q:Krk(_q,a2m(data.vnext),v,a2m(data.f)),q)
d = np.vstack([data.did_dq, data.dv_dq])