Example #1
0
    def __init__(self,
                 name,
                 node1,
                 node2,
                 stiffnesses=[0, 0, 0, 0, 0, 0],
                 index1=0,
                 index2=0):
        self.node = node1.createChild(name)
        self.dofs = self.node.createObject('MechanicalObject',
                                           template='Vec6' + template_suffix,
                                           name='dofs',
                                           position='0 0 0 0 0 0')
        input = []  # @internal
        input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
        input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')

        self.mapping = self.node.createObject(
            'RigidJointMultiMapping',
            name='mapping',
            input=concat(input),
            output='@dofs',
            pairs=str(index1) + " " + str(index2),
            geometricStiffness=geometric_stiffness)
        self.forcefield = self.node.createObject('DiagonalStiffness',
                                                 name='ff',
                                                 stiffness=concat(stiffnesses))
        node2.addChild(self.node)
Example #2
0
        def __init__(self, node, masks, limits, compliances):
            self.node = node.createChild("limits")

            set = []
            position = [0] * len(masks)
            offset = []

            for i in range(len(masks)):
                set = set + [0] + masks[i]
                offset.append(limits[i])

            self.dofs = self.node.createObject('MechanicalObject',
                                               template='Vec1' +
                                               template_suffix,
                                               name='dofs',
                                               position=concat(position))
            self.mapping = self.node.createObject('ProjectionMapping',
                                                  set=concat(set),
                                                  offset=concat(offset))
            self.compliance = self.node.createObject(
                'DiagonalCompliance',
                name='compliance',
                compliance=concat(compliances))
            # self.type = self.node.createObject('Stabilization')
            self.constraint = self.node.createObject('UnilateralConstraint')
Example #3
0
 def __init__(self, name, node1, node2, mask, compliance=0, index1=0, index2=0, isCompliance=True):
     self.node = node1.createChild(name)
     self.mask = mask
     self.dofs = self.node.createObject(
         "MechanicalObject", template="Vec6" + template_suffix, name="dofs", position="0 0 0 0 0 0"
     )
     input = []  # @internal
     input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
     if not node2 is None:
         input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
         self.mapping = self.node.createObject(
             "RigidJointMultiMapping",
             name="mapping",
             input=concat(input),
             output="@dofs",
             pairs=str(index1) + " " + str(index2),
             geometricStiffness=geometric_stiffness,
         )
         node2.addChild(self.node)
     else:
         self.mapping = self.node.createObject(
             "RigidJointMapping",
             name="mapping",
             input=concat(input),
             output="@dofs",
             pairs=str(index1) + " " + str(index2),
             geometricStiffness=geometric_stiffness,
         )
     self.constraint = GenericRigidJoint.Constraint(self.node, mask, compliance, isCompliance)
Example #4
0
 def __init__(self, node, mask, velocities, compliance):
     self.node = node.createChild( "controller" )
     position = [0] * len(mask)
     self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
     self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
     self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=1)
     self.type = self.node.createObject('VelocityConstraintValue', velocities=concat(velocities))
Example #5
0
 def __init__(self,
              node,
              name,
              node1,
              node2,
              stiffnesses=[0, 0, 0, 0, 0, 0],
              index1=0,
              index2=0):
     self.node = node.createChild(name)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec6d',
                                        name='dofs',
                                        position='0 0 0 0 0 0')
     input = []  # @internal
     input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
     input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
     self.mapping = self.node.createObject('RigidJointMultiMapping',
                                           template='Rigid,Vec6d',
                                           name='mapping',
                                           input=concat(input),
                                           output='@dofs',
                                           pairs=str(index1) + " " +
                                           str(index2))
     compliances = vec.inv(stiffnesses)
     self.compliance = self.node.createObject(
         'DiagonalCompliance',
         template="Vec6d",
         name='compliance',
         compliance=concat(compliances),
         isCompliance=0)
