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 run():
ok=True
qi= Quaternion.id()
ok &= EXPECT_VEC_EQ([0,0,0,1],qi)
q1=[0,0.5,0,1]
ok &= test_inv(q1)
return ok
def __init__(self,
node,
filepath,
scale3d,
offset,
name_suffix='',
generatedDir=None):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.node = node.createChild('collision' + name_suffix) # node
self.loader = SofaPython.Tools.meshLoader(self.node,
filename=filepath,
name='loader',
scale3d=concat(scale3d),
translation=concat(
offset[:3]),
rotation=concat(r),
triangulate=True)
self.topology = self.node.createObject('MeshTopology',
name='topology',
src='@loader')
self.dofs = self.node.createObject('MechanicalObject',
name='dofs',
template='Vec3' +
template_suffix)
self.triangles = self.node.createObject('TriangleModel',
name='model')
if generatedDir is None:
self.mapping = self.node.createObject('LinearMapping',
template='Affine,Vec3' +
template_suffix,
name='mapping')
else:
serialization.importLinearMapping(
self.node, generatedDir + "_collisionmapping.json")
self.normals = None
def getImageTransform(filename, scaleFactor=1):
""" Returns dim, voxelsize and rigid position of an image given an .mhd header image file
a scaleFactor can be given to normalize image length units (usually in mm)
"""
scale=[0,0,0]
tr=[0,0,0]
dim=[0,0,0]
with open(filename,'r') as f:
for line in f:
splitted = line.split()
if len(splitted)!=0:
if 'ElementSpacing'==splitted[0] or 'spacing'==splitted[0] or 'scale3d'==splitted[0] or 'voxelSize'==splitted[0]:
scale = map(float,splitted[2:5])
if 'Position'==splitted[0] or 'Offset'==splitted[0] or 'translation'==splitted[0] or 'origin'==splitted[0]:
tr = map(float,splitted[2:5])
if 'Orientation'==splitted[0] or 'Rotation'==splitted[0] or 'TransformMatrix'==splitted[0] :
R = numpy.array([map(float,splitted[2:5]),map(float,splitted[5:8]),map(float,splitted[8:11])])
if 'DimSize'==splitted[0] or 'dimensions'==splitted[0] or 'dim'==splitted[0]:
dim = map(int,splitted[2:5])
q = quat.from_matrix(R)
if scaleFactor!=1:
scale = [s*scaleFactor for s in scale]
tr = [t*scaleFactor for t in tr]
offset=[tr[0],tr[1],tr[2],q[0],q[1],q[2],q[3]]
return (dim,scale,offset)
def setup(self, parentNode, density=2000, param=None, generatedDir=None):
# Computation the offset according to the attribute self.transform
offset = [0, 0, 0, 0, 0, 0, 1]
offset[:3] = self.transform[:3]
offset[3:] = Quaternion.from_euler(self.transform[3:6])
scale3d = self.transform[6:]
if self.type not in BoneType:
self.type = "short" # --> to set 1 frame on bone which not have a type
# Creation of the shearless affine body
self.body = RigidScale.API.ShearlessAffineBody(parentNode, self.name)
# Depending on the frame set in the constructor, let decide how the body will be initialized
if (self.frame
== None) or (len(self.frame) < FramePerBoneType[self.type]):
self.body.setFromMesh(self.filepath, density, offset, scale3d,
self.voxelSize, FramePerBoneType[self.type])
else:
scale3dList = []
for i in range(len(self.frame)):
scale3dList.append(scale3d)
self.body.setManually(self.filepath,
self.frame,
self.voxelSize,
density=1000,
generatedDir=generatedDir)
# Add of the behavior model, the collision model and the visual model
localGeneratedDir = None if generatedDir is None else generatedDir + self.name
self.behavior = self.body.addBehavior(
self.elasticity,
IntegrationPointPerBoneType[self.type],
generatedDir=localGeneratedDir)
self.collision = self.body.addCollisionMesh(
self.filepath, scale3d, offset, generatedDir=localGeneratedDir)
self.visual = self.collision.addVisualModel()
return self.body
def transformToData(scale, offset, timeOffset=0, timeScale=1, isPerspective=0):
""" Returns a transform, formatted to sofa data given voxelsize, rigid position (offset), time and camera parameters
"""
return concat(offset[:3]) + ' ' + concat(
quat.to_euler(offset[3:]) * 180. / math.pi) + ' ' + concat(
scale) + ' ' + str(timeOffset) + ' ' + str(timeScale) + ' ' + str(
int(isPerspective))
def getImageTransform(filename, scaleFactor=1):
""" Returns dim, voxelsize and rigid position of an image given an .mhd header image file
a scaleFactor can be given to normalize image length units (usually in mm)
"""
scale=[0,0,0]
tr=[0,0,0]
dim=[0,0,0]
with open(filename,'r') as f:
for line in f:
splitted = line.split()
if len(splitted)!=0:
if 'ElementSpacing'==splitted[0] or 'spacing'==splitted[0] or 'scale3d'==splitted[0] or 'voxelSize'==splitted[0]:
scale = map(float,splitted[2:5])
if 'Position'==splitted[0] or 'Offset'==splitted[0] or 'translation'==splitted[0] or 'origin'==splitted[0]:
tr = map(float,splitted[2:5])
if 'Orientation'==splitted[0] or 'Rotation'==splitted[0] or 'TransformMatrix'==splitted[0] :
R = numpy.array([map(float,splitted[2:5]),map(float,splitted[5:8]),map(float,splitted[8:11])])
if 'DimSize'==splitted[0] or 'dimensions'==splitted[0] or 'dim'==splitted[0]:
dim = map(int,splitted[2:5])
q = quat.from_matrix(R)
if scaleFactor!=1:
scale = [s*scaleFactor for s in scale]
tr = [t*scaleFactor for t in tr]
offset=[tr[0],tr[1],tr[2],q[0],q[1],q[2],q[3]]
return (dim,scale,offset)
def setup(self, parentNode, density=2000, param=None, generatedDir = None ):
# Computation the offset according to the attribute self.transform
offset = [0,0,0,0,0,0,1]
offset[:3] = self.transform[:3]
offset[3:] = Quaternion.from_euler(self.transform[3:6])
scale3d = self.transform[6:]
if self.type not in BoneType:
self.type = "short" # --> to set 1 frame on bone which not have a type
# Creation of the shearless affine body
self.body = RigidScale.API.ShearlessAffineBody(parentNode, self.name)
# Depending on the frame set in the constructor, let decide how the body will be initialized
if (self.frame == None) or (len(self.frame) < FramePerBoneType[self.type]):
self.body.setFromMesh(self.filepath, density, offset, scale3d, self.voxelSize, FramePerBoneType[self.type], generatedDir=generatedDir)
for p in self.body.frame: self.frame.append(p.offset())
else:
scale3dList = []
for i in range(len(self.frame)):
scale3dList.append(scale3d)
self.body.setManually(self.filepath, self.frame, self.voxelSize, density=1000, generatedDir=generatedDir)
# Add of the behavior model, the collision model and the visual model
localGeneratedDir=None if generatedDir is None else generatedDir+self.name
self.behavior = self.body.addBehavior(self.elasticity, IntegrationPointPerBoneType[self.type], generatedDir=localGeneratedDir )
self.collision = self.body.addCollisionMesh(self.filepath, scale3d, offset, generatedDir=localGeneratedDir)
self.visual = self.collision.addVisualModel()
return self.body
def __init__(self,
node,
filepath,
scale3d,
offset,
name_suffix='',
generatedDir=None):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
global idxVisualModel
self.node = node.createChild('visual' + name_suffix) # node
self.model = self.node.createObject('VisualModel',
name='visual' +
str(idxVisualModel),
fileMesh=filepath,
scale3d=concat(scale3d),
translation=concat(offset[:3]),
rotation=concat(r))
if generatedDir is None:
self.mapping = self.node.createObject(
'LinearMapping',
template='Affine,ExtVec3f',
name='mapping')
else:
serialization.importLinearMapping(
self.node, generatedDir + "_visualmapping.json")
idxVisualModel += 1
def run():
ok = True
qi = Quaternion.id()
ok &= EXPECT_VEC_EQ([0, 0, 0, 1], qi)
q1 = [0, 0.5, 0, 1]
ok &= test_inv(q1)
return ok
def __init__(self, node, filepath, scale3d, offset):
self.node = node.createChild( "collision" ) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.loader = self.node.createObject("MeshObjLoader", name='loader', filename=filepath, scale3d=concat(scale3d), translation=concat(offset[:3]), rotation=concat(r), triangulate=1 )
self.topology = self.node.createObject('MeshTopology', name='topology', src="@loader" )
self.dofs = self.node.createObject('MechanicalObject', name='dofs')
self.triangles = self.node.createObject('TriangleModel', template='Vec3d', name='model')
self.mapping = self.node.createObject('RigidMapping', name="mapping")
def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
global idxVisualModel
self.node = node.createChild( "visual"+name_suffix ) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel', name="visual"+str(idxVisualModel), fileMesh=filepath,
scale3d=concat(scale3d), translation=concat(offset[:3]) , rotation=concat(r),
useNormals=False, updateNormals=True)
self.mapping = self.node.createObject('RigidMapping', name="mapping")
idxVisualModel+=1
def __init__(self, node, filepath, scale3d, offset, name_suffix='', generatedDir=None):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
global idxVisualModel;
self.node = node.createChild('visual'+name_suffix) # node
self.model = self.node.createObject('VisualModel', name='visual'+str(idxVisualModel), fileMesh=filepath, scale3d=concat(scale3d), translation=concat(offset[:3]), rotation=concat(r))
if generatedDir is None:
self.mapping = self.node.createObject('LinearMapping', template='Affine,ExtVec3f', name='mapping')
else:
serialization.importLinearMapping(self.node, generatedDir+"_visualmapping.json")
idxVisualModel+=1
def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
self.node = node.createChild( "collision"+name_suffix ) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.loader = SofaPython.Tools.meshLoader(self.node, filename=filepath, name='loader', scale3d=concat(scale3d), translation=concat(offset[:3]) , rotation=concat(r), triangulate=True)
self.topology = self.node.createObject('MeshTopology', name='topology', src="@loader" )
self.dofs = self.node.createObject('MechanicalObject', name='dofs', template="Vec3"+template_suffix )
self.triangles = self.node.createObject('TriangleModel', name='model')
self.mapping = self.node.createObject('RigidMapping', name="mapping")
self.normals = None
self.visual = None
def update(self):
value = rigid.id()
value[3:] = quat.exp(self.dofs.position[0][0] * self.axis)
self.value = value
jacobian = np.zeros(self.size)
jacobian[3:, :] = self.axis.reshape((3, 1))
self.jacobian = jacobian
def getWorldInertia(self):
""" @return inertia with respect to world reference frame
"""
R = Quaternion.to_matrix(self.inertia_rotation)
# I in world axis
I = numpy.dot(R.transpose(), numpy.dot(numpy.diag(self.diagonal_inertia), R))
# I at world origin, using // axis theorem
# see http://www.colorado.edu/physics/phys3210/phys3210_sp14/lecnotes.2014-03-07.More_on_Inertia_Tensors.html
# or https://en.wikipedia.org/wiki/Moment_of_inertia
a=numpy.array(self.com).reshape(3,1)
return I + self.mass*(pow(numpy.linalg.norm(self.com),2)*numpy.eye(3) - a*a.transpose())
def getWorldInertia(self):
""" @return inertia with respect to world reference frame
"""
R = Quaternion.to_matrix(self.inertia_rotation)
# I in world axis
I = numpy.dot(R.transpose(), numpy.dot(numpy.diag(self.diagonal_inertia), R))
# I at world origin, using // axis theorem
# see http://www.colorado.edu/physics/phys3210/phys3210_sp14/lecnotes.2014-03-07.More_on_Inertia_Tensors.html
# or https://en.wikipedia.org/wiki/Moment_of_inertia
a=numpy.array(self.com).reshape(3,1)
return I + self.mass*(pow(numpy.linalg.norm(self.com),2)*numpy.eye(3) - a*a.transpose())
def update(self):
value = rigid.id()
value[3:] = quat.exp( self.dofs.position[0][0] * self.axis )
self.value = value
jacobian = np.zeros( self.size )
jacobian[3:, :] = self.axis.reshape( (3, 1) )
self.jacobian = jacobian
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 decomposeInertia(inertia):
""" Decompose an inertia matrix into
- a diagonal inertia
- the rotation (quaternion) to get to the frame in wich the inertia is diagonal
"""
U, diagonal_inertia, V = numpy.linalg.svd(inertia)
# det should be 1->rotation or -1->reflexion
if numpy.linalg.det(U) < 0 : # reflexion
# made it a rotation by negating a column
U[:,0] = -U[:,0]
inertia_rotation = Quaternion.from_matrix( U )
return diagonal_inertia, inertia_rotation
def decomposeInertia(inertia):
""" Decompose an inertia matrix into
- a diagonal inertia
- the rotation (quaternion) to get to the frame in wich the inertia is diagonal
"""
U, diagonal_inertia, V = numpy.linalg.svd(inertia)
# det should be 1->rotation or -1->reflexion
if numpy.linalg.det(U) < 0: # reflexion
# made it a rotation by negating a column
U[:, 0] = -U[:, 0]
inertia_rotation = Quaternion.from_matrix(U)
return diagonal_inertia, inertia_rotation
def __init__(self, node, filepath, scale3d, offset, name_suffix='', generatedDir=None):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.node = node.createChild('collision'+name_suffix) # node
self.loader = SofaPython.Tools.meshLoader(self.node, filename=filepath, name='loader', scale3d=concat(scale3d), translation=concat(offset[:3]) , rotation=concat(r), triangulate=True)
self.topology = self.node.createObject('MeshTopology', name='topology', src='@loader')
self.dofs = self.node.createObject('MechanicalObject', name='dofs', template='Vec3'+template_suffix)
self.triangles = self.node.createObject('TriangleModel', name='model')
if generatedDir is None:
self.mapping = self.node.createObject('LinearMapping', template='Affine,Vec3'+template_suffix, name='mapping')
else:
serialization.importLinearMapping(self.node, generatedDir+"_collisionmapping.json")
self.normals = None
def __init__(self, node, filepath, scale3d, offset):
self.node = node.createChild("visual") # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel',
template='ExtVec3f',
name='model',
fileMesh=filepath,
scale3d=concat(scale3d),
translation=concat(offset[:3]),
rotation=concat(r))
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
def loadVisualCylinder(self,
meshPath,
offset=[0, 0, 0, 0, 0, 0, 1],
scale=[1, 1, 1],
color=[1, 1, 1, 1],
radius=0.01,
**kwargs):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.visual = self.node.createObject("OglCylinderModel",
radius=radius,
position="@topology.position",
edges="@topology.edges")
self.normals = self.visual
def loadMesh(self,
meshPath,
offset=[0, 0, 0, 0, 0, 0, 1],
scale=[1, 1, 1],
triangulate=False):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.meshLoader = SofaPython.Tools.meshLoader(self.node,
meshPath,
translation=concat(
offset[:3]),
rotation=concat(r),
scale3d=concat(scale),
triangulate=triangulate)
self.topology = self.node.createObject("MeshTopology",
name="topology",
src="@" + self.meshLoader.name)
def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
global idxVisualModel
self.node = node.createChild("visual" + name_suffix) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel',
name="visual" +
str(idxVisualModel),
fileMesh=filepath,
scale3d=concat(scale3d),
translation=concat(offset[:3]),
rotation=concat(r),
useNormals=False,
updateNormals=True)
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
idxVisualModel += 1
def read_rigid(rigidFileName):
rigidFile = open(rigidFileName, "r")
line = list(rigidFile)
rigidFile.close()
# for i in xrange(len(line)):
# print str(i) + str(line[i])
start = 1
res = MassInfo()
res.mass = float(line[start].split(' ')[1])
#volm = float( line[start + 1].split(' ')[1])
res.com = map(float, line[start + 3].split(' ')[1:])
inertia = map(
float, line[start + 2].split(' ')[1:]) # pick inertia matrix from file
res.inertia = array([res.mass * x for x in inertia
]).reshape(3, 3) # convert it in numpy 3x3 matrix
# extracting principal axes basis and corresponding rotation and diagonal inertia
if inertia[1] > 1e-5 or inertia[2] > 1e-5 or inertia[
5] > 1e-5: # if !diagonal (1e-5 seems big but the precision from a mesh is poor)
# print res.inertia
U, res.diagonal_inertia, V = linalg.svd(res.inertia)
# det should be 1->rotation or -1->reflexion
if linalg.det(U) < 0: # reflexion
# made it a rotation by negating a column
# print "REFLEXION"
U[:, 0] = -U[:, 0]
res.inertia_rotation = quat.from_matrix(U)
# print "generate_rigid not diagonal U" +str(U)
# print "generate_rigid not diagonal V" +str(V)
# print "generate_rigid not diagonal d" +str(res.diagonal_inertia)
else:
res.diagonal_inertia = res.inertia.diagonal()
res.inertia_rotation = [0, 0, 0, 1]
# print "generate_rigid " + str(res.mass) + " " + str( res.inertia ) + " " + str( res.diagonal_inertia )
return res
def __init__(self, node, filepath, scale3d, offset):
self.node = node.createChild("collision") # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.loader = self.node.createObject("MeshObjLoader",
name='loader',
filename=filepath,
scale3d=concat(scale3d),
translation=concat(
offset[:3]),
rotation=concat(r),
triangulate=1)
self.topology = self.node.createObject('MeshTopology',
name='topology',
src="@loader")
self.dofs = self.node.createObject('MechanicalObject', name='dofs')
self.triangles = self.node.createObject('TriangleModel',
template='Vec3d',
name='model')
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
def read_rigid(rigidFileName):
rigidFile = open( rigidFileName, "r" )
line = list( rigidFile )
rigidFile.close()
# for i in xrange(len(line)):
# print str(i) + str(line[i])
start = 1
res = MassInfo()
res.mass = float( line[start].split(' ')[1] )
#volm = float( line[start + 1].split(' ')[1])
res.com = map(float, line[start + 3].split(' ')[1:] )
inertia = map(float, line[start + 2].split(' ')[1:] ) # pick inertia matrix from file
res.inertia = array( [res.mass * x for x in inertia] ).reshape( 3, 3 ) # convert it in numpy 3x3 matrix
# extracting principal axes basis and corresponding rotation and diagonal inertia
if inertia[1]>1e-5 or inertia[2]>1e-5 or inertia[5]>1e-5 : # if !diagonal (1e-5 seems big but the precision from a mesh is poor)
# print res.inertia
U, res.diagonal_inertia, V = linalg.svd(res.inertia)
# det should be 1->rotation or -1->reflexion
if linalg.det(U) < 0 : # reflexion
# made it a rotation by negating a column
# print "REFLEXION"
U[:,0] = -U[:,0]
res.inertia_rotation = quat.from_matrix( U )
# print "generate_rigid not diagonal U" +str(U)
# print "generate_rigid not diagonal V" +str(V)
# print "generate_rigid not diagonal d" +str(res.diagonal_inertia)
else :
res.diagonal_inertia = res.inertia.diagonal()
res.inertia_rotation = [0,0,0,1]
# print "generate_rigid " + str(res.mass) + " " + str( res.inertia ) + " " + str( res.diagonal_inertia )
return res
def loadVisual(self,
meshPath,
offset=[0, 0, 0, 0, 0, 0, 1],
scale=[1, 1, 1],
color=[1, 1, 1, 1],
**kwargs):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.visual = self.node.createObject("VisualModel",
name="model",
filename=meshPath,
translation=concat(offset[:3]),
rotation=concat(r),
scale3d=concat(scale),
color=concat(color),
putOnlyTexCoords=True,
computeTangents=True,
**kwargs)
# self.visual = self.node.createObject("VisualModel", name="model", filename=meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), color=concat(color), **kwargs)
self.visual.setColor(
color[0], color[1], color[2], color[3]
) # the previous assignement fails when reloading a scene..
self.normals = self.visual
def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
self.node = node.createChild("collision" + name_suffix) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.loader = SofaPython.Tools.meshLoader(self.node,
filename=filepath,
name='loader',
scale3d=concat(scale3d),
translation=concat(
offset[:3]),
rotation=concat(r),
triangulate=True)
self.topology = self.node.createObject('MeshTopology',
name='topology',
src="@loader")
self.dofs = self.node.createObject('MechanicalObject',
name='dofs',
template="Vec3" +
template_suffix)
self.triangles = self.node.createObject('TriangleModel',
name='model')
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
self.normals = None
self.visual = None
def addMeshLoader(
self,
meshFile,
value,
insideValue=None,
closingValue=None,
roiIndices=list(),
roiValue=list(),
name=None,
offset=[0, 0, 0, 0, 0, 0, 1],
scale=[1, 1, 1],
):
mesh = Image.Mesh(value, insideValue)
_name = name if not name is None else os.path.splitext(os.path.basename(meshFile))[0]
mesh.mesh = SofaPython.Tools.meshLoader(
self.node,
meshFile,
name="meshLoader_" + _name,
triangulate=True,
translation=concat(offset[:3]),
rotation=concat(Quaternion.to_euler(offset[3:]) * 180.0 / math.pi),
scale3d=concat(scale),
)
self.__addMesh(mesh, closingValue, roiIndices, roiValue, _name)
def transformToData(scale,offset,timeOffset=0,timeScale=1,isPerspective=0):
""" Returns a transform, formatted to sofa data given voxelsize, rigid position (offset), time and camera parameters
"""
return concat(offset[:3])+' '+concat(quat.to_euler(offset[3:])*180./math.pi)+' '+concat(scale)+' '+str(timeOffset)+' '+str(timeScale)+' '+str(int(isPerspective))
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 normalizeRotation(self):
## Numerical drift can produce non-unit quaternions.
## Forcing its normalization.
self.rotation = quat.normalized(self.rotation)
def inv(self):
res = Frame()
res.rotation = quat.conj(self.rotation)
res.translation = -quat.rotate(res.rotation, self.translation)
return res
def __init__(self, node, filepath, scale3d, offset):
self.node = node.createChild( "visual" ) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel', template='ExtVec3f', name='model', fileMesh=filepath, scale3d=concat(scale3d), translation=concat(offset[:3]) , rotation=concat(r) )
self.mapping = self.node.createObject('RigidMapping', name="mapping")
def loadVisual(self, meshPath, offset = [0,0,0,0,0,0,1], scale=[1,1,1], color=[1,1,1,1],**kwargs):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.visual = self.node.createObject("VisualModel", name="model", filename=meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), color=concat(color), putOnlyTexCoords=True,computeTangents=True,**kwargs)
# self.visual = self.node.createObject("VisualModel", name="model", filename=meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), color=concat(color), **kwargs)
self.visual.setColor(color[0],color[1],color[2],color[3]) # the previous assignement fails when reloading a scene..
self.normals = self.visual
def test_inv(q):
q_inv=Quaternion.inv(q)
return EXPECT_VEC_EQ(Quaternion.id(), Quaternion.prod(q,q_inv), "test_inv")
def __init__(self):
self.mass=0.
self.com=[0.,0.,0.]
self.diagonal_inertia=[0.,0.,0.]
self.inertia_rotation=Quaternion.id()
def test_inv(q):
q_inv = Quaternion.inv(q)
return EXPECT_VEC_EQ(Quaternion.id(), Quaternion.prod(q, q_inv),
"test_inv")
def rotateAround(self, axis, angle, field="position"):
p = self.rigidobject.getData(field)
pq = p.value[0]
p.value = pq[:3] + list(
Quaternion.prod(axisToQuat(axis, angle), pq[3:]))
def addMeshLoader(self, meshFile, value, insideValue=None, closingValue=None, roiIndices=list(), roiValue=list(), name=None, offset = [0,0,0,0,0,0,1], scale=[1,1,1]):
mesh = Image.Mesh(value, insideValue)
_name = name if not name is None else os.path.splitext(os.path.basename(meshFile))[0]
mesh.mesh = SofaPython.Tools.meshLoader(self.node, meshFile, name="meshLoader_"+_name, triangulate=True, translation=concat(offset[:3]) , rotation=concat(Quaternion.to_euler(offset[3:]) * 180.0 / math.pi), scale3d=concat(scale))
self.__addMesh(mesh,closingValue,roiIndices,roiValue,_name)
def run():
ok = True
info = SofaPython.mass.RigidMassInfo()
# testing axis-aligned known geometric shapes
for m in xrange(len(meshes)):
mesh = meshes[m]
mesh_path = path + meshes[m]
for s in xrange(len(scales)):
scale = scales[s]
if mesh=="cylinder.obj" and scale[0]!=scale[1]:
continue
for d in xrange(len(densities)):
density=densities[d]
info.setFromMesh( mesh_path, density, scale )
error = " ("+meshes[m]+", s="+Tools.cat(scale)+" d="+str(density)+")"
ok &= EXPECT_TRUE( almostEqualReal(info.mass, masses[m][s][d]), "mass"+error+" "+str(info.mass)+"!="+str(masses[m][s][d]) )
ok &= EXPECT_TRUE( almostEqualLists(info.com,[x*0.5 for x in scale]), "com"+error+" "+Tools.cat(info.com)+"!="+Tools.cat([x*0.5 for x in scale]) )
ok &= EXPECT_TRUE( almostEqualLists(info.diagonal_inertia,inertia[m][s][d]), "inertia"+error+" "+str(info.diagonal_inertia)+"!="+str(inertia[m][s][d]) )
# testing diagonal inertia extraction from a rotated cuboid
mesh = "cube.obj"
mesh_path = path + mesh
scale = scales[3]
density = 1
theory = sorted(inertia[0][3][0])
for r in rotations:
info.setFromMesh( mesh_path, density, scale, r )
local = sorted(info.diagonal_inertia)
ok &= EXPECT_TRUE( almostEqualLists(local,theory), "inertia "+str(local)+"!="+str(theory)+" (rotation="+str(r)+")" )
# testing extracted inertia rotation
mesh = "rotated_cuboid_12_35_-27.obj"
mesh_path = path + mesh
density = 1
info.setFromMesh( mesh_path, density )
# theoretical results
scale = [2,3,1]
mass = density * scale[0]*scale[1]*scale[2]
inertiat = numpy.empty(3)
inertiat[0] = 1.0/12.0 * mass * (scale[1]*scale[1]+scale[2]*scale[2]) # x
inertiat[1] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[2]*scale[2]) # y
inertiat[2] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[1]*scale[1]) # z
# used quaternion in mesh
q = Quaternion.normalized( Quaternion.from_euler( [12*math.pi/180.0, 35*math.pi/180.0, -27*math.pi/180.0] ) )
# corresponding rotation matrices (ie frame defined by columns)
mt = Quaternion.to_matrix( q )
m = Quaternion.to_matrix( info.inertia_rotation )
# matching inertia
idxt = numpy.argsort(inertiat)
idx = numpy.argsort(info.diagonal_inertia)
# checking if each axis/column are parallel (same or opposite for unitary vectors)
for i in xrange(3):
ok &= EXPECT_TRUE( almostEqualLists(mt[:,idxt[i]].tolist(),m[:,idx[i]].tolist(),1e-5) or almostEqualLists(mt[:,idxt[i]].tolist(),(-m[:,idx[i]]).tolist(),1e-5), "wrong inertia rotation" )
# print mt[:,idxt]
# print m [:,idx ]
return ok
def axisToQuat(axis, angle):
na = numpy.zeros(3)
na[0] = axis[0]
na[1] = axis[1]
na[2] = axis[2]
return list(Quaternion.axisToQuat(na, angle))
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'
def inv(self):
res = Frame()
res.rotation = quat.conj( self.rotation )
res.translation = - quat.rotate(res.rotation, self.translation)
return res
def normalizeRotation(self):
## Numerical drift can produce non-unit quaternions.
## Forcing its normalization.
self.rotation = quat.normalized(self.rotation)
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 loadMesh(self, meshPath, offset = [0,0,0,0,0,0,1], scale=[1,1,1], triangulate=False):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.meshLoader = SofaPython.Tools.meshLoader(self.node, meshPath, translation=concat(offset[:3]) , rotation=concat(r), scale3d=concat(scale), triangulate=triangulate)
self.topology = self.node.createObject("MeshTopology", name="topology", src="@"+self.meshLoader.name )
def __init__(self):
self.mass=0.
self.com=[0.,0.,0.]
self.diagonal_inertia=[0.,0.,0.]
self.inertia_rotation=Quaternion.id()
self.density = 0.
def loadVisualCylinder(self, meshPath, offset = [0,0,0,0,0,0,1], scale=[1,1,1], color=[1,1,1,1],radius=0.01,**kwargs):
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.visual = self.node.createObject("OglCylinderModel", radius=radius, position="@topology.position", edges="@topology.edges" )
self.normals = self.visual
def generate_rigid(filename, density = 1000.0, scale=[1,1,1], rotation=[0,0,0]):
# TODO bind GenerateRigid
# - faster than writing in a file
# - more robust (if several processes try to work in the same file)
tmpfilename = Tools.path( __file__ ) +"/tmp.rigid"
cmd = [ Sofa.build_dir() + '/bin/GenerateRigid', filename, tmpfilename, str(density), str(scale[0]), str(scale[1]), str(scale[2]), str(rotation[0]), str(rotation[1]), str(rotation[2]) ]
# print cmd
try:
output = Popen(cmd, stdout=PIPE)
except OSError:
# try the debug version
cmd[0] += 'd'
try:
output = Popen(cmd, stdout=PIPE)
except OSError:
print 'error when calling GenerateRigid, do you have GenerateRigid built in SOFA?'
raise
output.communicate() # wait until Popen command is finished!!!
# GenerateRigid output is stored in the file tmpfilename
rigidFile = open( tmpfilename, "r" )
line = list( rigidFile )
rigidFile.close()
# for i in xrange(len(line)):
# print str(i) + str(line[i])
start = 1
res = MassInfo()
res.mass = float( line[start].split(' ')[1] )
#volm = float( line[start + 1].split(' ')[1] )
res.com = map(float, line[start + 3].split(' ')[1:] )
inertia = map(float, line[start + 2].split(' ')[1:] ) # pick inertia matrix from file
res.inertia = array( [res.mass * x for x in inertia] ).reshape( 3, 3 ) # convert it in numpy 3x3 matrix
# extracting principal axes basis and corresponding rotation and diagonal inertia
if inertia[1]>1e-5 or inertia[2]>1e-5 or inertia[5]>1e-5 : # if !diagonal (1e-5 seems big but the precision from a mesh is poor)
# print res.inertia
U, res.diagonal_inertia, V = linalg.svd(res.inertia)
# det should be 1->rotation or -1->reflexion
if linalg.det(U) < 0 : # reflexion
# made it a rotation by negating a column
# print "REFLEXION"
U[:,0] = -U[:,0]
res.inertia_rotation = quat.from_matrix( U )
# print "generate_rigid not diagonal U" +str(U)
# print "generate_rigid not diagonal V" +str(V)
# print "generate_rigid not diagonal d" +str(res.diagonal_inertia)
else :
res.diagonal_inertia = res.inertia.diagonal()
res.inertia_rotation = [0,0,0,1]
# print "generate_rigid " + str(res.mass) + " " + str( res.inertia ) + " " + str( res.diagonal_inertia )
return res