Launch meshcat visualizer node when using Simple

go2_simulation/simple_wrapper.py

@@ -1,10 +1,9 @@
 import numpy as np
-from go2_description import loadGo2
-from go2_description import GO2_DESCRIPTION_URDF_PATH, GO2_DESCRIPTION_PACKAGE_DIR
 import hppfcl
 import pinocchio as pin
 import simple
 from go2_simulation.abstract_wrapper import AbstractSimulatorWrapper
+from go2_simulation.utils import addFloor, addSystemCollisionPairs, setPhysicsProperties, removeBVHModelsIfAny, loadGo2Model
 
 class SimpleSimulator:
     def __init__(self, model, geom_model, visual_model, q0, args):
@@ -97,103 +96,10 @@ def view_state(self, q):
         self.vizer.display(q)
 
 
-def setPhysicsProperties(
-    geom_model: pin.GeometryModel, material: str, compliance: float
-):
-    for gobj in geom_model.geometryObjects:
-        if material == "ice":
-            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE
-        elif material == "plastic":
-            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC
-        elif material == "wood":
-            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD
-        elif material == "metal":
-            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL
-        elif material == "concrete":
-            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE
-
-        # Compliance
-        gobj.physicsMaterial.compliance = compliance
-
-
-def removeBVHModelsIfAny(geom_model: pin.GeometryModel):
-    for gobj in geom_model.geometryObjects:
-        gobj: pin.GeometryObject
-        bvh_types = [hppfcl.BV_OBBRSS, hppfcl.BV_OBB, hppfcl.BV_AABB]
-        ntype = gobj.geometry.getNodeType()
-        if ntype in bvh_types:
-            gobj.geometry.buildConvexHull(True, "Qt")
-            gobj.geometry = gobj.geometry.convex
-
-
-def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel):
-    # Collision object
-    # floor_collision_shape = hppfcl.Box(10, 10, 2)
-    # M = pin.SE3(np.eye(3), np.zeros(3))
-    # M.translation = np.array([0.0, 0.0, -(1.99 / 2.0)])
-    floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0)
-    # floor_collision_shape = hppfcl.Plane(0, 0, 1, 0)
-    # floor_collision_shape.setSweptSphereRadius(0.5)
-    M = pin.SE3.Identity()
-    floor_collision_object = pin.GeometryObject("floor", 0, 0, M, floor_collision_shape)
-    geom_model.addGeometryObject(floor_collision_object)
-
-    # Visual object
-    floor_visual_shape = hppfcl.Box(10, 10, 0.01)
-    floor_visual_object = pin.GeometryObject(
-        "floor", 0, 0, pin.SE3.Identity(), floor_visual_shape
-    )
-    visual_model.addGeometryObject(floor_visual_object)
-
-def addSystemCollisionPairs(model, geom_model, qref):
-    """
-    Add the right collision pairs of a model, given qref.
-    qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision
-    in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered
-    to always be in collision.
-    """
-    data = model.createData()
-    geom_data = geom_model.createData()
-    pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref)
-    geom_model.removeAllCollisionPairs()
-    num_col_pairs = 0
-    for i in range(len(geom_model.geometryObjects)):
-        for j in range(i+1, len(geom_model.geometryObjects)):
-            # Don't add collision pair if same object
-            if i != j:
-                gobj_i: pin.GeometryObject = geom_model.geometryObjects[i]
-                gobj_j: pin.GeometryObject = geom_model.geometryObjects[j]
-                if gobj_i.name == "floor" or gobj_j.name == "floor":
-                    num_col_pairs += 1
-                    col_pair = pin.CollisionPair(i, j)
-                    geom_model.addCollisionPair(col_pair)
-                else:
-                    if gobj_i.parentJoint != gobj_j.parentJoint or gobj_i.parentJoint == 0:
-                        if gobj_i.parentJoint != model.parents[gobj_j.parentJoint] and gobj_j.parentJoint != model.parents[gobj_i.parentJoint] or gobj_i.parentJoint == 0 or gobj_j.parentJoint == 0:
-                            # Compute collision between the geometries. Only add the collision pair if there is no collision.
-                            M1 = geom_data.oMg[i]
-                            M2 = geom_data.oMg[j]
-                            colreq = hppfcl.CollisionRequest()
-                            colreq.security_margin = 1e-2 # 1cm of clearance
-                            colres = hppfcl.CollisionResult()
-                            hppfcl.collide(gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres)
-                            if not colres.isCollision():
-                                num_col_pairs += 1
-                                col_pair = pin.CollisionPair(i, j)
-                                geom_model.addCollisionPair(col_pair)
-    print("Num col pairs = ", num_col_pairs)
-
 class SimpleWrapper(AbstractSimulatorWrapper):
     def __init__(self, node, timestep):
         ########################## Load robot model and geometry