Example #6
0
    def insert(self, parent):
        self.node = parent.createChild(self.name)

        # build input data for multimapping
        input = []
        for b, o in zip(self.body, self.offset):
            if o is None:
                input.append('@' + Tools.node_path_rel(self.node, b) + '/dofs')
            else:
                joint = b.createChild(self.name + '-offset')

                joint.createObject('MechanicalObject',
                                   template='Rigid',
                                   name='dofs')

                joint.createObject('AssembledRigidRigidMapping',
                                   template="Rigid,Rigid",
                                   source='0 ' + str(o))

                input.append('@' + Tools.node_path_rel(self.node, b) + '/' +
                             joint.name + '/dofs')

        if len(input) == 0:
            print 'warning: empty joint'
            return None

        dofs = self.node.createObject('MechanicalObject',
                                      template='Vec6',
                                      name='dofs',
                                      position='0 0 0 0 0 0')

        map = self.node.createObject('RigidJointMultiMapping',
                                     name='mapping',
                                     template='Rigid3,Vec6',
                                     input=concat(input),
                                     output='@dofs',
                                     pairs="0 0")

        sub = self.node.createChild("constraints")

        sub.createObject('MechanicalObject', template='Vec1', name='dofs')

        mask = [(1 - d) for d in self.dofs]

        map = sub.createObject('MaskMapping',
                               name='mapping',
                               template='Vec6,Vec1',
                               input='@../',
                               output='@dofs',
                               dofs=concat(mask))

        compliance = sub.createObject('UniformCompliance',
                                      name='compliance',
                                      template='Vec1',
                                      compliance=self.compliance)

        stab = sub.createObject('Stabilization')

        return self.node
Example #7
0
 def __init__(self, node, mask, threshold):
     self.node = node.createChild( "resistance" )
     position = [0] * len(mask)
     self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
     self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
     self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=0, isCompliance=1)
     self.type = self.node.createObject('VelocityConstraintValue', velocities=concat([0]) )
     self.constraint = self.node.createObject('ResistanceConstraint', threshold=threshold)
Example #8
0
 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
Example #9
0
    def insert(self, node):
        res = node.createChild(self.name)

        dofs = self.dofs.insert(res, name="dofs")

        mass_node = res

        if self.offset != None:
            mass_node = res.createChild("mapped_mass")
            self.offset.insert(mass_node, name="dofs")
            mapping = mass_node.createObject(
                "AssembledRigidRigidMapping", template="Rigid", source="0 " + str(self.offset)
            )
        # mass
        mass = mass_node.createObject(
            "RigidMass",
            template="Rigid",
            name="mass",
            mass=self.mass,
            inertia=concat(self.inertia),
            inertia_forces=self.inertia_forces,
        )

        # visual model
        if self.visual != None:
            visual_template = "ExtVec3f"

            visual = res.createChild("visual")
            ogl = visual.createObject(
                "OglModel",
                template=visual_template,
                name="mesh",
                fileMesh=self.visual,
                color=concat(self.color),
                scale3d="1 1 1",
            )

            visual_map = visual.createObject("RigidMapping", template="Rigid" + ", " + visual_template, input="@../")
        # collision model
        if self.collision != None:
            collision = res.createChild("collision")

            collision.createObject("MeshObjLoader", name="loader", filename=self.collision)

            collision.createObject("MeshTopology", name="topology", triangles="@loader.triangles")

            collision.createObject("MechanicalObject", name="dofs", position="@loader.position")

            model = collision.createObject("TriangleModel", name="model", template="Vec3d")
            if self.group != None:
                model.group = self.group

            collision.createObject("RigidMapping", template="Rigid,Vec3d", input="@../", output="@./")
        return res
Example #10
0
def insertVisual(parentNode, solid, color):
    node = parentNode.createChild("node_" + solid.name)
    translation = solid.position[:3]
    rotation = Quaternion.to_euler(solid.position[3:]) * 180.0 / math.pi
    for m in solid.mesh:
        Tools.meshLoader(node, m.source, name="loader_" + m.id)
        node.createObject("OglModel",
                          src="@loader_" + m.id,
                          translation=concat(translation),
                          rotation=concat(rotation),
                          color=color)
Example #11
0
 def __init__(self, node, mask, velocities, compliance):
     self.node = node.createChild("controller")
     position = [0] * len(mask)
     self.dofs = self.node.createObject(
         "MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
     )
     self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
     self.compliance = self.node.createObject(
         "UniformCompliance", name="compliance", compliance=compliance, isCompliance=1
     )
     self.type = self.node.createObject("VelocityConstraintValue", velocities=concat(velocities))
Example #12
0
 def __init__(self, node, mask, threshold):
     self.node = node.createChild("resistance")
     position = [0] * len(mask)
     self.dofs = self.node.createObject(
         "MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
     )
     self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
     self.compliance = self.node.createObject(
         "UniformCompliance", name="compliance", compliance=0, isCompliance=1
     )
     self.type = self.node.createObject("VelocityConstraintValue", velocities=concat([0]))
     self.constraint = self.node.createObject("ResistanceConstraint", threshold=threshold)
