ADD a prefab spot [DRAFT]

examples/stlib/PrefabScene_beginner.py

@@ -0,0 +1,30 @@
+from stlib.entities.rigid import Rigid
+from stlib.entities.deformable import Deformable
+from stlib.geometry.cube import CubeParameters
+from stlib.geometry.file import FileParameters
+from splib.simulation.headers import setupLagrangianCollision
+from splib.simulation.linear_solvers import addLinearSolver
+from splib.simulation.ode_solvers import addImplicitODE
+
+#To be added in splib
+def addSolvers(root):
+    addLinearSolver(root)
+    addImplicitODE(root)
+    root.addObject("LinearSolverConstraintCorrection", linearsolver="@LinearSolver")
+
+def createScene(root):
+    root.gravity = [0, 0, -9.81]
+
+    setupLagrangianCollision(root)
+    addSolvers(root)
+
+    rigidParams = Rigid.getParameters()
+    rigidParams.geometry = CubeParameters([0, 0, 0], 1, 3)
+    root.add(Rigid,rigidParams)
+
+    deformableParams = Deformable.getParameters()
+    #Add transformation somewhere here
+    deformableParams.geometry = FileParameters("SofaScene/Logo.vtk")
+    root.add(Deformable,deformableParams)
+
+    return root

examples/stlib/PrefabScene_expert.py

@@ -0,0 +1,14 @@
+from splib.helper import exportScene
+from stlib.misc.entity import Entity
+
+
+def createScene(root):
+    Entity.Deformable(Mesh,MechaProperties)
+
+    return root
+
+
+if __name__=="__main__":
+    root = exportScene()
+    createScene(root)
+    pass

examples/stlib/PrefabScene_intermediate.py

@@ -0,0 +1,8 @@
+from splib.helper import exportScene
+from stlib.misc.entity import Entity
+
+
+def createScene(root):
+
+
+    return root

live-example/models/compliancerobotics/robots/spot/__entity__.py

@@ -0,0 +1,59 @@
+from stlib.entities import Entity
+from compliancerobotics.robots.spot.__parameters__ import SpotRobotParameters
+import numpy as np
+
+class SpotRobot(Entity):
+    """Boston dynamic spot robots"""
+    @staticmethod
+    def get_asset(name) -> str:
+        import os
+        return os.path.join(os.path.join(os.path.dirname(__file__), "assets"), name)
+
+    @staticmethod
+    def getParameters(**kwargs) -> SpotRobotParameters:
+        return SpotRobotParameters(**kwargs)
+
+    def __init__(self, parameter : SpotRobotParameters = None, **kwargs):
+        if parameter is None:
+            parameter = SpotRobotParameters(**kwargs)
+        Entity.__init__(self, parameter)
+
+        self.create(parameter)
+
+    def create(self, parameter):
+        # Robot node
+        settings = self.addChild('settings')
+        settings.addObject("RequiredPlugin", name="SoftRobots") 
+        settings.addObject("RequiredPlugin", name="Sofa.RigidBodyDynamics") # Needed to use components [URDFModelLoader]
+        settings.addObject('RequiredPlugin', name='Sofa.Component.Mapping.NonLinear') # Needed to use components [RigidMapping]  
+        settings.addObject('RequiredPlugin', name='Sofa.Component.Mass') # Needed to use components [UniformMass]
+        settings.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Constant') # Needed to use components [MeshTopology]  
+        settings.addObject('RequiredPlugin', name='Sofa.GL.Component.Rendering3D') # Needed to use components [OglModel]  
+
+        self.addObject('URDFModelLoader', 
+                        filename=SpotRobot.get_asset("model.urdf"), 
+                        modelDirectory=SpotRobot.get_asset(""), 
+                        useFreeFlyerRootJoint=False, 
+                        printLog=False, 
+                        addCollision=False, 
+                        addJointsActuators=False)
+
+        robot = self.getChild("Robot")
+        robot.name = "model"
+        mechanical = robot.Model.getMechanicalState()
+        mechanical.showObject = False
+        mechanical.showObjectScale = 0.01
+        mechanical.drawMode = 0 
+
+        # Direct problem
+        names = robot.Joints.children
+        positions = np.copy(robot.getMechanicalState().position.value)
+        for i in range(len(positions)):
+            jointName = names[i+1].name.value
+            value = 0 if jointName not in parameter.spot_ctrl_joint_infos else parameter.spot_ctrl_joint_infos[jointName].pos_desired
+            positions[i] = value
+            joint = robot.addObject('JointConstraint', template='Vec1', name='joint' + jointName, index=i, 
+                                valueType="angle", 
+                                value=value
+                                )
+

live-example/models/compliancerobotics/robots/spot/__init__.py

@@ -0,0 +1,2 @@
+from __entity__ import SpotRobot
+from __scene__ import createScene as ExampleScene 

live-example/models/compliancerobotics/robots/spot/__parameters__.py

@@ -0,0 +1,28 @@
+from stlib.core.baseParameters import BaseParameters, dataclasses
+
+@dataclasses.dataclass
+class ControlJointValue:
+    Kp: float = 0.0
+    Kd: float = 0.0
+    Ki: float = 0.0
+    i_clamp: float = 0.0
+    pos_desired: float = 0.0
+    vel_desired: float = 0.0
+
+@dataclasses.dataclass
+class SpotRobotParameters(BaseParameters): 
+    name : str = "SpotRobot"
+
+    spot_ctrl_joint_infos = {
+        "fl.hy": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.91, 0.0),
+        "fl.kn": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, -1.52, 0.0),
+        "fr.hy": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.91, 0.0),
+        "fr.kn": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, -1.52, 0.0),
+        "hl.hy": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.91, 0.0),
+        "hl.kn": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, -1.52, 0.0),
+        "hr.hy": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.91, 0.0),
+        "hr.kn": ControlJointValue(  10000.0, 0.01, 1.0, 14.0, -1.52, 0.0),
+    }
+
+    def toDict(self):
+        return dataclasses.asdict(self)