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:
J1=pin.getJointJacobian(self.rmodel,self.rdata,joint1,pin.ReferenceFrame.LOCAL)
J2=pin.getJointJacobian(self.rmodel,self.rdata,joint2,pin.ReferenceFrame.LOCAL)
Jc1=cMj1.action*J1
Jc2=cMj2.action*J2
Jc1=cMj1.action@J1
Jc2=cMj2.action@J2
J = (Jc1-Jc2)[2,:]
return J
......@@ -71,18 +71,21 @@ class CollisionWrapper:
a = (cMj1*a1-cMj2*a2).linear[2]
return a
def getCollisionJacobian(self,collisions):
def getCollisionJacobian(self,collisions=None):
'''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])
J = np.vstack([ self._getCollisionJacobian(c,r) for (i,c,r) in collisions ])
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([])
a0 = np.vstack([ self._getCollisionJdotQdot(c,r) for (i,c,r) in collisions ])
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([])
dist = np.array([ self.gdata.distanceResults[i].min_distance for (i,c,r) in collisions ])
return dist
......@@ -157,11 +160,6 @@ if __name__ == "__main__":
viz.display(q)
col = CollisionWrapper(robot,viz)
# for i in range(1000):
#col.computeCollisions(q)
#cols = col.getCollisionList()
#if len(cols)>0: break
col.initDisplay()
col.createDisplayPatchs(1)
col.computeCollisions(q)
......@@ -170,6 +168,8 @@ if __name__ == "__main__":
ci=cols[0][2]
col.displayContact(0,ci.getContact(0))
### Try to find a random contact
if 0:
q = robot.q0.copy()
vq = np.random.rand(robot.model.nv)*2-1
for i in range(10000):
......@@ -186,3 +186,14 @@ if __name__ == "__main__":
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()
'''
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
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. '''
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.meshColor = np.array(color)
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