Example #13
0
 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")
Example #14
0
 def setManually(self, offset=[0, 0, 0, 0, 0, 0, 1], mass=1, inertia=[1, 1, 1], inertia_forces=False):
     ## create the rigid body by manually giving its inertia
     self.frame = Frame.Frame(offset)
     self.dofs = self.frame.insert(self.node, name="dofs", template="Rigid3" + template_suffix)
     self.mass = self.node.createObject(
         "RigidMass", name="mass", mass=mass, inertia=concat(inertia), inertia_forces=inertia_forces
     )
Example #15
0
 def __init__(self, node, compliances):
     self.node = node.createChild("constraint")
     self.dofs = self.node.createObject("MechanicalObject", template="Vec6" + template_suffix, name="dofs")
     self.mapping = self.node.createObject("IdentityMapping")
     self.compliance = self.node.createObject(
         "DiagonalCompliance", name="compliance", compliance=concat(compliances)
     )
Example #16
0
 def __init__(self, node, mask, compliance, isCompliance):
     self.node = node.createChild("constraint")
     self.dofs = self.node.createObject("MechanicalObject", template="Vec1" + template_suffix, name="dofs")
     self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
     self.compliance = self.node.createObject(
         "UniformCompliance", name="compliance", compliance=compliance, isCompliance=isCompliance
     )
Example #17
0
File: sml.py Project: Sreevis/sofa
def insertVisual(parentNode, solid, color):
    node = parentNode.createChild("node_"+solid.name)
    translation=solid.position[:3]
    rotation = Quaternion.to_euler(solid.position[3:])  * 180.0 / math.pi
    for m in solid.mesh:
        Tools.meshLoader(node, m.source, name="loader_"+m.id)
        node.createObject("OglModel",src="@loader_"+m.id, translation=concat(translation),rotation=concat(rotation), color=color)
Example #18
0
 def __init__(self,
              name,
              node1,
              node2,
              compliance=0,
              index1=0,
              index2=0,
              isCompliance=True):
     # GenericRigidJoint.__init__(self, name, node1, node2, [1,1,1,0,0,0], compliance, index1, index2) # GenericRigidJoint can work but is way overkill
     self.node = node1.createChild(name)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec3' + template_suffix,
                                        name='dofs',
                                        position='0 0 0')
     input = []  # @internal
     input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
     input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
     self.mapping = self.node.createObject('DifferenceMultiMapping',
                                           name='mapping',
                                           input=concat(input),
                                           output='@dofs',
                                           pairs=str(index1) + " " +
                                           str(index2))
     self.compliance = self.node.createObject('UniformCompliance',
                                              name='compliance',
                                              compliance=compliance,
                                              isCompliance=isCompliance)
     node2.addChild(self.node)
Example #19
0
 def __init__(self,
              node,
              name,
              node1,
              node2,
              compliance=0,
              index1=0,
              index2=0,
              rest_lenght=-1):
     self.node = node.createChild(name)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Rigid',
                                        name='dofs')
     input = []  # @internal
     input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
     input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
     self.mapping = self.node.createObject('SubsetMultiMapping',
                                           template='Rigid,Rigid',
                                           name='mapping',
                                           input=concat(input),
                                           output='@dofs',
                                           indexPairs="0 " + str(index1) +
                                           " 1 " + str(index2))
     self.constraint = DistanceRigidJoint.Constraint(
         self.node, compliance, rest_lenght)
Example #20
0
 def __init__(self,
              node,
              name,
              node1,
              node2,
              mask,
              compliance=0,
              index1=0,
              index2=0):
     self.node = node.createChild(name)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec6d',
                                        name='dofs',
                                        position='0 0 0 0 0 0')
     input = []  # @internal
     input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
     input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
     self.mapping = self.node.createObject('RigidJointMultiMapping',
                                           template='Rigid,Vec6d',
                                           name='mapping',
                                           input=concat(input),
                                           output='@dofs',
                                           pairs=str(index1) + " " +
                                           str(index2))
     self.constraint = GenericRigidJoint.Constraint(self.node, mask,
                                                    compliance)
Example #21
0
 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
Example #22
0
 def addSpring(self, stiffness):
     mask = [(1 - d) for d in self.mask]
     mask = vec.scal(1.0 / stiffness, mask)
     return self.node.createObject('DiagonalCompliance',
                                   template="Rigid",
                                   isCompliance="0",
                                   compliance=concat(mask))
