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