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 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)
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 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)
dataPlot[k, 7] = q[6]
dataPlot[k, 8] = v[0]
dataPlot[k, 9] = v[1]
dataPlot[k, 10] = v[2]
dataPlot[k, 11] = v[3]
dataPlot[k, 12] = v[4]
dataPlot[k, 13] = v[5]
omega = v[3:6]
print("omega", omega)
angular_momentum = np.dot(ds.inertia(),omega)
am= SiconosVector(angular_momentum)
changeFrameBodyToAbs(q,am)
dataPlot[k, 14] = am.getValue(0)
dataPlot[k, 15] = am.getValue(1)
dataPlot[k, 16] = am.getValue(2)
dataPlot[k, 17] = am.norm2()
rotationVector = SiconosVector(3)
rotationVectorFromQuaternion(q[3],q[4],q[5],q[6], rotationVector)
dataPlot[k, 18] = rotationVector.getValue(0)
dataPlot[k, 19] = rotationVector.getValue(1)
dataPlot[k, 20] = rotationVector.getValue(2)
dataPlot[k, 22] = h* omega[0]
dataPlot[k, 23] = h* omega[1]
dataPlot[k, 24] = h* omega[2]
dataPlot[k, 25] = np.linalg.norm(h*omega)