Example #23
0
 def addMotor(self, forces=[0, 0, 0, 0, 0, 0]):
     ## adding a constant force/torque at the offset location (that could be driven by a controller to simulate a motor)
     return self.node.createObject('ConstantForceField',
                                   template='Rigid3' + template_suffix,
                                   name='motor',
                                   points='0',
                                   forces=concat(forces))
Example #24
0
 def __init__(self,
              name,
              node1,
              node2,
              compliances=[0, 0, 0, 0, 0, 0],
              index1=0,
              index2=0):
     self.node = node1.createChild(name)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec6' + template_suffix,
                                        name='dofs',
                                        position='0 0 0 0 0 0')
     input = []  # @internal
     input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
     input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
     self.mapping = self.node.createObject(
         'RigidJointMultiMapping',
         name='mapping',
         input=concat(input),
         output='@dofs',
         pairs=str(index1) + " " + str(index2),
         geometricStiffness=geometric_stiffness)
     self.constraint = CompleteRigidJoint.Constraint(
         self.node, compliances
     )  # the constraint compliance cannot be in the same branch as eventual limits...
     node2.addChild(self.node)
Example #25
0
 def __init__(self, node, mask, velocities, compliance):
     self.node = node.createChild("controller")
     position = [0] * len(mask)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec1' +
                                        template_suffix,
                                        name='dofs',
                                        position=concat(position))
     self.mapping = self.node.createObject('MaskMapping',
                                           dofs=concat(mask))
     self.compliance = self.node.createObject('UniformCompliance',
                                              name='compliance',
                                              compliance=compliance,
                                              isCompliance=1)
     self.type = self.node.createObject('VelocityConstraintValue',
                                        velocities=concat(velocities))
Example #26
0
 def addMotor(self, forces=[0, 0, 0, 0, 0, 0]):
     ## adding a constant force/torque to the rigid body (that could be driven by a controller to simulate a motor)
     return self.node.createObject('ConstantForceField',
                                   template='Rigid',
                                   name='motor',
                                   points='0',
                                   forces=concat(forces))
Example #27
0
 def addSpring(self, translation_stiffness, rotation_stiffness):
     mask = [0] * 6
     mask[self.axis] = 1.0 / translation_stiffness
     mask[3 + self.axis] = 1.0 / rotation_stiffness
     return self.node.createObject('DiagonalCompliance',
                                   template="Rigid",
                                   isCompliance="0",
                                   compliance=concat(mask))
Example #28
0
 def __init__(self, node, mask, threshold):
     self.node = node.createChild("resistance")
     position = [0] * len(mask)
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec1' +
                                        template_suffix,
                                        name='dofs',
                                        position=concat(position))
     self.mapping = self.node.createObject('MaskMapping',
                                           dofs=concat(mask))
     self.compliance = self.node.createObject('UniformCompliance',
                                              name='compliance',
                                              compliance=0,
                                              isCompliance=1)
     self.type = self.node.createObject('VelocityConstraintValue',
                                        velocities=concat([0]))
     self.constraint = self.node.createObject('ResistanceConstraint',
                                              threshold=threshold)
Example #29
0
        def __init__(self, node, mask, target, compliance, isCompliance=False):
            self.node = node.createChild( "controller-mask" )

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
            self.node.createObject('MaskMapping', dofs=concat(mask))
            self.nodeTarget = self.node.createChild( "controller-target" )
            self.nodeTarget.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
            self.mapping = self.nodeTarget.createObject('DifferenceFromTargetMapping', targets=concat(target))
            self.compliance = self.nodeTarget.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=isCompliance)
Example #30
0
        def __init__(self, node, mask, forces):
            self.node = node.createChild("controller")

            size = sum(x != 0 for x in mask)

            position = [0] * size
            points = numpy.arange(0, size, 1)

            self.dofs = self.node.createObject(
                "MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
            )
            self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
            self.force = self.node.createObject(
                "ConstantForceField",
                template="Vec1" + template_suffix,
                forces=concat(forces),
                points=concat(points.tolist()),
            )
Example #31
0
 def addMotor(self, forces=[0, 0, 0, 0, 0, 0]):
     ## adding a constant force/torque at the offset location (that could be driven by a controller to simulate a motor)
     return self.node.createObject(
         "ConstantForceField",
         template="Rigid3" + template_suffix,
         name="motor",
         points="0",
         forces=concat(forces),
     )
Example #32
0
 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
Example #33
0
        def __init__(self, node, mask, target, compliance, isCompliance=False):
            self.node = node.createChild("controller-mask")

            self.dofs = self.node.createObject('MechanicalObject',
                                               template='Vec1' +
                                               template_suffix,
                                               name='dofs')
            self.node.createObject('MaskMapping', dofs=concat(mask))
            self.nodeTarget = self.node.createChild("controller-target")
            self.nodeTarget.createObject('MechanicalObject',
                                         template='Vec1' + template_suffix,
                                         name='dofs')
            self.mapping = self.nodeTarget.createObject(
                'DifferenceFromTargetMapping', targets=concat(target))
            self.compliance = self.nodeTarget.createObject(
                'UniformCompliance',
                name='compliance',
                compliance=compliance,
                isCompliance=isCompliance)
Example #34
0
 def addSpring(self, stiffness1, stiffness2):
     axis1 = (self.normal + 1) % 3
     axis2 = (self.normal + 2) % 3
     if axis1 > axis2:
         axis1, axis2 = axis2, axis1
     mask = [0] * 6
     mask[axis1] = stiffness1
     mask[axis2] = stiffness2
     return self.node.createObject('DiagonalStiffness',
                                   stiffness=concat(mask))
Example #35
0
 def setManually(self, offset = [0,0,0,0,0,0,1], mass = 1, inertia = [1,1,1], inertia_forces = False ):
         ## create the rigid body by manually giving its inertia
         self.frame = Rigid.Frame( offset )
         self.dofs = self.frame.insert( self.node, name='dofs' )
         self.mass = self.node.createObject('RigidMass',
                                 template = 'Rigid',
                                 name = 'mass',
                                 mass = mass,
                                 inertia = concat(inertia),
                                 inertia_forces = inertia_forces )
Example #36
0
        def __init__(self, node, mask, forces):
            self.node = node.createChild("controller")

            size = sum(x != 0 for x in mask)

            position = [0] * size
            points = numpy.arange(0, size, 1)

            self.dofs = self.node.createObject('MechanicalObject',
                                               template='Vec1' +
                                               template_suffix,
                                               name='dofs',
                                               position=concat(position))
            self.mapping = self.node.createObject('MaskMapping',
                                                  dofs=concat(mask))
            self.force = self.node.createObject('ConstantForceField',
                                                template='Vec1' +
                                                template_suffix,
                                                forces=concat(forces),
                                                points=concat(points.tolist()))
Example #37
0
 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")
Example #38
0
        def __init__(self, node, mask, target, compliance, isCompliance=False):
            self.node = node.createChild("controller-mask")

            self.dofs = self.node.createObject("MechanicalObject", template="Vec1" + template_suffix, name="dofs")
            self.node.createObject("MaskMapping", dofs=concat(mask))
            self.nodeTarget = self.node.createChild("controller-target")
            self.nodeTarget.createObject("MechanicalObject", template="Vec1" + template_suffix, name="dofs")
            self.mapping = self.nodeTarget.createObject("DifferenceFromTargetMapping", targets=concat(target))
            self.compliance = self.nodeTarget.createObject(
                "UniformCompliance", name="compliance", compliance=compliance, isCompliance=isCompliance
            )
Example #39
0
 def __init__(self, node, compliances):
     self.node = node.createChild("constraint")
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec6' +
                                        template_suffix,
                                        name='dofs')
     self.mapping = self.node.createObject('IdentityMapping')
     self.compliance = self.node.createObject(
         'DiagonalCompliance',
         name='compliance',
         compliance=concat(compliances))
Example #40
0
 def __init__(self, node, mask, compliance):
     self.node = node.createChild("constraint")
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec1d',
                                        name='dofs')
     self.mapping = self.node.createObject('MaskMapping',
                                           dofs=concat(mask))
     self.compliance = self.node.createObject('UniformCompliance',
                                              name='compliance',
                                              compliance=compliance)
     self.type = self.node.createObject('Stabilization')
Example #41
0
        def __init__(self, node, masks, limits, compliances):
            self.node = node.createChild("limits")

            set = []
            position = [0] * len(masks)
            offset = []

            for i in range(len(masks)):
                set = set + [0] + masks[i]
                offset.append(limits[i])

            self.dofs = self.node.createObject(
                "MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
            )
            self.mapping = self.node.createObject("ProjectionMapping", set=concat(set), offset=concat(offset))
            self.compliance = self.node.createObject(
                "DiagonalCompliance", name="compliance", compliance=concat(compliances)
            )
            # self.type = self.node.createObject('Stabilization')
            self.constraint = self.node.createObject("UnilateralConstraint")
Example #42
0
 def addSpring(self, stiffness1, stiffness2):
     axis1 = (self.normal + 1) % 3
     axis2 = (self.normal + 2) % 3
     if axis1 > axis2:
         axis1, axis2 = axis2, axis1
     mask = [0] * 6
     mask[axis1] = 1.0 / stiffness1
     mask[axis2] = 1.0 / stiffness2
     return self.node.createObject('DiagonalCompliance',
                                   template="Rigid",
                                   isCompliance="0",
                                   compliance=concat(mask))
Example #43
0
 def addSpring(self, stiffness1, stiffness2):
     index1 = 3 + (self.axis + 1) % 3
     index2 = 3 + (self.axis + 2) % 3
     if index1 > index2:
         index1, index2 = index2, index1
     mask = [0] * 6
     mask[index1] = 1.0 / stiffness1
     mask[index2] = 1.0 / stiffness2
     return self.node.createObject('DiagonalCompliance',
                                   template="Rigid",
                                   isCompliance="0",
                                   compliance=concat(mask))
Example #44
0
 def __init__(self, node, mask, compliance, isCompliance):
     self.node = node.createChild("constraint")
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec1' +
                                        template_suffix,
                                        name='dofs')
     self.mapping = self.node.createObject('MaskMapping',
                                           dofs=concat(mask))
     self.compliance = self.node.createObject('UniformCompliance',
                                              name='compliance',
                                              compliance=compliance,
                                              isCompliance=isCompliance)
Example #45
0
 def addSpring( self, translation_stiffness=None, rotation_stiffness=None ):
     stiffnesses=[]
     for i in range(0,3):
         if not self.mask[i]:
             stiffnesses.append( float(translation_stiffness) )
         else:
             stiffnesses.append(0)
     for i in range(3,6):
         if not self.mask[i]:
             stiffnesses.append( float(rotation_stiffness) )
         else:
             stiffnesses.append(0)
     return self.node.createObject('DiagonalStiffness', stiffness=concat(stiffnesses))
Example #46
0
    def setFromRigidInfo(self, info, offset = [0,0,0,0,0,0,1], inertia_forces = False) :
        self.framecom = Frame.Frame()
        self.framecom.rotation = info.inertia_rotation
        self.framecom.translation = info.com

        self.frame = Frame.Frame(offset) * self.framecom

        self.dofs = self.frame.insert( self.node, name = 'dofs', template="Rigid3"+template_suffix )
        self.mass = self.node.createObject('RigidMass',
                                name = 'mass',
                                mass = info.mass,
                                inertia = concat(info.diagonal_inertia),
                                inertia_forces = inertia_forces )
Example #47
0
 def __init__(self, node, compliances):
     self.node = node.createChild("constraint")
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec6d',
                                        name='dofs')
     self.mapping = self.node.createObject('IdentityMapping',
                                           template='Vec6d,Vec6d')
     self.compliance = self.node.createObject(
         'DiagonalCompliance',
         template="Vec6d",
         name='compliance',
         compliance=concat(compliances))
     self.type = self.node.createObject('Stabilization')
Example #48
0
    def __init__(self, name, node1, node2, stiffnesses=[0, 0, 0, 0, 0, 0], index1=0, index2=0):
        self.node = node1.createChild(name)
        self.dofs = self.node.createObject(
            "MechanicalObject", template="Vec6" + template_suffix, name="dofs", position="0 0 0 0 0 0"
        )
        input = []  # @internal
        input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
        input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")

        self.mapping = self.node.createObject(
            "RigidJointMultiMapping",
            name="mapping",
            input=concat(input),
            output="@dofs",
            pairs=str(index1) + " " + str(index2),
            geometricStiffness=geometric_stiffness,
        )
        compliances = 1.0 / numpy.array(stiffnesses)
        self.compliance = self.node.createObject(
            "DiagonalCompliance", name="compliance", compliance=concat(compliances), isCompliance=0
        )
        node2.addChild(self.node)
