Commit e4b399d8 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

TP6 final version.

parent e53ac0ec
This diff is collapsed.
...@@ -46,8 +46,8 @@ class CollisionWrapper: ...@@ -46,8 +46,8 @@ class CollisionWrapper:
J1=pin.getJointJacobian(self.rmodel,self.rdata,joint1,pin.ReferenceFrame.LOCAL) J1=pin.getJointJacobian(self.rmodel,self.rdata,joint1,pin.ReferenceFrame.LOCAL)
J2=pin.getJointJacobian(self.rmodel,self.rdata,joint2,pin.ReferenceFrame.LOCAL) J2=pin.getJointJacobian(self.rmodel,self.rdata,joint2,pin.ReferenceFrame.LOCAL)
Jc1=cMj1.action*J1 Jc1=cMj1.action@J1
Jc2=cMj2.action*J2 Jc2=cMj2.action@J2
J = (Jc1-Jc2)[2,:] J = (Jc1-Jc2)[2,:]
return J return J
...@@ -71,18 +71,21 @@ class CollisionWrapper: ...@@ -71,18 +71,21 @@ class CollisionWrapper:
a = (cMj1*a1-cMj2*a2).linear[2] a = (cMj1*a1-cMj2*a2).linear[2]
return a return a
def getCollisionJacobian(self,collisions): def getCollisionJacobian(self,collisions=None):
'''From a collision list, return the Jacobian corresponding to the normal direction. ''' '''From a collision list, return the Jacobian corresponding to the normal direction. '''
if collisions is None: collisions = self.getCollisionList()
if len(collisions)==0: return np.ndarray([0,self.rmodel.nv]) if len(collisions)==0: return np.ndarray([0,self.rmodel.nv])
J = np.vstack([ self._getCollisionJacobian(c,r) for (i,c,r) in collisions ]) J = np.vstack([ self._getCollisionJacobian(c,r) for (i,c,r) in collisions ])
return J return J
def getCollisionJdotQdot(self,collisions): def getCollisionJdotQdot(self,collisions=None):
if collisions is None: collisions = self.getCollisionList()
if len(collisions)==0: return np.array([]) if len(collisions)==0: return np.array([])
a0 = np.vstack([ self._getCollisionJdotQdot(c,r) for (i,c,r) in collisions ]) a0 = np.vstack([ self._getCollisionJdotQdot(c,r) for (i,c,r) in collisions ])
return a0 return a0
def getCollisionDistances(self,collisions): def getCollisionDistances(self,collisions=None):
if collisions is None: collisions = self.getCollisionList()
if len(collisions)==0: return np.array([]) if len(collisions)==0: return np.array([])
dist = np.array([ self.gdata.distanceResults[i].min_distance for (i,c,r) in collisions ]) dist = np.array([ self.gdata.distanceResults[i].min_distance for (i,c,r) in collisions ])
return dist return dist
...@@ -157,11 +160,6 @@ if __name__ == "__main__": ...@@ -157,11 +160,6 @@ if __name__ == "__main__":
viz.display(q) viz.display(q)
col = CollisionWrapper(robot,viz) col = CollisionWrapper(robot,viz)
# for i in range(1000):
#col.computeCollisions(q)
#cols = col.getCollisionList()
#if len(cols)>0: break
col.initDisplay() col.initDisplay()
col.createDisplayPatchs(1) col.createDisplayPatchs(1)
col.computeCollisions(q) col.computeCollisions(q)
...@@ -170,19 +168,32 @@ if __name__ == "__main__": ...@@ -170,19 +168,32 @@ if __name__ == "__main__":
ci=cols[0][2] ci=cols[0][2]
col.displayContact(0,ci.getContact(0)) col.displayContact(0,ci.getContact(0))
q = robot.q0.copy() ### Try to find a random contact
vq = np.random.rand(robot.model.nv)*2-1 if 0:
for i in range(10000): q = robot.q0.copy()
q+=vq*1e-3 vq = np.random.rand(robot.model.nv)*2-1
col.computeCollisions(q) for i in range(10000):
cols = col.getCollisionList() q+=vq*1e-3
if len(cols)>0: break col.computeCollisions(q)
if not i % 20: viz.display(q) cols = col.getCollisionList()
if len(cols)>0: break
if not i % 20: viz.display(q)
viz.display(q)
viz.display(q) col.displayCollisions()
p = cols[0][1]
ci = cols[0][2].getContact(0)
print(robot.gmodel.geometryObjects[p.first].name,robot.gmodel.geometryObjects[p.second].name)
print(ci.pos)
import pickle
with open('/tmp/bug.pickle', 'wb') as file:
pickle.dump([ col.gdata.oMg[11],
col.gdata.oMg[3],
#col.gmodel.geometryObjects[11].geometry,
] , file)
dist=col.getCollisionDistances()
J = col.getCollisionJacobian()
col.displayCollisions()
p = cols[0][1]
ci = cols[0][2].getContact(0)
print(robot.gmodel.geometryObjects[p.first].name,robot.gmodel.geometryObjects[p.second].name)
print(ci.pos)
'''
This program presents a minimal example of using the QP solver quadprog.
sudo apt install robotpkg-py35-quadprog
'''
import quadprog
import numpy as np
from numpy.linalg import inv,pinv,norm,eig,svd
A = np.random.rand(5,5)*2-1
A = A @ A.T ### Make it positive symmetric
b = np.random.rand(5)
C = np.random.rand(10,5)
d = np.random.rand(10)
assert(np.all(eig(A)[0]>0))
[x,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d) # Notice that C.T is passed instead of C
assert( np.isclose(x@A@x/2-b@x,cost) )
assert( np.all( (C@x-d)>=-1e-6) ) # Check primal KKT condition
assert( np.all( np.isclose( (C@x-d)[iact-1],0 ) ) ) # Check primal complementarity
assert( np.all( np.isclose( (A@x-b -lag@C),0 ) ) ) # Check dual KKT condition
assert( np.all( np.isclose( lag[[ not i in iact-1 for i in range(len(lag)) ]],0 ) ) ) # Check dual complementarity
...@@ -9,7 +9,9 @@ import hppfcl ...@@ -9,7 +9,9 @@ import hppfcl
def Capsule(name,joint,radius,length,placement,color=[.7,.7,0.98,1]): def Capsule(name,joint,radius,length,placement,color=[.7,.7,0.98,1]):
'''Create a Pinocchio::FCL::Capsule to be added in the Geom-Model. ''' '''Create a Pinocchio::FCL::Capsule to be added in the Geom-Model. '''
hppgeom = hppfcl.Capsule(radius,length) ### They should be capsules ... but hppfcl current version is buggy with Capsules...
#hppgeom = hppfcl.Capsule(radius,length)
hppgeom = hppfcl.Cylinder(radius,length)
geom = pin.GeometryObject(name,joint,hppgeom,placement) geom = pin.GeometryObject(name,joint,hppgeom,placement)
geom.meshColor = np.array(color) geom.meshColor = np.array(color)
return geom return geom
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment