diff --git a/examples/cartesian_space/dual_arm_clik.py b/examples/cartesian_space/dual_arm_clik.py
index b9cfc1f0c5025f2f0f0f2b8c9fece43a8dce83b5..a517a6f2a1b0880ae020b6a6822ab549b6482713 100644
--- a/examples/cartesian_space/dual_arm_clik.py
+++ b/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()
diff --git a/examples/cartesian_space/yumi_self_collision.py b/examples/cartesian_space/yumi_self_collision.py
index f0c681ad0d121c605870dae1b0a188d513ea899a..721330932a9cb83b741818fbde0b0d3ba1cdd88f 100644
--- a/examples/cartesian_space/yumi_self_collision.py
+++ b/examples/cartesian_space/yumi_self_collision.py
@@ -1,216 +1,61 @@
-#!/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
+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,
+)
+
+
+import argparse
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,
+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
- 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)
-
- 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
- )
-
- 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()
diff --git a/python/smc/control/cartesian_space/pink_p2p.py b/python/smc/control/cartesian_space/pink_p2p.py
new file mode 100644
index 0000000000000000000000000000000000000000..87526d5c9dc1b0683757fea1236916f1943d8c00
--- /dev/null
+++ b/python/smc/control/cartesian_space/pink_p2p.py
@@ -0,0 +1,151 @@
+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()
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index 0e3175d715f0477842134342e9566b9eee327b0d..d7bfefb8a9b59f0337eadddd1400cf989e06cf6d 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -1,5 +1,6 @@
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.
diff --git a/python/smc/control/joint_space/joint_space_point_to_point.py b/python/smc/control/joint_space/joint_space_point_to_point.py
index d506184c74e9d5992e2f4402d575b8edcf1ccb1d..93e30cfe0f0a02b246b76eb17de0a9459f71da2f 100644
--- a/python/smc/control/joint_space/joint_space_point_to_point.py
+++ b/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
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index 6605d92b26b30d35d11275bd55a6a3c8df16cb40..5a298331264f5652bc783c2b08040321095664ec 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/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.
diff --git a/python/smc/robots/implementations/yumi.py b/python/smc/robots/implementations/yumi.py
index 82194c49d6270391893c31d1e5a50346410c3d73..2bd17e842329458bce6ef828e29f51f078db98a4 100644
--- a/python/smc/robots/implementations/yumi.py
+++ b/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
diff --git a/python/smc/visualization/visualizer.py b/python/smc/visualization/visualizer.py
index 2851764d1b7b6fd55bfa0fb0e5150afae7caf8db..041dcbae7d17949db4997530243675b0ba5018e1 100644
--- a/python/smc/visualization/visualizer.py
+++ b/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":