Example #49
0
    def insert(self, parent):
        res = Joint.insert(self, parent)

        if self.lower_limit == None and self.upper_limit == None:
            return res

        limit = res.createChild('limit')

        dofs = limit.createObject('MechanicalObject', template='Vec1')
        map = limit.createObject('ProjectionMapping', template='Vec6,Vec1')

        limit.createObject('UniformCompliance',
                           template='Vec1',
                           compliance='0')
        limit.createObject('UnilateralConstraint')

        # don't stabilize as we need to detect violated
        # constraints first
        # limit.createObject('Stabilization');

        set = []
        position = []
        offset = []

        if self.lower_limit != None:
            set = set + [0] + self.dofs
            position.append(0)
            offset.append(self.lower_limit)

        if self.upper_limit != None:
            set = set + [0] + vec.minus(self.dofs)
            position.append(0)
            offset.append(-self.upper_limit)

        map.set = concat(set)
        map.offset = concat(offset)
        dofs.position = concat(position)

        return res
Example #50
0
 def __init__(self,
              name,
              node1,
              node2,
              mask,
              compliance=0,
              index1=0,
              index2=0,
              isCompliance=True):
     self.node = node1.createChild(name)
     self.mask = mask
     self.dofs = self.node.createObject('MechanicalObject',
                                        template='Vec6' + template_suffix,
                                        name='dofs',
                                        position='0 0 0 0 0 0')
     input = []  # @internal
     input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
     if not node2 is None:
         input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
         self.mapping = self.node.createObject(
             'RigidJointMultiMapping',
             name='mapping',
             input=concat(input),
             output='@dofs',
             pairs=str(index1) + " " + str(index2),
             geometricStiffness=geometric_stiffness)
         node2.addChild(self.node)
     else:
         self.mapping = self.node.createObject(
             'RigidJointMapping',
             name='mapping',
             input=concat(input),
             output='@dofs',
             pairs=str(index1) + " " + str(index2),
             geometricStiffness=geometric_stiffness)
     self.constraint = GenericRigidJoint.Constraint(self.node, mask,
                                                    compliance,
                                                    isCompliance)
Example #51
0
 def addSpring(self, translation_stiffness=None, rotation_stiffness=None):
     stiffnesses = []
     for i in range(0, 3):
         if not self.mask[i]:
             stiffnesses.append(float(translation_stiffness))
         else:
             stiffnesses.append(0)
     for i in range(3, 6):
         if not self.mask[i]:
             stiffnesses.append(float(rotation_stiffness))
         else:
             stiffnesses.append(0)
     return self.node.createObject('DiagonalStiffness',
                                   stiffness=concat(stiffnesses))
Example #52
0
        def insert(self, parent):
                res = Joint.insert(self, parent)

                if self.lower_limit == None and self.upper_limit == None:
                        return res
                
                limit = res.createChild('limit')

                dofs = limit.createObject('MechanicalObject', template = 'Vec1')
                map = limit.createObject('ProjectionMapping', template = 'Vec6,Vec1' )

                limit.createObject('UniformCompliance', template = 'Vec1', compliance = '0' )
                limit.createObject('UnilateralConstraint');

                # don't stabilize as we need to detect violated
                # constraints first
                # limit.createObject('Stabilization');

                set = []
                position = []
                offset = []

                if self.lower_limit != None:
                        set = set + [0] + self.dofs
                        position.append(0)
                        offset.append(self.lower_limit)

                if self.upper_limit != None:
                        set = set + [0] + vec.minus(self.dofs)
                        position.append(0)
                        offset.append(- self.upper_limit)
                
                map.set = concat(set)
                map.offset = concat(offset)
                dofs.position = concat(position)

                return res
Example #53
0
 def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, rest_lenght=-1):
     self.node = node1.createChild(name)
     self.dofs = self.node.createObject("MechanicalObject", template="Rigid3" + template_suffix, name="dofs")
     input = []  # @internal
     input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
     input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
     self.mapping = self.node.createObject(
         "SubsetMultiMapping",
         name="mapping",
         input=concat(input),
         output="@dofs",
         indexPairs="0 " + str(index1) + " 1 " + str(index2),
     )
     self.constraint = DistanceRigidJoint.Constraint(self.node, compliance, rest_lenght)
     node2.addChild(self.node)
Example #54
0
        def __init__(self, node, masks, limits, compliances):
            self.node = node.createChild( "limits" )

            set = []
            position = [0] * len(masks)
            offset = []

            for i in range(len(masks)):
                set = set + [0] + masks[i]
                offset.append(limits[i])

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1d', name='dofs', position=concat(position))
            self.mapping = self.node.createObject('ProjectionMapping', set=concat(set), offset=concat(offset))
            self.compliance = self.node.createObject('DiagonalCompliance', name='compliance', compliance=concat(compliances))
            self.type = self.node.createObject('ConstraintValue') # cannot be stabilized for now
            self.constraint = self.node.createObject('UnilateralConstraint')
