added dual-arm end-effector self-collision avoidance done through pink. now...

added dual-arm end-effector self-collision avoidance done through pink. now have a hang of pink, will add in self-collision avoidance for the whole body. first will probably look into posture tasks and similar. key point is pink works within smc

examples/cartesian_space/dual_arm_clik.py

@@ -28,17 +28,17 @@ if __name__ == "__main__":
     args = get_args()
     robot = getRobotFromArgs(args)
     if args.randomly_generate_goal:
-        T_w_goal = getRandomlyGeneratedGoal(args)
+        T_w_absgoal = getRandomlyGeneratedGoal(args)
         if args.visualizer:
-            robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal})
+            robot.visualizer_manager.sendCommand({"Mgoal": T_w_absgoal})
     else:
-        T_w_goal = defineGoalPointCLI(robot)
-        T_w_goal.rotation = np.eye(3)
-    T_absgoal_lgoal = pin.SE3.Identity()
-    T_absgoal_lgoal.translation[1] = 0.1
-    T_absgoal_rgoal = pin.SE3.Identity()
-    T_absgoal_rgoal.translation[1] = -0.1
-    moveLDualArm(args, robot, T_w_goal, T_absgoal_lgoal, T_absgoal_rgoal)
+        T_w_absgoal = defineGoalPointCLI(robot)
+        T_w_absgoal.rotation = np.eye(3)
+    T_absgoal_l = pin.SE3.Identity()
+    T_absgoal_l.translation[1] = 0.1
+    T_absgoal_r = pin.SE3.Identity()
+    T_absgoal_r.translation[1] = -0.1
+    moveLDualArm(args, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r)
 
     if args.real:
         robot.stopRobot()

examples/cartesian_space/yumi_self_collision.py

