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
def step(self):
self.count += 1
# Make a temporary BlockVector containing both qs
bv = BlockVector(self.ds1.q(), self.ds2.q())
force1 = np.zeros(3)
force2 = np.zeros(3)
# Apply an impulse to watch the response
if (self.count == 1000):
tmp = np.array(self.joint1.normalDoF(bv, 0)) * -1000
print('applying impulse', tmp)
force1 += np.array(tmp) / 2
force2 += -np.array(tmp) / 2
# Get the position and use it to project a force vector
# onto the DoF (spring force)
pos = SiconosVector(1)
self.joint1.computehDoF(0, bv, pos, 0)
setpoint = 1.0
pos_diff = setpoint - pos.getValue(0)
spring_force = np.array(self.joint1.normalDoF(bv,
0)) * pos_diff * 100.0
# Get the velocity of each body projected onto the DoF and
# calculate their difference (damping force)
vel1 = self.joint1.projectVectorDoF(self.ds1.linearVelocity(True), bv,
0)
vel2 = self.joint1.projectVectorDoF(self.ds2.linearVelocity(True), bv,
0)
vel_diff = vel1 - vel2
damping_force = vel_diff * 10.0
# Calculate total forces for each body
force1 += -(spring_force + damping_force) / 2
force2 += +(spring_force + damping_force) / 2
print('applying spring-damper forces', force1, force2)
self.ds1.setFExtPtr(force1)
self.ds2.setFExtPtr(force2)
def __init__(self,x, v):
I = np.zeros((3, 3))
I[0, 0] = 5.0
I[1, 1] = 10.0
I[2, 2] = 1.0
m=1.0
NewtonEulerDS.__init__(self,x, v, m, I)
# Allocation of _MExt
self.setMExtPtr(SiconosVector(3))
# specify that MExt is expressed in the inertial frame.
self.setIsMextExpressedInInertialFrame(True)
def __init__(self,x, v):
I = np.zeros((3, 3))
I[0, 0] = 5.0
I[1, 1] = 5.0
I[2, 2] = 1.0
m=1.0
NewtonEulerDS.__init__(self,x, v, m, I)
self._Mg=20
self._l=1.0
self.setComputeJacobianMIntqByFD(True)
# Allocation of _mInt
self._mInt = SiconosVector(3)
def step(self):
self.count += 1
# Make a temporary BlockVector containing both qs
bv = BlockVector(self.ds1.q(), self.ds2.q())
torque1 = np.zeros(3)
torque2 = np.zeros(3)
# Get the position and use it to project a torque vector
# onto the DoF (spring torque)
angle = SiconosVector(1)
self.joint1.computehDoF(0, bv, angle, 0)
setpoint = np.pi / 4
ang_diff = setpoint - angle.getValue(0)
spring_torque = np.array(self.joint1.normalDoF(bv,
0)) * ang_diff * 500.0
# Get the velocity of each body projected onto the DoF and
# calculate their difference (damping torque)
vel1 = self.joint1.projectVectorDoF(self.ds1.angularVelocity(True), bv,
0)
vel2 = self.joint1.projectVectorDoF(self.ds2.angularVelocity(True), bv,
0)
vel_diff = vel1 - vel2
damping_torque = vel_diff * 5.0
# Calculate total torques for each body
torque1 += -(spring_torque + damping_torque) / 2
torque2 += +(spring_torque + damping_torque) / 2
print('applying spring-damper torques', torque1, torque2)
self.ds1.setMExtPtr(torque1)
self.ds2.setMExtPtr(torque2)
def computeh(self, time, q, y):
print(q)
vec_q = SiconosVector(q)
height = vec_q[0] - self._ballRadius
y[0] = height
nnc = [1, 0, 0]
self.setnc(nnc)
ppc1 = [height, vec_q[1], vec_q[2]]
self.setpc1(ppc1)
ppc2 = [0.0, vec_q[1], vec_q[2]]
self.setpc2(ppc2)
def computeMInt(self, time, q, v, mInt=None):
if mInt is None :
mInt = self._mInt
if isinstance(mInt,SiconosVector):
r = self.centermass(q)
m = self._Mg*self._l*np.cross(r,[0,0,1.0])
mInt.setValue(0,m[0])
mInt.setValue(1,m[1])
mInt.setValue(2,m[2])
changeFrameAbsToBody(q,mInt)
#print("mInt========")
mInt.display()
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)
#!/usr/bin/env python
from siconos.kernel import SiconosVector
import sys
import os
from siconos.tests_setup import working_dir
v1 = SiconosVector([1.0, 2.0, 3.0])
v2 = SiconosVector()
def test_serialization1():
''' test xml IO'''
v2.xml_import(v1.xml_export())
assert (v2.getValue(0) == 1.0)
assert (v2.getValue(1) == 2.0)
assert (v2.getValue(2) == 3.0)
# question to Maurice: should we keep that or is it completely obsolete?
#def test_serialization2():
# ''' test text IO'''
#
# v2.text_import(v1.text_export())
#
# assert(v2.getValue(0) == 1.0)
# assert(v2.getValue(1) == 2.0)
# assert(v2.getValue(2) == 3.0)
#print("mInt========")
mInt.display()
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