Example #55
0
        def __init__(self, node, masks, limits, compliance):
            self.node = node.createChild( "limits" )

            set = []
            position = [0] * len(masks)
            offset = []

            for i in range(len(masks)):
                set = set + [0] + masks[i]
                offset.append(limits[i])

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
            self.mapping = self.node.createObject('ProjectionMapping', set=concat(set), offset=concat(offset))
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance)
            # self.type = self.node.createObject('Stabilization')
            self.constraint = self.node.createObject('UnilateralConstraint')
Example #56
0
        def setFromMesh(self, filepath, density = 1000.0, offset = [0,0,0,0,0,0,1], scale3d=[1,1,1], inertia_forces = False ):
                ## create the rigid body from a mesh (inertia and com are automatically computed)
                info = Rigid.generate_rigid(filepath, density, scale3d)

                self.framecom = Rigid.Frame()
                self.framecom.rotation = info.inertia_rotation
                self.framecom.translation = info.com

                self.frame = Rigid.Frame(offset) * self.framecom

                self.dofs = self.frame.insert( self.node, name = 'dofs' )
                self.mass = self.node.createObject('RigidMass',
                                        template = 'Rigid',
                                        name = 'mass',
                                        mass = info.mass,
                                        inertia = concat(info.diagonal_inertia.tolist()),
                                        inertia_forces = inertia_forces )
Example #57
0
 def __init__(self, name, node1, node2, compliances=[0, 0, 0, 0, 0, 0], index1=0, index2=0):
     self.node = node1.createChild(name)
     self.dofs = self.node.createObject(
         "MechanicalObject", template="Vec6" + template_suffix, name="dofs", position="0 0 0 0 0 0"
     )
     input = []  # @internal
     input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
     input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
     self.mapping = self.node.createObject(
         "RigidJointMultiMapping",
         name="mapping",
         input=concat(input),
         output="@dofs",
         pairs=str(index1) + " " + str(index2),
         geometricStiffness=geometric_stiffness,
     )
     self.constraint = CompleteRigidJoint.Constraint(
         self.node, compliances
     )  # the constraint compliance cannot be in the same branch as eventual limits...
     node2.addChild(self.node)
Example #58
0
 def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True):
     # GenericRigidJoint.__init__(self, name, node1, node2, [1,1,1,0,0,0], compliance, index1, index2) # GenericRigidJoint can work but is way overkill
     self.node = node1.createChild(name)
     self.dofs = self.node.createObject(
         "MechanicalObject", template="Vec3" + template_suffix, name="dofs", position="0 0 0"
     )
     input = []  # @internal
     input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
     input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
     self.mapping = self.node.createObject(
         "DifferenceMultiMapping",
         name="mapping",
         input=concat(input),
         output="@dofs",
         pairs=str(index1) + " " + str(index2),
     )
     self.compliance = self.node.createObject(
         "UniformCompliance", name="compliance", compliance=compliance, isCompliance=isCompliance
     )
     node2.addChild(self.node)
Example #59
0
 def __init__(self, name, node1, node2, stiffnesses=[0,0,0,0,0,0], index1=0, index2=0):
         self.node = node1.createChild( name )
         self.dofs = self.node.createObject('MechanicalObject', template = 'Vec6'+template_suffix, name = 'dofs', position = '0 0 0 0 0 0' )
         input = [] # @internal
         input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
         input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
         
         self.mapping = self.node.createObject('RigidJointMultiMapping',  name = 'mapping', input = concat(input), output = '@dofs', pairs = str(index1)+" "+str(index2),
                                               geometricStiffness = geometric_stiffness)
         compliances = 1./numpy.array(stiffnesses);
         self.compliance = self.node.createObject('DiagonalCompliance', name='compliance', compliance=concat(compliances), isCompliance=0)
         node2.addChild( self.node )
Example #60
0
 def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, rest_lenght=-1 ):
     self.node = node1.createChild( name )
     self.dofs = self.node.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs' )
     input = [] # @internal
     input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
     input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
     self.mapping = self.node.createObject('SubsetMultiMapping', name='mapping', input = concat(input), output = '@dofs', indexPairs="0 "+str(index1)+" 1 "+str(index2) )
     self.constraint = DistanceRigidJoint.Constraint(self.node, compliance, rest_lenght)
     node2.addChild( self.node )