-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-#
-# SPDX-License-Identifier: Apache-2.0
-# Copyright 2022 Stéphane Caron
-
-"""Yumi two-armed manipulator with definition of custom frame and
-collision avoidance w.r.t. those frames."""
-
-import meshcat_shapes
-import numpy as np
-import pinocchio as pin
-import qpsolvers
-from loop_rate_limiters import RateLimiter
-
-import pink
-from pink import solve_ik
-from pink.barriers import BodySphericalBarrier
-from pink.tasks import FrameTask, PostureTask
-from pink.visualization import start_meshcat_visualizer
-
-try:
-    from robot_descriptions.loaders.pinocchio import load_robot_description
-except ModuleNotFoundError as exc:
-    raise ModuleNotFoundError(
-        "Examples need robot_descriptions, "
-        "try `[conda|pip] install robot_descriptions`"
-    ) from exc  # noqa: E501
-
-
-if __name__ == "__main__":
-    # Load the robot and define the custom frames
-    robot = load_robot_description("yumi_description", root_joint=None)
-
-    # Left arm frame
-    l_frame_placement = pin.SE3()
-    l_frame_placement.translation = np.array([0.0, 0.0, 0.05])
-    l_frame_placement.rotation = np.eye(3)
-
-    l_frame = pin.Frame(
-        "yumi_barrier_l",
-        robot.model.getJointId("yumi_joint_6_l"),
-        robot.model.getFrameId("yumi_link_7_l"),
-        l_frame_placement,
-        pin.FrameType.OP_FRAME,
+from smc import getMinimalArgParser, getRobotFromArgs
+from smc.control.cartesian_space import getClikArgs
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+from smc.control.joint_space.joint_space_point_to_point import moveJP
+from smc.util.define_random_goal import getRandomlyGeneratedGoal
+from smc.robots.utils import defineGoalPointCLI
+from smc.control.cartesian_space.pink_p2p import (
+    DualArmIKSelfAvoidanceViaEndEffectorSpheres,
 )
 
-    robot.model.addFrame(l_frame)
-
-    # Right arm frame
-    r_frame_placement = pin.SE3()
-    r_frame_placement.translation = np.array([0.0, 0.0, 0.05])
-    r_frame_placement.rotation = np.eye(3)
 
-    r_frame = pin.Frame(
-        "yumi_barrier_r",
-        robot.model.getJointId("yumi_joint_6_r"),
-        robot.model.getFrameId("yumi_link_7_r"),
-        r_frame_placement,
-        pin.FrameType.OP_FRAME,
-    )
-
-    robot.model.addFrame(r_frame)
-    # Redefining robot data to take into account the new frames
-    robot.data = pin.Data(robot.model)
-
-    viz = start_meshcat_visualizer(robot)
-
-    # Pink tasks
-    left_end_effector_task = FrameTask(
-        "yumi_link_7_l",
-        position_cost=50.0,  # [cost] / [m]
-        orientation_cost=10.0,  # [cost] / [rad]
-    )
-    right_end_effector_task = FrameTask(
-        "yumi_link_7_r",
-        position_cost=50.0,  # [cost] / [m]
-        orientation_cost=10.0,  # [cost] / [rad]
-    )
-
-    # Pink barriers
-    ee_barrier = BodySphericalBarrier(
-        ("yumi_barrier_l", "yumi_barrier_r"),
-        d_min=0.25,
-        gain=100.0,
-        safe_displacement_gain=1.0,
-    )
-
-    posture_task = PostureTask(
-        cost=1e-3,  # [cost] / [rad]
-    )
-
-    cbf_list = [ee_barrier]
-    tasks = [left_end_effector_task, right_end_effector_task, posture_task]
-
-    q_ref = np.array(
-        [
-            0.045,
-            -0.155,
-            -0.394,
-            -0.617,
-            -0.939,
-            -0.343,
-            -1.216,
-            0,
-            0,
-            -0.374,
-            -0.249,
-            0.562,
-            -0.520,
-            0.934,
-            -0.337,
-            1.400,
-            0,
-            0,
-        ]
-    )
-    configuration = pink.Configuration(robot.model, robot.data, q_ref)
-    for task in tasks:
-        task.set_target_from_configuration(configuration)
-    viz.display(configuration.q)
-
-    viewer = viz.viewer
-    meshcat_shapes.frame(viewer["left_end_effector"], opacity=1.0)
-    meshcat_shapes.sphere(
-        viewer["left_ee_barrier"],
-        opacity=0.4,
-        color=0xFF0000,
-        radius=0.125,
-    )
-    meshcat_shapes.sphere(
-        viewer["right_ee_barrier"],
-        opacity=0.4,
-        color=0x00FF00,
-        radius=0.125,
-    )
-    meshcat_shapes.frame(viewer["right_end_effector"], opacity=1.0)
-    meshcat_shapes.frame(viewer["left_end_effector_target"], opacity=1.0)
-    meshcat_shapes.frame(viewer["right_end_effector_target"], opacity=1.0)
-
-    # Select QP solver
-    solver = qpsolvers.available_solvers[0]
-    if "osqp" in qpsolvers.available_solvers:
-        solver = "osqp"
-
-    rate = RateLimiter(frequency=200.0)
-    dt = rate.period
-    t = 0.0  # [s]
-    l_y_des = np.array([0.392, 0.392, 0.6])
-    r_y_des = np.array([0.392, 0.392, 0.6])
-    l_dy_des = np.zeros(3)
-    r_dy_des = np.zeros(3)
-
-    while True:
-        # Calculate desired trajectory
-        A = 0.1
-        B = 0.1
-        # z -- 0.4 - 0.8
-        l_y_des[:] = (
-            0.6,
-            0.1 + B * np.sin(t),
-            0.6 + A * np.sin(t),
-        )
-        r_y_des[:] = (
-            0.6,
-            -0.1 - B * np.sin(t),
-            0.6 + A * np.sin(t),
-        )
-        l_dy_des[:] = 0, B * np.cos(t), A * np.cos(t)
-        r_dy_des[:] = 0, -B * np.cos(t), A * np.cos(t)
+import argparse
+import pinocchio as pin
 
-        left_end_effector_task.transform_target_to_world.translation = l_y_des
-        right_end_effector_task.transform_target_to_world.translation = r_y_des
 
-        # Update visualization frames
-        viewer["left_end_effector"].set_transform(
-            configuration.get_transform_frame_to_world(left_end_effector_task.frame).np
-        )
-        viewer["right_end_effector"].set_transform(
-            configuration.get_transform_frame_to_world(right_end_effector_task.frame).np
-        )
-        viewer["left_end_effector_target"].set_transform(
-            left_end_effector_task.transform_target_to_world.np
-        )
-        viewer["right_end_effector_target"].set_transform(
-            right_end_effector_task.transform_target_to_world.np
+def get_args() -> argparse.Namespace:
+    parser = getMinimalArgParser()
+    parser.description = "Run closed loop inverse kinematics \
+    of various kinds. Make sure you know what the goal is before you run!"
+    parser = getClikArgs(parser)
+    parser.add_argument(
+        "--randomly-generate-goal",
+        action=argparse.BooleanOptionalAction,
+        default=False,
+        help="if true, the target pose is randomly generated, if false you type it target translation in via text input",
     )
+    args = parser.parse_args()
+    return args
 
-        lb = configuration.get_transform_frame_to_world("yumi_barrier_l")
-        rb = configuration.get_transform_frame_to_world("yumi_barrier_r")
 
-        viewer["left_ee_barrier"].set_transform(lb.np)
-        viewer["right_ee_barrier"].set_transform(rb.np)
-
-        velocity = solve_ik(
-            configuration,
-            tasks,
-            dt,
-            solver=solver,
-            barriers=cbf_list,
-        )
-        configuration.integrate_inplace(velocity, dt)
-        dist_ee = (
-            np.linalg.norm(
-                configuration.get_transform_frame_to_world("yumi_barrier_l").translation
-                - configuration.get_transform_frame_to_world(
-                    "yumi_barrier_r"
-                ).translation
-            )
-            * 100
-        )
-        print(f"Distance between end effectors: {dist_ee :0.1f}cm")
-        # Visualize result at fixed FPS
-        viz.display(configuration.q)
-        rate.sleep()
-        t += dt
+if __name__ == "__main__":
+    args = get_args()
+    robot = getRobotFromArgs(args)
+    assert issubclass(robot.__class__, DualArmInterface)
+    # go to home position
+    moveJP(robot._comfy_configuration, args, robot)
+
+    if args.randomly_generate_goal:
+        T_w_absgoal = getRandomlyGeneratedGoal(args)
+        if args.visualizer:
+            robot.visualizer_manager.sendCommand({"Mgoal": T_w_absgoal})
+    else:
+        T_w_absgoal = defineGoalPointCLI(robot)
+        T_w_absgoal.rotation = robot.T_w_abs.rotation.copy()
+    T_absgoal_l = pin.SE3.Identity()
+    T_absgoal_l.translation[1] = 0.15
+    T_absgoal_r = pin.SE3.Identity()
+    T_absgoal_r.translation[1] = -0.15
+
+    DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+        T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
+    )
+
+    if args.real:
+        robot.stopRobot()
+
+    if args.visualizer:
+        robot.killManipulatorVisualizer()
+
+    if args.save_log:
+        robot._log_manager.saveLog()

python/smc/control/cartesian_space/pink_p2p.py

+from smc.control.control_loop_manager import ControlLoopManager
+from smc.control.controller_templates.point_to_point import DualEEP2PCtrlLoopTemplate
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
+
+import pink
+from pink.barriers import BodySphericalBarrier
+from pink.tasks import FrameTask, PostureTask
+
+import numpy as np
+import qpsolvers
+import argparse
+from functools import partial
+from collections import deque
+import pinocchio as pin
+
+
+# TODO: butcher pink to avoid redundancies with smc like configuration.
+# right now there is no time and we're just shoving it in.
+def DualArmIKSelfAvoidanceViaEndEffectorSpheresCtrlLoop(
+    tasks: list[pink.FrameTask],
+    cbf_list: list[pink.barriers.Barrier],
+    solver: str,
+    T_w_absgoal: pin.SE3,
+    T_absgoal_l: pin.SE3,
+    T_absgoal_r: pin.SE3,
+    args: argparse.Namespace,
+    robot: DualArmInterface,
+    t: int,
+    past_data: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+    configuration = pink.Configuration(robot.model, robot.data, robot.q)
+    # NOTE: there are limits in pink, but they are a class where G matrix is constucted
+    # combining lb and ub and forming Gx \leq h
+    # this makes it possible to combine other stuff into this inequality constraint
+    configuration.model.velocityLimit = robot._max_v
+
+    T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
+    T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
+    # next_goal_l = pin.SE3.Interpolate(robot.T_w_l, T_w_lgoal, 0.001)
+    # next_goal_r = pin.SE3.Interpolate(robot.T_w_r, T_w_rgoal, 0.001)
+    # tasks[0].set_target(next_goal_l)
+    # tasks[1].set_target(next_goal_r)
+    tasks[0].set_target(T_w_lgoal)
+    tasks[1].set_target(T_w_rgoal)
+
+    v_cmd = pink.solve_ik(
+        configuration,
+        tasks,
+        robot.dt,
+        solver=solver,
+        barriers=cbf_list,
+        # safety_break=True,
+        safety_break=False,
+    )
+    dist_ee = np.linalg.norm(robot.T_w_l.translation - robot.T_w_r.translation)
+    log_item = {"dist_ees": dist_ee.reshape((1,))}
+    return v_cmd, {}, log_item
+
+
+def DualArmIKSelfAvoidanceViaEndEffectorSpheres(
+    T_w_absgoal: pin.SE3,
+    T_absgoal_l: pin.SE3,
+    T_absgoal_r: pin.SE3,
+    args: argparse.Namespace,
+    robot: DualArmInterface,
+    run: bool = True,
+) -> ControlLoopManager | None:
+
+    # Pink tasks
+    left_end_effector_task = FrameTask(
+        "robl_tool0",
+        position_cost=50.0,  # [cost] / [m]
+        orientation_cost=10.0,  # [cost] / [rad]
+    )
+    right_end_effector_task = FrameTask(
+        "robr_tool0",
+        position_cost=50.0,  # [cost] / [m]
+        orientation_cost=10.0,  # [cost] / [rad]
+    )
+
+    # Pink barriers
+    ee_barrier = BodySphericalBarrier(
+        ("robl_tool0", "robr_tool0"),
+        d_min=0.15,
+        gain=100.0,
+        safe_displacement_gain=1.0,
+    )
+
+    posture_task = PostureTask(
+        cost=1e-3,  # [cost] / [rad]
+    )
+
+    cbf_list = [ee_barrier]
+    tasks = [left_end_effector_task, right_end_effector_task, posture_task]
+
+    # NOTE: model and data are shared pointers between configuration and robot.
+    # this is redundant as hell, but I don't have the time butcher pink right now
+    configuration = pink.Configuration(robot.model, robot.data, robot.q)
+    posture_task.set_target_from_configuration(configuration)
+    left_end_effector_task.set_target(T_absgoal_l.act(T_w_absgoal))
+    right_end_effector_task.set_target(T_absgoal_r.act(T_w_absgoal))
+
+    #    meshcat_shapes.sphere(
+    #        viewer["left_ee_barrier"],
+    #        opacity=0.4,
+    #        color=0xFF0000,
+    #        radius=0.125,
+    #    )
+    #    meshcat_shapes.sphere(
+    #        viewer["right_ee_barrier"],
+    #        opacity=0.4,
+    #        color=0x00FF00,
+    #        radius=0.125,
+    #    )
+    #    meshcat_shapes.frame(viewer["left_end_effector_target"], opacity=1.0)
+    #    meshcat_shapes.frame(viewer["right_end_effector_target"], opacity=1.0)
+
+    # TODO: allow proxsuite solvers also (primarily because they can be saved and reused with warmstarting)
+    # Select QP solver
+    solver = qpsolvers.available_solvers[0]
+    if "osqp" in qpsolvers.available_solvers:
+        solver = "osqp"
+
+    ctrl_loop = partial(
+        DualArmIKSelfAvoidanceViaEndEffectorSpheresCtrlLoop,
+        tasks,
+        cbf_list,
+    )
+    control_loop = partial(
+        DualEEP2PCtrlLoopTemplate,
+        solver,
+        T_w_absgoal,
+        T_absgoal_l,
+        T_absgoal_r,
+        ctrl_loop,
+        args,
+        robot,
+    )
+    log_item = {
+        "qs": robot.q,
+        "dqs": np.zeros(robot.nv),
+        "dqs_cmd": np.zeros((robot.model.nv,)),
+        "l_err_norm": np.zeros((1,)),
+        "r_err_norm": np.zeros((1,)),
+        "dist_ees": np.zeros((1,)),
+    }
+    loop_manager = ControlLoopManager(robot, control_loop, args, {}, log_item)
+    if not run:
+        return loop_manager
+    else:
+        loop_manager.run()

python/smc/control/control_loop_manager.py

 from argparse import Namespace
 from smc.robots.abstract_robotmanager import AbstractRobotManager
+from smc.robots.interfaces.dual_arm_interface import DualArmInterface
 from smc.robots.interfaces.single_arm_interface import SingleArmInterface
 from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
 from smc.multiprocessing.process_manager import ProcessManager
@@ -151,12 +152,21 @@ class ControlLoopManager:
                         "q": self.robot_manager._q,
                     }
                 )
