def centermass(self,q):
r= np.zeros(3)
E3 = SiconosVector(3)
E3.zero()
E3.setValue(2,1.0)
rotateAbsToBody(q,E3)
r[0] = E3.getValue(0)
r[1] = E3.getValue(1)
r[2] = E3.getValue(2)
return r
else:
r = self.centermass(q)
m = self._Mg*self._l*np.cross(r,[0,0,1.0])
m_sv = SiconosVector(m)
changeFrameAbsToBody(q,m_sv)
m_sv.display()
mInt[0] = m_sv.getValue(0)
mInt[1] = m_sv.getValue(1)
mInt[2] = m_sv.getValue(2)
print("mInt", mInt)
rotationVector_init= SiconosVector(3)
rotationVector_init.zero()
rotationVector_init.setValue(0,0.3)
x=SiconosVector(7)
quaternionFromRotationVector(rotationVector_init,x)
#x = [0, 0, 0, 1.0, 0, 0, 0] # initial configuration
v = [0, 0, 0, 0, 0, 50] # initial velocity
heavytop = HeavyTop(x, v)
ds = unstableRotation
#ds = heavytop
ds.display()
