def createScene(root): ##### global parameters root.createObject( 'VisualStyle', displayFlags="showBehavior showWireframe showCollisionModels") root.dt = 0.01 root.gravity = [0, -10, 0] root.createObject('RequiredPlugin', pluginName='Compliant') root.createObject('CompliantAttachButtonSetting') ##### SOLVER root.createObject('CompliantImplicitSolver', stabilization=0, neglecting_compliance_forces_in_geometric_stiffness=0) root.createObject('SequentialSolver', iterations=100, precision=0) #root.createObject('LUResponse') root.createObject('LDLTResponse') bodies = [] points = [] N = 10 for i in xrange(N): body = StructuralAPI.RigidBody(root, "body_" + str(i)) body.setManually([i, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1]) body.dofs.showObject = True body.dofs.showObjectScale = .5 bodies.append(body) bodies[0].node.createObject('FixedConstraint') bodies[N - 1].mass.mass = 10 bodies[N - 1].mass.inertia = "10 10 10" for i in xrange(N - 1): p0 = bodies[i].addMappedPoint("right", [0.5, 0, 0]) p0.dofs.showObject = True p0.dofs.showObjectScale = .1 p0.dofs.drawMode = 1 p1 = bodies[i + 1].addMappedPoint("left", [-0.5, 0, 0]) p1.dofs.showObject = True p1.dofs.showObjectScale = .1 p1.dofs.drawMode = 2 d = p0.node.createChild("d" + str(i)) d.createObject('MechanicalObject', template='Vec3' + StructuralAPI.template_suffix, name='dofs', position='0 0 0') input = [] # @internal input.append('@' + Tools.node_path_rel(root, p0.node) + '/dofs') input.append('@' + Tools.node_path_rel(root, p1.node) + '/dofs') d.createObject('DifferenceMultiMapping', name='mapping', input=Tools.cat(input), output='@dofs', pairs="0 0") p1.node.addChild(d) d.createObject('UniformCompliance', name='compliance', compliance="0")
def createScene(root): ##### global parameters root.createObject('VisualStyle', displayFlags="showBehavior showWireframe showCollisionModels" ) root.dt = 0.01 root.gravity = [0, -10, 0] root.createObject('RequiredPlugin', pluginName = 'Compliant') root.createObject('CompliantAttachButtonSetting') ##### SOLVER root.createObject('CompliantImplicitSolver', stabilization=0, neglecting_compliance_forces_in_geometric_stiffness=0) root.createObject('SequentialSolver', iterations=100, precision=0) #root.createObject('LUResponse') root.createObject('LDLTResponse') bodies = [] points = [] N = 10 for i in xrange(N): body = StructuralAPI.RigidBody( root, "body_"+str(i) ) body.setManually( [i,0,0,0,0,0,1], 1, [1,1,1] ) body.dofs.showObject = True body.dofs.showObjectScale = .5 bodies.append( body ) bodies[0].node.createObject('FixedConstraint') bodies[N-1].mass.mass = 10 bodies[N-1].mass.inertia = "10 10 10" for i in xrange(N-1): p0 = bodies[i].addMappedPoint( "right", [0.5, 0, 0] ) p0.dofs.showObject = True p0.dofs.showObjectScale = .1 p0.dofs.drawMode=1 p1 = bodies[i+1].addMappedPoint( "left", [-0.5, 0, 0] ) p1.dofs.showObject = True p1.dofs.showObjectScale = .1 p1.dofs.drawMode=2 d = p0.node.createChild( "d"+str(i) ) d.createObject('MechanicalObject', template = 'Vec3'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,p0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,p1.node) + '/dofs' ) d.createObject('DifferenceMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 0" ) p1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
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 createScene(root): ##### global parameters root.createObject('VisualStyle', displayFlags="showBehavior showCollisionModels" ) root.dt = 0.01 root.gravity = [0, -10, 0] root.createObject('RequiredPlugin', pluginName = 'Compliant') root.createObject('CompliantAttachButton') root.createObject('DefaultPipeline', name='DefaultCollisionPipeline', depth="6") root.createObject('BruteForceBroadPhase', name='N2') root.createObject('BVHNarrowPhase') root.createObject('DiscreteIntersection') root.createObject('DefaultContactManager', name="Response", response="CompliantContact", responseParams="compliance=0&restitution=0" ) ##### SOLVER root.createObject('CompliantImplicitSolver', stabilization=1, neglecting_compliance_forces_in_geometric_stiffness=1) root.createObject('SequentialSolver', iterations=100, precision=0) #root.createObject('LUResponse') root.createObject('LDLTResponse') ##### GEAR gearNode = root.createChild( "GEAR" ) r0 = 0.33 r1 = 0.66 body0 = StructuralAPI.RigidBody( gearNode, "body_0" ) body0.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = r0*1.1 body0.dofs.velocity="0 0 0 0 1 0" body0.node.createObject('Sphere', radius=r0) body1 = StructuralAPI.RigidBody( gearNode, "body_1" ) body1.setManually( [1,0,0,0,0,0,1], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = r1*1.1 body1.node.createObject('Sphere', radius=r1) body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = -r0/r1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" ) ##### driving belt / chain beltNode = root.createChild( "BELT" ) r0 = 0.7 r1 = 0.3 body0 = StructuralAPI.RigidBody( beltNode, "body_0" ) body0.setManually( [0,-2,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = r0*1.1 body0.dofs.velocity="0 0 0 0 1 0" body0.node.createObject('Sphere', radius=r0) body1 = StructuralAPI.RigidBody( beltNode, "body_1" ) body1.setManually( [1.5,-2,0,0,0,0,1], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = r1*1.1 body1.node.createObject('Sphere', radius=r1) body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = r0/r1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" ) ##### angle transmission angleNode = root.createChild( "ANGLE" ) r0 = 0.49 r1 = 0.49 body0 = StructuralAPI.RigidBody( angleNode, "body_0" ) body0.setManually( [0,-4,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = r0*1.1 body0.dofs.velocity="0 0 0 1 0 0" body0.node.createObject('Sphere', radius=r0) body1 = StructuralAPI.RigidBody( angleNode, "body_1" ) body1.setManually( [1,-4,0,0,0,0,1], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = r1*1.1 body1.node.createObject('Sphere', radius=r1) body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 0 1 1") body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 3 0 4", ratio = r0/r1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" ) ##### rack rackNode = root.createChild( "RACK" ) body0 = StructuralAPI.RigidBody( rackNode, "body_0" ) body0.setManually( [0,-6,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = 0.55 body0.dofs.velocity="0 0 0 0 0 1" body0.node.createObject('Sphere', radius=0.5) body1 = StructuralAPI.RigidBody( rackNode, "body_1" ) body1.setManually( [-2,-6.71,0, 0,0,0.7071067811865476,0.7071067811865476], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = 0.3 body1.node.createObject('Capsule', radii="0.2", heights="5") body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 1 0") body1.node.createObject('PartialFixedConstraint', fixedDirections="0 1 1 1 1 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 5 0 0", ratio = 1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
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 createScene(root): ##### global parameters root.createObject('VisualStyle', displayFlags="showBehavior showCollisionModels" ) root.dt = 0.01 root.gravity = [0, -10, 0] root.createObject('RequiredPlugin', pluginName = 'Compliant') root.createObject('CompliantAttachButton') root.createObject('DefaultPipeline', name='DefaultCollisionPipeline', depth="6") root.createObject('BruteForceDetection') root.createObject('DiscreteIntersection') root.createObject('DefaultContactManager', name="Response", response="CompliantContact", responseParams="compliance=0&restitution=0" ) ##### SOLVER root.createObject('CompliantImplicitSolver', stabilization=1, neglecting_compliance_forces_in_geometric_stiffness=1) root.createObject('SequentialSolver', iterations=100, precision=0) #root.createObject('LUResponse') root.createObject('LDLTResponse') ##### GEAR gearNode = root.createChild( "GEAR" ) r0 = 0.33 r1 = 0.66 body0 = StructuralAPI.RigidBody( gearNode, "body_0" ) body0.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = r0*1.1 body0.dofs.velocity="0 0 0 0 1 0" body0.node.createObject('Sphere', radius=r0) body1 = StructuralAPI.RigidBody( gearNode, "body_1" ) body1.setManually( [1,0,0,0,0,0,1], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = r1*1.1 body1.node.createObject('Sphere', radius=r1) body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = -r0/r1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" ) ##### driving belt / chain beltNode = root.createChild( "BELT" ) r0 = 0.7 r1 = 0.3 body0 = StructuralAPI.RigidBody( beltNode, "body_0" ) body0.setManually( [0,-2,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = r0*1.1 body0.dofs.velocity="0 0 0 0 1 0" body0.node.createObject('Sphere', radius=r0) body1 = StructuralAPI.RigidBody( beltNode, "body_1" ) body1.setManually( [1.5,-2,0,0,0,0,1], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = r1*1.1 body1.node.createObject('Sphere', radius=r1) body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = r0/r1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" ) ##### angle transmission angleNode = root.createChild( "ANGLE" ) r0 = 0.49 r1 = 0.49 body0 = StructuralAPI.RigidBody( angleNode, "body_0" ) body0.setManually( [0,-4,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = r0*1.1 body0.dofs.velocity="0 0 0 1 0 0" body0.node.createObject('Sphere', radius=r0) body1 = StructuralAPI.RigidBody( angleNode, "body_1" ) body1.setManually( [1,-4,0,0,0,0,1], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = r1*1.1 body1.node.createObject('Sphere', radius=r1) body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 0 1 1") body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 3 0 4", ratio = r0/r1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" ) ##### rack rackNode = root.createChild( "RACK" ) body0 = StructuralAPI.RigidBody( rackNode, "body_0" ) body0.setManually( [0,-6,0,0,0,0,1], 1, [1,1,1] ) body0.dofs.showObject = True body0.dofs.showObjectScale = 0.55 body0.dofs.velocity="0 0 0 0 0 1" body0.node.createObject('Sphere', radius=0.5) body1 = StructuralAPI.RigidBody( rackNode, "body_1" ) body1.setManually( [-2,-6.71,0, 0,0,0.7071067811865476,0.7071067811865476], 1, [1,1,1] ) body1.dofs.showObject = True body1.dofs.showObjectScale = 0.3 body1.node.createObject('Capsule', radii="0.2", heights="5") body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 1 0") body1.node.createObject('PartialFixedConstraint', fixedDirections="0 1 1 1 1 1") d = body0.node.createChild( "d" ) d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' ) input = [] # @internal input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' ) input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' ) d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 5 0 0", ratio = 1 ) body1.node.addChild( d ) d.createObject('UniformCompliance', name = 'compliance', compliance="0" )