-        robot = loadGo2()
-        self.rmodel = robot.model
-
-        with open(GO2_DESCRIPTION_URDF_PATH, 'r') as file:
-            file_content = file.read()
-
-        self.geom_model = pin.GeometryModel()
-        pin.buildGeomFromUrdfString(self.rmodel, file_content, pin.GeometryType.VISUAL, self.geom_model, GO2_DESCRIPTION_PACKAGE_DIR)
+        self.rmodel, self.geom_model = loadGo2Model()
 
         # Load parameters from node
         self.params = {

go2_simulation/utils.py

@@ -0,0 +1,107 @@
+import hppfcl
+
+import pinocchio as pin
+
+from go2_description import loadGo2
+from go2_description import GO2_DESCRIPTION_URDF_PATH, GO2_DESCRIPTION_PACKAGE_DIR
+
+
+def loadGo2Model() -> tuple[pin.Model, pin.GeometryModel]:
+    robot = loadGo2()
+    model = robot.model
+
+    with open(GO2_DESCRIPTION_URDF_PATH, 'r') as file:
+        file_content = file.read()
+
+    geom_model = pin.GeometryModel()
+    pin.buildGeomFromUrdfString(model, file_content, pin.GeometryType.VISUAL, geom_model, GO2_DESCRIPTION_PACKAGE_DIR)
+
+    return model, geom_model
+
+
+def setPhysicsProperties(
+    geom_model: pin.GeometryModel, material: str, compliance: float
+):
+    for gobj in geom_model.geometryObjects:
+        if material == "ice":
+            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.ICE
+        elif material == "plastic":
+            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.PLASTIC
+        elif material == "wood":
+            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.WOOD
+        elif material == "metal":
+            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.METAL
+        elif material == "concrete":
+            gobj.physicsMaterial.materialType = pin.PhysicsMaterialType.CONCRETE
+
+        # Compliance
+        gobj.physicsMaterial.compliance = compliance
+
+
+def removeBVHModelsIfAny(geom_model: pin.GeometryModel):
+    for gobj in geom_model.geometryObjects:
+        gobj: pin.GeometryObject
+        bvh_types = [hppfcl.BV_OBBRSS, hppfcl.BV_OBB, hppfcl.BV_AABB]
+        ntype = gobj.geometry.getNodeType()
+        if ntype in bvh_types:
+            gobj.geometry.buildConvexHull(True, "Qt")
+            gobj.geometry = gobj.geometry.convex
+
+
+def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel):
+    # Collision object
+    # floor_collision_shape = hppfcl.Box(10, 10, 2)
+    # M = pin.SE3(np.eye(3), np.zeros(3))
+    # M.translation = np.array([0.0, 0.0, -(1.99 / 2.0)])
+    floor_collision_shape = hppfcl.Halfspace(0, 0, 1, 0)
+    # floor_collision_shape = hppfcl.Plane(0, 0, 1, 0)
+    # floor_collision_shape.setSweptSphereRadius(0.5)
+    M = pin.SE3.Identity()
+    floor_collision_object = pin.GeometryObject("floor", 0, 0, M, floor_collision_shape)
+    geom_model.addGeometryObject(floor_collision_object)
+
+    # Visual object
+    floor_visual_shape = hppfcl.Box(10, 10, 0.01)
+    floor_visual_object = pin.GeometryObject(
+        "floor", 0, 0, pin.SE3.Identity(), floor_visual_shape
+    )
+    visual_model.addGeometryObject(floor_visual_object)
+
+
+def addSystemCollisionPairs(model, geom_model, qref):
+    """
+    Add the right collision pairs of a model, given qref.
+    qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision
+    in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered
+    to always be in collision.
+    """
+    data = model.createData()
+    geom_data = geom_model.createData()
+    pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref)
+    geom_model.removeAllCollisionPairs()
+    num_col_pairs = 0
+    for i in range(len(geom_model.geometryObjects)):
+        for j in range(i+1, len(geom_model.geometryObjects)):
+            # Don't add collision pair if same object
+            if i != j:
+                gobj_i: pin.GeometryObject = geom_model.geometryObjects[i]
+                gobj_j: pin.GeometryObject = geom_model.geometryObjects[j]
+                if gobj_i.name == "floor" or gobj_j.name == "floor":
+                    num_col_pairs += 1
+                    col_pair = pin.CollisionPair(i, j)
+                    geom_model.addCollisionPair(col_pair)
+                else:
+                    if gobj_i.parentJoint != gobj_j.parentJoint or gobj_i.parentJoint == 0:
+                        if gobj_i.parentJoint != model.parents[gobj_j.parentJoint] and gobj_j.parentJoint != model.parents[gobj_i.parentJoint] or gobj_i.parentJoint == 0 or gobj_j.parentJoint == 0:
+                            # Compute collision between the geometries. Only add the collision pair if there is no collision.
+                            M1 = geom_data.oMg[i]
+                            M2 = geom_data.oMg[j]
+                            colreq = hppfcl.CollisionRequest()
+                            colreq.security_margin = 1e-2 # 1cm of clearance
+                            colres = hppfcl.CollisionResult()
+                            hppfcl.collide(gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres)
+                            if not colres.isCollision():
+                                num_col_pairs += 1
+                                col_pair = pin.CollisionPair(i, j)
+                                geom_model.addCollisionPair(col_pair)
+    print("Num col pairs = ", num_col_pairs)

