def run(): ok=True # cube_1, size 1, one corner at origin cubeMass_1 = mass.RigidMassInfo() cubeMass_1.mass = 1. cubeMass_1.com=[0.5,0.5,0.5] cubeMass_1.diagonal_inertia=[1./6.,1./6.,1./6.] cubeMass_1.density = 1.5 # cube_2, half cube, along x axis, positive side cubeMass_2 = mass.RigidMassInfo() cubeMass_2.mass = 0.5 cubeMass_2.com=[0.25,0.,0.] cubeMass_2.diagonal_inertia=(0.5/12.)*numpy.array([2.,1.25,1.25]) cubeMass_2.density = 1. # cube_3, half cube, along x axis, negative side cubeMass_3 = mass.RigidMassInfo() cubeMass_3.mass = 0.5 cubeMass_3.com=[-0.25,0.,0.] cubeMass_3.diagonal_inertia=cubeMass_2.diagonal_inertia cubeMass_3.density = 2. ok &= EXPECT_MAT_EQ([[2./3., -0.25, -0.25],[-0.25,2./3.,-0.25],[-0.25,-0.25,2./3.]], cubeMass_1.getWorldInertia(), "RigidMassInfo.getWorldInertia() cube 1") cubeMass_1_1 = cubeMass_1+cubeMass_1 ok &= EXPECT_FLOAT_EQ(2., cubeMass_1_1.mass, "RigidMassInfo.add cube_1+cube_1 - mass") ok &= EXPECT_FLOAT_EQ(cubeMass_1.density, cubeMass_1_1.density, "RigidMassInfo.add cube_1+cube_1 - density") ok &= EXPECT_VEC_EQ([0.5,0.5,0.5], cubeMass_1_1.com, "RigidMassInfo.add cube_1+cube_1 - com") ok &= EXPECT_MAT_EQ([1./3.,1./3.,1./3.], cubeMass_1_1.diagonal_inertia, "RigidMassInfo.add cube_1+cube_1 - diagonal_inertia" ) cubeMass_2_3 = cubeMass_2+cubeMass_3 ok &= EXPECT_FLOAT_EQ(1., cubeMass_2_3.mass, "RigidMassInfo.add cube_2+cube_3 - mass") ok &= EXPECT_FLOAT_EQ(1.+1./3., cubeMass_2_3.density, "RigidMassInfo.add cube_2+cube_3 - density") ok &= EXPECT_VEC_EQ([0.,0.,0.], cubeMass_2_3.com, "RigidMassInfo.add cube_2+cube_3 - com") ok &= EXPECT_MAT_EQ([1./6.,1./6.,1./6.], cubeMass_2_3.diagonal_inertia, "RigidMassInfo.add cube_2+cube_3 - diagonal_inertia" ) # modif cube 2 and 3 to be rotated around z axis qq = [ Quaternion.axisToQuat([0.,0.,1.],math.radians(30)), Quaternion.axisToQuat([0.,-2.,1.],math.radians(-60)), Quaternion.axisToQuat([-3.,2.,-1.],math.radians(160)) ] for q in qq: cubeMass_2.com=Quaternion.rotate(q, [0.25,0.,0.]) cubeMass_2.inertia_rotation=Quaternion.inv(q) cubeMass_3.com=Quaternion.rotate(q, [-0.25,0.,0.]) cubeMass_3.inertia_rotation=Quaternion.inv(q) cubeMass_2_3 = cubeMass_2+cubeMass_3 ok &= EXPECT_FLOAT_EQ(1., cubeMass_2_3.mass, "RigidMassInfo.add rotated cube_2+cube_3 - mass") ok &= EXPECT_VEC_EQ([0.,0.,0.], cubeMass_2_3.com, "RigidMassInfo.add rotated cube_2+cube_3 - com") ok &= EXPECT_MAT_EQ([1./6.,1./6.,1./6.], cubeMass_2_3.diagonal_inertia, "RigidMassInfo.add rotated cube_2+cube_3 - diagonal_inertia" ) return ok
def inv(x): res = np.zeros(7) res[3:] = quat.conj(x[3:]) res[:3] = -quat.rotate(res[3:], x[:3]) return res
def __mul__(self, other): res = Frame() res.translation = self.translation + quat.rotate( self.rotation, other.translation) res.rotation = quat.prod(self.rotation, other.rotation) return res
def prod(x, y): print x.inv() res = np.zeros(7) res[:3] = x[:3] + quat.rotate(x[3:], y[:3]) res[3:] = quat.prod(x[3:], y[3:]) return res
def onBeginAnimationStep(self, dt): # current mu from current plane angle currentMu = math.tan( self.currentAngle ) if self.counter < 100 : # does not rotate the plane for 100 time steps self.counter += 1 return 0 # is it a mu we want to test? if numpy.allclose( self.muToTest, currentMu, 1e-3, 1e-3 ) : # at the end of 100 time steps, check if the box was sticking or sliding self.counter = 0 self.muToTest += 0.1 # look at the box velocity along its x-axis localbox = Quaternion.rotate(Quaternion.conj( Frame.Frame( shared.plane.position[0] ).rotation ), shared.box.velocity[0][:3]) vel = localbox[0] #print 'plane/ground angle:', self.currentAngle #print 'velocity:',vel #print shared.box.position[0], shared.box.velocity[0][:3] #print vel, currentMu, shared.mu testVel = (vel > 1e-1) if testVel: testMu = (currentMu>=shared.mu-1e-2) else: testMu = (currentMu>=shared.mu) EXPECT_FALSE( testVel ^ testMu, str(vel)+' '+str(currentMu)+'mu='+str(shared.mu) ) # xor #print testVel, testMu #sys.stdout.flush() # all finished if currentMu >= shared.mu + .1: self.sendSuccess() # update plane orientation self.currentAngle += 0.001 q = Quaternion.from_euler( [0,0,-self.currentAngle] ) p = shared.plane.position p[0] = [0,0,0,q[3],q[2],q[1],q[0]] shared.plane.position = p return 0
def __mul__(self, other): res = Frame() res.translation = vec.sum(self.translation, quat.rotate(self.rotation, other.translation)) res.rotation = quat.prod( self.rotation, other.rotation) return res
def onBeginAnimationStep(self, dt): # current mu from current plane angle currentMu = math.tan(self.currentAngle) if self.counter < 100: # does not rotate the plane for 100 time steps self.counter += 1 return 0 # is it a mu we want to test? if numpy.allclose(self.muToTest, currentMu, 1e-3, 1e-3): # at the end of 100 time steps, check if the box was sticking or sliding self.counter = 0 self.muToTest += 0.1 # look at the box velocity along its x-axis localbox = Quaternion.rotate( Quaternion.conj( Frame.Frame(shared.plane.position[0]).rotation), shared.box.velocity[0][:3]) vel = localbox[0] #print 'plane/ground angle:', self.currentAngle #print 'velocity:',vel #print shared.box.position[0], shared.box.velocity[0][:3] # print vel, currentMu, shared.mu testVel = (vel > 1e-1) if testVel: testMu = (currentMu >= shared.mu - 1e-2) else: testMu = (currentMu >= shared.mu) EXPECT_FALSE(testVel ^ testMu, str(vel) + ' ' + str(currentMu) + 'mu=' + str(shared.mu)) # xor #print testVel, testMu #sys.stdout.flush() # all finished if currentMu >= shared.mu + .1: self.sendSuccess() # update plane orientation self.currentAngle += 0.001 q = Quaternion.from_euler([0, 0, -self.currentAngle]) p = shared.plane.position p[0] = [0, 0, 0, q[3], q[2], q[1], q[0]] shared.plane.position = p return 0
def apply(self, vec): """ apply transformation to vec (a [x,y,z] vector) return the result """ return array( quat.rotate(self.rotation, vec) + asarray(self.translation))
def inv(self): res = Frame() res.rotation = quat.conj(self.rotation) res.translation = -quat.rotate(res.rotation, self.translation) return res
def apply(self, vec): """ apply transformation to vec (a [x,y,z] vector) return the result """ return array(quat.rotate(self.rotation, vec) + asarray(self.translation))
def inv(self): res = Frame() res.rotation = quat.conj( self.rotation ) res.translation = - quat.rotate(res.rotation, self.translation) return res
def createScene(node): node.createObject('RequiredPlugin', pluginName = 'Compliant') ode = node.createObject('CompliantImplicitSolver') num = node.createObject('SequentialSolver') # ode.debug = 1 node.dt = 0.01 pos = np.zeros(7) vel = np.zeros(6) force = np.zeros(6) alpha = math.pi / 4.0 q = quat.exp([0, 0, alpha]) pos[:3] = [-0.5, 0, 0] pos[3:] = q mass = 1.0 # change this for more fun dim = np.array([1, 2, 1]) dim2 = dim * dim inertia = mass / 12.0 * (dim2[ [1, 2, 0] ] + dim2[ [2, 0, 1] ]) volume = 1.0 force[3:] = quat.rotate(q, [0, 1, 0]) scene = node.createChild('scene') good = scene.createChild('good') dofs = good.createObject('MechanicalObject', template = 'Rigid', name = 'dofs', position = pos, velocity = vel, showObject = 1) good.createObject('RigidMass', template = 'Rigid', name = 'mass', mass = mass, inertia = inertia) good.createObject('ConstantForceField', template = 'Rigid', name = 'ff', forces = force) bad = scene.createChild('bad') pos[:3] = [0.5, 0, 0] dofs = bad.createObject('MechanicalObject', template = 'Rigid', name = 'dofs', position = pos, velocity = vel, showObject = 1) inertia_matrix = np.diag(inertia) def cat(x): return ' '.join( map(str, x)) def print_matrix(x): return '[' + ','.join(map(str, x)) + ']' bad.createObject('UniformMass', template = 'Rigid', name = 'mass', mass = cat([mass, volume, print_matrix(inertia_matrix / mass)])) bad.createObject('ConstantForceField', template = 'Rigid', name = 'ff', forces = force) node.gravity = '0 0 0'