+                # NOTE: for dual armed robots it displays T_w_abs
                 if issubclass(self.robot_manager.__class__, SingleArmInterface):
                     self.robot_manager.visualizer_manager.sendCommand(
                         {
                             "T_w_e": self.robot_manager.T_w_e,
                         }
                     )
+                # TODO: implement in visualizer
+                #                if issubclass(self.robot_manager.__class__, DualArmInterface):
+                #                    self.robot_manager.visualizer_manager.sendCommand(
+                #                        {
+                #                            "T_w_l": self.robot_manager.T_w_l,
+                #                            "T_w_r": self.robot_manager.T_w_r,
+                #                        }
+                #                    )
                 if issubclass(self.robot_manager.__class__, MobileBaseInterface):
                     self.robot_manager.visualizer_manager.sendCommand(
                         {"T_base": self.robot_manager.T_w_b}
@@ -164,7 +174,7 @@ class ControlLoopManager:
 
                 if self.args.visualize_collision_approximation:
                     raise NotImplementedError
-                # TODO: here call robot manager's update ellipses function
+                # TODO: here call robot manager's update ellipses function or whatever approximation is used
 
             if self.args.plotter:
                 # don't put new stuff in if it didn't handle the previous stuff.

python/smc/control/joint_space/joint_space_point_to_point.py

@@ -5,6 +5,7 @@ import numpy as np
 from functools import partial
 from collections import deque
 from argparse import Namespace
+import pinocchio as pin
 
 
 def moveJControlLoop(
@@ -21,13 +22,20 @@ def moveJControlLoop(
     """
     breakFlag = False
     q = robot.q
-    q_error = q_desired - q
-    if np.linalg.norm(q_error) < 1e-3:
+    q_error = pin.difference(robot.model, q, q_desired)
+    err_norm = np.linalg.norm(q_error)
+    if err_norm < 1e-3:
         breakFlag = True
-    K = 120
+    K = 320
     qd = K * q_error * robot.dt
     robot.sendVelocityCommand(qd)
-    return breakFlag, {}, {}
+    log_item = {
+        "qs": robot.q,
+        "dqs": robot.v,
+        "dqs_cmd": qd,
+        "err_norm": err_norm.reshape((1,)),
+    }
+    return breakFlag, {}, log_item
 
 
 # TODO:
@@ -44,10 +52,14 @@ def moveJP(q_desired: np.ndarray, args: Namespace, robot: AbstractRobotManager)
     assert type(q_desired) == np.ndarray
     controlLoop = partial(moveJControlLoop, q_desired, args, robot)
     # we're not using any past data or logging, hence the empty arguments
-    loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
+    log_item = {
+        "qs": np.zeros(robot.nq),
+        "dqs": np.zeros(robot.nv),
+        "dqs_cmd": np.zeros(robot.nv),
+        "err_norm": np.zeros((1,)),
+    }
+    loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
     loop_manager.run()
-    # TODO: remove, this isn't doing anything
-    # time.sleep(0.01)
     if args.debug_prints:
         print("MoveJP done: convergence achieved, reached destionation!")
 
@@ -122,8 +134,8 @@ def moveJPIControlLoop(
 
     log_item["qs"] = q
     log_item["dqs"] = robot.v
-    log_item["integral_error"] = integral_error  # Save updated integral error
-    log_item["e_prev"] = q_error  # Save current position error
+    log_item["integral_error"] = integral_error.flatten()  # Save updated integral error
+    log_item["e_prev"] = q_error.flatten()  # Save current position error
     return breakFlag, save_past_dict, log_item
 
 

python/smc/robots/abstract_robotmanager.py

@@ -58,9 +58,11 @@ class AbstractRobotManager(abc.ABC):
         self._max_v = np.clip(self._MAX_V, 0.0, args.max_v_percentage * self._MAX_V)
         # NOTE: make sure you've read pinocchio docs and understand why
         #       nq and nv are not necessarily the same number
-        self._q = np.zeros(self.model.nq)
-        self._v = np.zeros(self.model.nv)
-        self._a = np.zeros(self.model.nv)
+        self._q = np.zeros(self.nq)
+        self._v = np.zeros(self.nv)
+        self._a = np.zeros(self.nv)
+
+        self._comfy_configuration: np.ndarray
 
         # TODO: each robot should know which grippers are available
         # and set this there.

python/smc/robots/implementations/yumi.py

@@ -6,6 +6,7 @@ from argparse import Namespace
 from importlib.resources import files
 from os import path
 import pinocchio as pin
+import numpy as np
 
 
 class AbstractYuMiRobotManager(DualArmInterface):
@@ -19,6 +20,42 @@ class AbstractYuMiRobotManager(DualArmInterface):
         self._r_ee_frame_id = self.model.getFrameId("robr_tool0")
         self._MAX_ACCELERATION = 1.7  # const
         self._MAX_QD = 3.14  # const
+        #        self._comfy_configuration = np.array(
+        #            [
+        #                -0.7019,
+        #                0.03946,
+        #                1.13817,
+        #                0.40438,
+        #                1.59454,
+        #                0.37243,
+        #                -1.3882,
+        #                1.17810,
+        #                -0.00055,
+        #                -1.7492,
+        #                0.41061,
+        #                -2.0604,
+        #                0.30449,
+        #                1.72462,
+        #            ]
+        #        )
+        self._comfy_configuration = np.array(
+            [
+                0.045,
+                -0.155,
+                -0.394,
+                -0.617,
+                -0.939,
+                -0.343,
+                -1.216,
+                -0.374,
+                -0.249,
+                0.562,
+                -0.520,
+                0.934,
+                -0.337,
+                1.400,
+            ]
+        )
 
         self._mode: DualArmInterface.control_mode = (
             DualArmInterface.control_mode.both_arms

python/smc/visualization/visualizer.py

@@ -2,6 +2,10 @@ import numpy as np
 from smc.visualization.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
 from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat
 import meshcat_shapes
+import pinocchio as pin
+from multiprocessing import Queue
+from argparse import Namespace
+from typing import Any
 
 # tkinter stuff for later reference
 # from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk)
@@ -9,16 +13,26 @@ import meshcat_shapes
 # from tkinter.ttk import *
 
 
-def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue):
+def manipulatorVisualizer(
+    model: pin.Model,
+    collision_model: pin.GeometryModel,
+    visual_model: pin.GeometryModel,
+    args: Namespace,
+    cmd: dict[str, Any],
+    queue: Queue,
+) -> None:
     viz = MeshcatVisualizer(
         model=model, collision_model=collision_model, visual_model=visual_model
     )
     viz.loadViewerModel()
     # display the initial pose
     viz.display(cmd["q"])
-    # set shapes we know we'll use
+    # TODO: load shapes depending on robot type
+    # set shapes we might use
     meshcat_shapes.frame(viz.viewer["Mgoal"], opacity=0.5)
     meshcat_shapes.frame(viz.viewer["T_w_e"], opacity=0.5)
+    meshcat_shapes.frame(viz.viewer["T_w_l"], opacity=0.5)
+    meshcat_shapes.frame(viz.viewer["T_w_r"], opacity=0.5)
     meshcat_shapes.frame(viz.viewer["T_base"], opacity=0.5)
     print("MANIPULATORVISUALIZER: FULLY ONLINE")
     try:
@@ -37,6 +51,10 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
                     viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
                 if key == "T_w_e":
                     viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous)
+                if key == "T_w_l":
+                    viz.viewer["T_w_l"].set_transform(cmd["T_w_l"].homogeneous)
+                if key == "T_w_r":
+                    viz.viewer["T_w_r"].set_transform(cmd["T_w_r"].homogeneous)
                 if key == "T_base":
                     viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous)
                 if key == "q":