go2_simulation/viewer_node.py

@@ -0,0 +1,66 @@
+import rclpy
+import meshcat
+
+import numpy as np
+import pinocchio as pin
+
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from unitree_go.msg import LowState
+from pinocchio.visualize import MeshcatVisualizer
+from go2_simulation.utils import loadGo2Model, addFloor
+
+
+class ViewerNode(Node,):
+    def __init__(self):
+        Node.__init__(self, "viewer")
+
+        model, geom_model = loadGo2Model()
+        visual_model = geom_model.copy()
+        addFloor(geom_model, visual_model)
+
+        q = pin.neutral(model)
+
+        viewer = meshcat.Visualizer()
+        viewer.delete()
+        self.vizer: MeshcatVisualizer = MeshcatVisualizer(model, geom_model, visual_model)
+        self.vizer.initViewer(viewer=viewer, open=False, loadModel=True)
+        self.vizer.display(q)
+
+        self.create_subscription(LowState, "/lowstate", self.receive_state_cb, 10)
+        self.last_state_msg = LowState()
+
+        self.create_subscription(Odometry, "/odometry/filtered", self.receive_odometry_cb, 10)
+        self.last_odometry_msg = Odometry()
+
+        fps = 30
+        period = 1./fps  # seconds
+        self.timer = self.create_timer(period, self.update)
+
+    def update(self):
+        joint_pos = [self.last_state_msg.motor_state[i].q for i in range(12)]
+        pos_msg = self.last_odometry_msg.pose.pose.position
+        ff_pos = [pos_msg.x, pos_msg.y, pos_msg.z]
+        quat_msg = self.last_odometry_msg.pose.pose.orientation
+        ff_quat = [quat_msg.x, quat_msg.y, quat_msg.z, quat_msg.w]
+        q = np.array(ff_pos + ff_quat + joint_pos)
+        self.vizer.display(q)
+
+    def receive_state_cb(self, msg):
+        self.last_state_msg = msg
+
+    def receive_odometry_cb(self, msg):
+        self.last_odometry_msg = msg
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ViewerNode()
+
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

launch/launch_sim.launch.py

@@ -1,6 +1,8 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
+from launch.substitutions import PythonExpression
 from launch_ros.actions import Node
 
 def generate_launch_description():
@@ -11,6 +13,8 @@ def generate_launch_description():
         description="Which simulator to use 'pybullet' or 'simple'"
     )
 
+    simulator = LaunchConfiguration('simulator')
+
     # Node configuration
     go2_simulation_node = Node(
         package='go2_simulation',  # Replace with the actual package name
@@ -19,13 +23,22 @@ def generate_launch_description():
         output='screen',
         parameters=[
             {
-                'simulator': LaunchConfiguration('simulator'),
+                'simulator': simulator,
             }
         ]
     )
 
+    go2_viewer_node = Node(
+        package='go2_simulation',
+        executable='viewer_node',
+        name='viewer_node',
+        output='screen',
+        condition=IfCondition(PythonExpression(["'", simulator, "' == 'simple'"]))
+    )
+
     # Launch description
     return LaunchDescription([
         simulator_arg,
-        go2_simulation_node
+        go2_simulation_node,
+        go2_viewer_node
     ])

setup.py

@@ -23,6 +23,7 @@
     entry_points={
         'console_scripts': [
             'simulation_node = go2_simulation.simulation_node:main',
+            'viewer_node = go2_simulation.viewer_node:main',
         ],
     },
 )