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 inv(x):
res = np.zeros(7)
res[3:] = quat.conj(x[3:])
res[:3] = -quat.rotate(res[3:], x[:3])
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 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'
