Commit 5247163b authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

IVIGIT tp6 ... temporarily version shared for debug.

parent 7b5769d3
This diff is collapsed.
import pinocchio as pin
import numpy as np
class CollisionWrapper:
def __init__(self,robot,viz=None):
self.robot=robot
self.viz=viz
self.rmodel = robot.model
self.rdata = self.rmodel.createData()
self.gmodel = self.robot.gmodel
self.gdata = self.gmodel.createData()
self.gdata.collisionRequests.enable_contact = True
def computeCollisions(self,q,vq=None):
res = pin.computeCollisions(self.rmodel,self.rdata,self.gmodel,self.gdata,q,False)
pin.computeDistances(self.rmodel,self.rdata,self.gmodel,self.gdata,q)
pin.computeJointJacobians(self.rmodel,self.rdata,q)
if vq is not None:
pin.forwardKinematics(self.rmodel,self.rdata,q,vq,0*vq)
return res
def getCollisionList(self):
'''Return a list of triplets [ index,collision,result ] where index is the
index of the collision pair, colision is gmodel.collisionPairs[index]
and result is gdata.collisionResults[index].
'''
return [ [ir,self.gmodel.collisionPairs[ir],r]
for ir,r in enumerate(self.gdata.collisionResults) if r.isCollision() ]
def _getCollisionJacobian(self,col,res):
'''Compute the jacobian for one collision only. '''
contact = res.getContact(0)
g1 = self.gmodel.geometryObjects[col.first]
g2 = self.gmodel.geometryObjects[col.second]
oMc = pin.SE3(pin.Quaternion.FromTwoVectors(np.array([0,0,1]),contact.normal).matrix(),contact.pos)
joint1 = g1.parentJoint
joint2 = g2.parentJoint
oMj1 = self.rdata.oMi[joint1]
oMj2 = self.rdata.oMi[joint2]
cMj1 = oMc.inverse()*oMj1
cMj2 = oMc.inverse()*oMj2
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
J = (Jc1-Jc2)[2,:]
return J
def _getCollisionJdotQdot(self,col,res):
'''Compute the Coriolis acceleration for one collision only. '''
contact = res.getContact(0)
g1 = self.gmodel.geometryObjects[col.first]
g2 = self.gmodel.geometryObjects[col.second]
oMc = pin.SE3(pin.Quaternion.FromTwoVectors(np.array([0,0,1]),contact.normal).matrix(),contact.pos)
joint1 = g1.parentJoint
joint2 = g2.parentJoint
oMj1 = self.rdata.oMi[joint1]
oMj2 = self.rdata.oMi[joint2]
cMj1 = oMc.inverse()*oMj1
cMj2 = oMc.inverse()*oMj2
a1 = self.rdata.a[joint1]
a2 = self.rdata.a[joint2]
a = (cMj1*a1-cMj2*a2).linear[2]
return a
def getCollisionJacobian(self,collisions):
'''From a collision list, return the Jacobian corresponding to the normal direction. '''
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):
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):
if len(collisions)==0: return np.array([])
dist = np.array([ self.gdata.distanceResults[i].min_distance for (i,c,r) in collisions ])
return dist
# --- DISPLAY -----------------------------------------------------------------------------------
# --- DISPLAY -----------------------------------------------------------------------------------
# --- DISPLAY -----------------------------------------------------------------------------------
def initDisplay(self,viz=None):
if viz is not None: self.viz = viz
assert(self.viz is not None)
self.patchName = 'world/contact_%d_%s'
self.ncollisions=10
self.createDisplayPatchs(0)
def createDisplayPatchs(self,ncollisions):
if ncollisions == self.ncollisions: return
elif ncollisions<self.ncollisions: # Remove patches
for i in range(ncollisions,self.ncollisions):
viz[self.patchName % (i,'a')].delete()
viz[self.patchName % (i,'b')].delete()
else:
for i in range(self.ncollisions,ncollisions):
viz.addCylinder( self.patchName % (i,'a') , .0005,.005,"red")
#viz.addCylinder( self.patchName % (i,'b') , .0005,.05,"red")
self.ncollisions = ncollisions
def displayContact(self,ipatch,contact):
'''
Display a small red disk at the position of the contact, perpendicular to the
contact normal.
@param ipatchf: use patch named "world/contact_%d" % contactRef.
@param contact: the contact object, taken from Pinocchio (HPP-FCL) e.g.
geomModel.collisionResults[0].geContact(0).
'''
name = self.patchName % (ipatch, 'a')
R = pin.Quaternion.FromTwoVectors(np.array([0,1,0]),contact.normal).matrix()
M = pin.SE3(R,contact.pos)
self.viz.applyConfiguration(name,M)
def displayCollisions(self,collisions = None):
'''Display in the viewer the collision list get from getCollisionList().'''
if self.viz is None: return
if collisions is None: collisions = self.getCollisionList()
self.createDisplayPatchs(len(collisions))
for ic,[i,c,r] in enumerate(collisions):
self.displayContact(ic,r.getContact(0))
if __name__ == "__main__":
from tp6.meshcat_viewer_wrapper import MeshcatVisualizer
from tp6.robot_hand import RobotHand
import time
import numpy as np
from numpy import cos,sin
robot = RobotHand()
viz = MeshcatVisualizer(robot,url='tcp://127.0.0.1:6000')#classical')
q = robot.q0.copy()
q[0]=.5
q[2:4]=1.7648
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)
cols = col.getCollisionList()
ci=cols[0][2]
col.displayContact(0,ci.getContact(0))
q = robot.q0.copy()
vq = np.random.rand(robot.model.nv)*2-1
for i in range(10000):
q+=vq*1e-3
col.computeCollisions(q)
cols = col.getCollisionList()
if len(cols)>0: break
if not i % 20: 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)
../tp2/meshcat_viewer_wrapper/
\ No newline at end of file
from pinocchio.explog import exp,log
from numpy.linalg import pinv,norm,pinv
from pinocchio.utils import rotate
import pinocchio as pin
import numpy as np
from numpy import pi
from numpy import cos,sin,pi,hstack,vstack,argmin
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)
geom = pin.GeometryObject(name,joint,hppgeom,placement)
geom.meshColor = np.array(color)
return geom
def Sphere(name,joint,radius,placement,color=[.9,.9,0.98,1]):
'''Create a Pinocchio::FCL::Capsule to be added in the Geom-Model. '''
hppgeom = hppfcl.Sphere(radius)
geom = pin.GeometryObject(name,joint,hppgeom,placement)
geom.meshColor = np.array(color)
return geom
class RobotHand:
'''
Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
The configuration is nq=7. The velocity is the same.
The members of the class are:
* viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
* model: the kinematic tree of the robot.
* data: the temporary variables to be used by the kinematic algorithms.
* visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
an object Visual (see above).
CollisionPairs is a list of visual indexes.
Reference to the collision pair is used in the collision test and jacobian of the collision
(which are simply proxy method to methods of the visual class).
'''
def __init__(self):
self.viewer = None
#self.visuals = []
self.model = pin.Model()
self.gmodel = pin.GeometryModel()
self.createHand()
self.addCollisionPairs()
self.data = self.model.createData()
self.gdata = pin.GeometryData(self.gmodel)
self.gdata.collisionRequests.enable_contact=True
self.q0 = np.zeros(self.model.nq)
#self.q0[0] = np.pi
self.q0[-2] = np.pi/3
self.q0[2:-4] = np.pi/6
self.q0[11:] = np.pi/4
self.v0 = np.zeros(self.model.nv)
self.collisionPairs = []
self.collision_model = self.gmodel
self.collision_data = self.gmodel.createData()
self.visual_model = self.gmodel
self.visual_data = self.gmodel.createData()
def addCollisionPairs(self):
pairs = [
[ 'finger12', 'wrist' ],
[ 'finger12', 'palm_left' ],
[ 'finger12', 'palm_right' ],
[ 'finger12', 'palm_front' ],
[ 'finger13', 'wrist' ],
[ 'finger13', 'palm_left' ],
[ 'finger13', 'palm_right' ],
[ 'finger13', 'palm_front' ],
[ 'finger13', 'palm2' ],
[ 'finger22', 'wrist' ],
[ 'finger22', 'palm_left' ],
[ 'finger22', 'palm_right' ],
[ 'finger22', 'palm_front' ],
[ 'finger23', 'wrist' ],
[ 'finger23', 'palm_left' ],
[ 'finger23', 'palm_right' ],
[ 'finger23', 'palm_front' ],
[ 'finger23', 'palm2' ],
[ 'finger32', 'wrist' ],
[ 'finger32', 'palm_left' ],
[ 'finger32', 'palm_right' ],
[ 'finger32', 'palm_front' ],
[ 'finger33', 'wrist' ],
[ 'finger33', 'palm_left' ],
[ 'finger33', 'palm_right' ],
[ 'finger33', 'palm_front' ],
[ 'finger33', 'palm2' ],
[ 'thumb1', 'wrist' ],
[ 'thumb1', 'palm_left' ],
[ 'thumb1', 'palm_front' ],
[ 'thumb1','palm2' ],
[ 'thumb1','finger11'],
[ 'thumb1','finger12'],
[ 'thumb1','finger13'],
[ 'thumb2', 'wrist' ],
[ 'thumb2', 'palm_left' ],
[ 'thumb2', 'palm_right' ],
[ 'thumb2', 'palm_front' ],
[ 'thumb2', 'palm2' ],
[ 'thumb2','finger11'],
[ 'thumb2','finger12'],
[ 'thumb2','finger13'],
[ 'thumb2','finger21'],
[ 'thumb2','finger22'],
[ 'thumb2','finger23'],
]
for (n1,n2) in pairs:
self.gmodel.addCollisionPair(pin.CollisionPair(self.gmodel.getGeometryId('world/'+n1),
self.gmodel.getGeometryId('world/'+n2)))
def addCapsule(self,name,joint,placement,radius,length,color=[1,1,0.78,1]):
caps = Capsule(name,joint,radius*0.99,length,placement)
caps.meshColor = np.array([1.]*4)
self.gmodel.addGeometryObject(caps)
#self.gmodel.geometryObjects[-1].meshColor = np.array([1.]*4)
def createHand(self,rootId=0,jointPlacement=None):
color = [red,green,blue,transparency] = [1,1,0.78,1.0]
colorred = [1.0,0.0,0.0,1.0]
jointId = rootId
cm = 1e-2
trans = lambda x,y,z: pin.SE3(np.eye(3),np.array([x,y,z]))
inertia = lambda m,c: pin.Inertia(m,np.array(c,np.double),np.eye(3)*m**2)
name = "wrist"
jointName,bodyName = [name+"_joint",name+"_body"]
#jointPlacement = jointPlacement if jointPlacement!=None else pin.SE3.Identity()
jointPlacement = jointPlacement if jointPlacement!=None else pin.SE3(pin.utils.rotate('y',np.pi),np.zeros(3))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(3,[0,0,0]),pin.SE3.Identity())
## Hand dimsensions: length, width, height(=depth), finger-length
L=3*cm;W=5*cm;H=1*cm; FL = 4*cm
self.gmodel.addGeometryObject( Sphere('world/wrist',jointId,.02,
pin.SE3(rotate('x',pi/2),np.array([0,0,0]))) )
self.gmodel.addGeometryObject( Capsule('world/palm_right',jointId,H,L,
pin.SE3(rotate('z',-.3)@rotate('y',pi/2),np.array([L/2,-W/2.6,0])) ) )
self.gmodel.addGeometryObject( Capsule('world/palm_left',jointId,H,L,
pin.SE3(rotate('y',pi/2),np.array([L/2,W/2,0]))) )
self.gmodel.addGeometryObject( Capsule('world/palm_front',jointId,H,W,
pin.SE3(rotate('x',pi/2),np.array([L,0,0]))) )
self.gmodel.addGeometryObject( Sphere('world/palm_frontleft',jointId,H,
pin.SE3(rotate('x',pi/2),np.array([L,W/2,0])),
color=[.7,.7,0.98,1]) )
self.gmodel.addGeometryObject( Sphere('world/palm_frontright',jointId,H,
pin.SE3(rotate('x',pi/2),np.array([L,-W/2,0])),
color=[.7,.7,0.98,1]) )
name = "palm"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([5*cm,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(2,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/palm2',jointId,1*cm,W,
pin.SE3(rotate('x',pi/2),np.zeros(3))) )
palmIdx = jointId
name = "finger11"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([2*cm,W/2,0]))
jointId = self.model.addJoint(palmIdx,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/finger11',jointId,H,FL-0*H,
pin.SE3(rotate('y',pi/2),np.array([FL/2-H,0,0])) ))
name = "finger12"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([FL,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/finger12',jointId,H,FL-0*H,
pin.SE3(rotate('y',pi/2),np.array([FL/2-H,0,0])) ))
name = "finger13"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([FL-H,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Sphere('world/finger13',jointId,H,
trans(H,0,0) ))
name = "finger21"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([2*cm,0,0]))
jointId = self.model.addJoint(palmIdx,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/finger21',jointId,H,FL-0*H,
pin.SE3(rotate('y',pi/2),np.array([FL/2-H,0,0])) ))
name = "finger22"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([FL,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/finger22',jointId,H,FL-0*H,
pin.SE3(rotate('y',pi/2),np.array([FL/2-H,0,0])) ))
name = "finger23"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([FL-H,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Sphere('world/finger23',jointId,H,
trans(H,0,0) ))
name = "finger31"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([2*cm,-W/2,0]))
jointId = self.model.addJoint(palmIdx,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/finger31',jointId,H,FL-0*H,
pin.SE3(rotate('y',pi/2),np.array([FL/2-H,0,0]))))
name = "finger32"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([FL,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/finger32',jointId,H,FL-0*H,
pin.SE3(rotate('y',pi/2),np.array([FL/2-H,0,0]))))
name = "finger33"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.array([FL-H,0,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.3,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Sphere('world/finger33',jointId,H,
trans(H,0,0)))
name = "thumb1"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(rotate('z',-1.5), np.array([1*cm,-W/2-H*1.3,0]))
jointId = self.model.addJoint(1,pin.JointModelRY(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
# self.gmodel.addGeometryObject( Capsule('world/thumb1_mid',jointId,H,2*cm,
# pin.SE3(rotate('z',pi/3)@rotate('x',pi/2),np.array([1*cm,-1*cm,0])) ))
name = "thumb1a"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(np.eye(3), np.zeros(3))
jointId = self.model.addJoint(jointId,pin.JointModelRX(),jointPlacement,jointName)
# self.model.appendBodyToJoint(jointId,inertia(.5,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/thumb1',jointId,H,2*cm,
pin.SE3(rotate('z',pi/3)@rotate('x',pi/2),np.array([0.3*cm,-1.0*cm,0]))))
name = "thumb2"
jointName,bodyName = [name+"_joint",name+"_body"]
jointPlacement = pin.SE3(rotate('z',pi/3)@rotate('x',pi), np.array([3.4*cm,-1.99*cm,0]))
jointId = self.model.addJoint(jointId,pin.JointModelRZ(),jointPlacement,jointName)
self.model.appendBodyToJoint(jointId,inertia(.4,[0,0,0]),pin.SE3.Identity())
self.gmodel.addGeometryObject( Capsule('world/thumb2',jointId,H,FL-0*H,
pin.SE3(rotate('x',2*pi/3),np.array([-0.007*cm,.008*cm,-0.5*cm]))) )
# Prepare some patches to represent collision points. Yet unvisible.
if self.viewer is not None:
self.maxContact = 10
for i in range(self.maxContact):
self.viewer.addCylinder('world/cpatch%d'%i, .01, .003, [ 1.0,0,0,1])
self.viewer.setVisibility('world/cpatch%d'%i,'OFF')
def hideContact(self,fromContactRef=0):
if fromContactRef<0: fromContactRef=self.maxContact+fromContactRef
for i in range(fromContactRef,self.maxContact):
name='world/cpatch%d'%i
self.viewer.setVisibility(name,'OFF')
def displayContact(self,contact,contactRef=0,refresh=False):
'''
Display a small red disk at the position of the contact, perpendicular to the
contact normal.
@param contact: the contact object, taken from Pinocchio (HPP-FCL) e.g.
geomModel.collisionResults[0].geContact(0).
@param contactRef: use patch named "world/cparch%d" % contactRef, 0 by default.
@param refresh: option to refresh the viewer before returning, False by default.
'''
name='world/cpatch%d'%contactRef
if self.viewer is None: return
self.viewer.setVisibility(name,'ON')
M = pin.SE3(pin.Quaternion.FromTwoVectors(np.array([0,0,1]),contact.normal).matrix(),contact.pos)
self.viewer.applyConfiguration(name,pin.se3ToXYZQUATtuple(M))
if refresh: self.viewer.refresh()
def display(self,q):
pin.forwardKinematics(self.model,self.data,q)
pin.updateGeometryPlacements(self.model,self.data,self.gmodel,self.gdata)
if self.viewer is None: return
for i,g in enumerate(self.gmodel.geometryObjects):
self.viewer.applyConfiguration(g.name, pin.se3ToXYZQUATtuple(self.gdata.oMg[i]))
self.viewer.refresh()
if __name__ == "__main__":
from tp6.meshcat_viewer_wrapper import MeshcatVisualizer
import time
import numpy as np
from numpy import cos,sin
robot = RobotHand()
viz = MeshcatVisualizer(robot,url='tcp://127.0.0.1:6002')#classical')
viz.display(robot.q0)
q = robot.q0.copy()
T = 1000
for t in range(T):
x = sin(t/30)
q[12:13] = x+.5
viz.display(q)
time.sleep(5e-2)
import pinocchio as pin
import numpy as np
import matplotlib.pylab as plt; plt.ion()
from numpy.linalg import inv, pinv, norm
import time
from tp6.robot_hand import RobotHand
from tp6.meshcat_viewer_wrapper import MeshcatVisualizer
robot = RobotHand()
viz = MeshcatVisualizer(robot,url='tcp://127.0.0.1:6002')#classical')
q = robot.q0.copy()
vq = np.zeros(robot.model.nv)
Kp = 50.
Kv = 2 * np.sqrt(Kp)
dt = 1e-3
from tp6.traj_ref import TrajRef
qdes = TrajRef(robot.q0,omega = np.array([0,.1,1,1.5,2.5,-1,-1.5,-2.5,.1,.2,.3,.4,.5,.6]),amplitude=1.5)
hq = [] ### For storing the logs of measured trajectory q
hqdes = [] ### For storing the logs of desired trajectory qdes
for i in range(10000):
t = i*dt
M = pin.crba(robot.model, robot.data, q)
b = pin.rnea(robot.model, robot.data, q, vq, np.zeros(robot.model.nv))
tauq = -Kp*(q-qdes(t)) - Kv*(vq-qdes.velocity(t)) + qdes.acceleration(t)
aq = inv(M) @ (tauq - b)
vq += aq * dt
q = pin.integrate(robot.model, q, vq * dt)
TDISP = 50e-3 # Display every 50ms
if not i % int(TDISP/dt): # Only display once in a while ...
viz.display(q)
time.sleep(TDISP)
hq.append(q.copy())
hqdes.append(qdes.copy())
import numpy as np
class TrajRef:
def __init__(self, q0, omega, amplitude):
self.q0 = q0.copy()
self.omega = omega