Skip to content
Snippets Groups Projects
Commit 8a53cfb0 authored by m-guberina's avatar m-guberina
Browse files

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
parent 7c15064c
Branches
No related tags found
No related merge requests found
...@@ -28,17 +28,17 @@ if __name__ == "__main__": ...@@ -28,17 +28,17 @@ if __name__ == "__main__":
args = get_args() args = get_args()
robot = getRobotFromArgs(args) robot = getRobotFromArgs(args)
if args.randomly_generate_goal: if args.randomly_generate_goal:
T_w_goal = getRandomlyGeneratedGoal(args) T_w_absgoal = getRandomlyGeneratedGoal(args)
if args.visualizer: if args.visualizer:
robot.visualizer_manager.sendCommand({"Mgoal": T_w_goal}) robot.visualizer_manager.sendCommand({"Mgoal": T_w_absgoal})
else: else:
T_w_goal = defineGoalPointCLI(robot) T_w_absgoal = defineGoalPointCLI(robot)
T_w_goal.rotation = np.eye(3) T_w_absgoal.rotation = np.eye(3)
T_absgoal_lgoal = pin.SE3.Identity() T_absgoal_l = pin.SE3.Identity()
T_absgoal_lgoal.translation[1] = 0.1 T_absgoal_l.translation[1] = 0.1
T_absgoal_rgoal = pin.SE3.Identity() T_absgoal_r = pin.SE3.Identity()
T_absgoal_rgoal.translation[1] = -0.1 T_absgoal_r.translation[1] = -0.1
moveLDualArm(args, robot, T_w_goal, T_absgoal_lgoal, T_absgoal_rgoal) moveLDualArm(args, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r)
if args.real: if args.real:
robot.stopRobot() robot.stopRobot()
......
#!/usr/bin/env python3 from smc import getMinimalArgParser, getRobotFromArgs
# -*- coding: utf-8 -*- from smc.control.cartesian_space import getClikArgs
# from smc.robots.interfaces.dual_arm_interface import DualArmInterface
# SPDX-License-Identifier: Apache-2.0 from smc.control.joint_space.joint_space_point_to_point import moveJP
# Copyright 2022 Stéphane Caron from smc.util.define_random_goal import getRandomlyGeneratedGoal
from smc.robots.utils import defineGoalPointCLI
"""Yumi two-armed manipulator with definition of custom frame and from smc.control.cartesian_space.pink_p2p import (
collision avoidance w.r.t. those frames.""" DualArmIKSelfAvoidanceViaEndEffectorSpheres,
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,
) )
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( import argparse
"yumi_barrier_r", import pinocchio as pin
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 def get_args() -> argparse.Namespace:
viewer["left_end_effector"].set_transform( parser = getMinimalArgParser()
configuration.get_transform_frame_to_world(left_end_effector_task.frame).np parser.description = "Run closed loop inverse kinematics \
) of various kinds. Make sure you know what the goal is before you run!"
viewer["right_end_effector"].set_transform( parser = getClikArgs(parser)
configuration.get_transform_frame_to_world(right_end_effector_task.frame).np parser.add_argument(
) "--randomly-generate-goal",
viewer["left_end_effector_target"].set_transform( action=argparse.BooleanOptionalAction,
left_end_effector_task.transform_target_to_world.np default=False,
) help="if true, the target pose is randomly generated, if false you type it target translation in via text input",
viewer["right_end_effector_target"].set_transform(
right_end_effector_task.transform_target_to_world.np
) )
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) if __name__ == "__main__":
viewer["right_ee_barrier"].set_transform(rb.np) args = get_args()
robot = getRobotFromArgs(args)
velocity = solve_ik( assert issubclass(robot.__class__, DualArmInterface)
configuration, # go to home position
tasks, moveJP(robot._comfy_configuration, args, robot)
dt,
solver=solver, if args.randomly_generate_goal:
barriers=cbf_list, T_w_absgoal = getRandomlyGeneratedGoal(args)
) if args.visualizer:
configuration.integrate_inplace(velocity, dt) robot.visualizer_manager.sendCommand({"Mgoal": T_w_absgoal})
dist_ee = ( else:
np.linalg.norm( T_w_absgoal = defineGoalPointCLI(robot)
configuration.get_transform_frame_to_world("yumi_barrier_l").translation T_w_absgoal.rotation = robot.T_w_abs.rotation.copy()
- configuration.get_transform_frame_to_world( T_absgoal_l = pin.SE3.Identity()
"yumi_barrier_r" T_absgoal_l.translation[1] = 0.15
).translation T_absgoal_r = pin.SE3.Identity()
) T_absgoal_r.translation[1] = -0.15
* 100
) DualArmIKSelfAvoidanceViaEndEffectorSpheres(
print(f"Distance between end effectors: {dist_ee :0.1f}cm") T_w_absgoal, T_absgoal_l, T_absgoal_r, args, robot
# Visualize result at fixed FPS )
viz.display(configuration.q)
rate.sleep() if args.real:
t += dt robot.stopRobot()
if args.visualizer:
robot.killManipulatorVisualizer()
if args.save_log:
robot._log_manager.saveLog()
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()
from argparse import Namespace from argparse import Namespace
from smc.robots.abstract_robotmanager import AbstractRobotManager 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.single_arm_interface import SingleArmInterface
from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
from smc.multiprocessing.process_manager import ProcessManager from smc.multiprocessing.process_manager import ProcessManager
...@@ -151,12 +152,21 @@ class ControlLoopManager: ...@@ -151,12 +152,21 @@ class ControlLoopManager:
"q": self.robot_manager._q, "q": self.robot_manager._q,
} }
) )
# NOTE: for dual armed robots it displays T_w_abs
if issubclass(self.robot_manager.__class__, SingleArmInterface): if issubclass(self.robot_manager.__class__, SingleArmInterface):
self.robot_manager.visualizer_manager.sendCommand( self.robot_manager.visualizer_manager.sendCommand(
{ {
"T_w_e": self.robot_manager.T_w_e, "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): if issubclass(self.robot_manager.__class__, MobileBaseInterface):
self.robot_manager.visualizer_manager.sendCommand( self.robot_manager.visualizer_manager.sendCommand(
{"T_base": self.robot_manager.T_w_b} {"T_base": self.robot_manager.T_w_b}
...@@ -164,7 +174,7 @@ class ControlLoopManager: ...@@ -164,7 +174,7 @@ class ControlLoopManager:
if self.args.visualize_collision_approximation: if self.args.visualize_collision_approximation:
raise NotImplementedError 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: if self.args.plotter:
# don't put new stuff in if it didn't handle the previous stuff. # don't put new stuff in if it didn't handle the previous stuff.
......
...@@ -5,6 +5,7 @@ import numpy as np ...@@ -5,6 +5,7 @@ import numpy as np
from functools import partial from functools import partial
from collections import deque from collections import deque
from argparse import Namespace from argparse import Namespace
import pinocchio as pin
def moveJControlLoop( def moveJControlLoop(
...@@ -21,13 +22,20 @@ def moveJControlLoop( ...@@ -21,13 +22,20 @@ def moveJControlLoop(
""" """
breakFlag = False breakFlag = False
q = robot.q q = robot.q
q_error = q_desired - q q_error = pin.difference(robot.model, q, q_desired)
if np.linalg.norm(q_error) < 1e-3: err_norm = np.linalg.norm(q_error)
if err_norm < 1e-3:
breakFlag = True breakFlag = True
K = 120 K = 320
qd = K * q_error * robot.dt qd = K * q_error * robot.dt
robot.sendVelocityCommand(qd) 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: # TODO:
...@@ -44,10 +52,14 @@ def moveJP(q_desired: np.ndarray, args: Namespace, robot: AbstractRobotManager) ...@@ -44,10 +52,14 @@ def moveJP(q_desired: np.ndarray, args: Namespace, robot: AbstractRobotManager)
assert type(q_desired) == np.ndarray assert type(q_desired) == np.ndarray
controlLoop = partial(moveJControlLoop, q_desired, args, robot) controlLoop = partial(moveJControlLoop, q_desired, args, robot)
# we're not using any past data or logging, hence the empty arguments # 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() loop_manager.run()
# TODO: remove, this isn't doing anything
# time.sleep(0.01)
if args.debug_prints: if args.debug_prints:
print("MoveJP done: convergence achieved, reached destionation!") print("MoveJP done: convergence achieved, reached destionation!")
...@@ -122,8 +134,8 @@ def moveJPIControlLoop( ...@@ -122,8 +134,8 @@ def moveJPIControlLoop(
log_item["qs"] = q log_item["qs"] = q
log_item["dqs"] = robot.v log_item["dqs"] = robot.v
log_item["integral_error"] = integral_error # Save updated integral error log_item["integral_error"] = integral_error.flatten() # Save updated integral error
log_item["e_prev"] = q_error # Save current position error log_item["e_prev"] = q_error.flatten() # Save current position error
return breakFlag, save_past_dict, log_item return breakFlag, save_past_dict, log_item
......
...@@ -58,9 +58,11 @@ class AbstractRobotManager(abc.ABC): ...@@ -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) 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 # NOTE: make sure you've read pinocchio docs and understand why
# nq and nv are not necessarily the same number # nq and nv are not necessarily the same number
self._q = np.zeros(self.model.nq) self._q = np.zeros(self.nq)
self._v = np.zeros(self.model.nv) self._v = np.zeros(self.nv)
self._a = np.zeros(self.model.nv) self._a = np.zeros(self.nv)
self._comfy_configuration: np.ndarray
# TODO: each robot should know which grippers are available # TODO: each robot should know which grippers are available
# and set this there. # and set this there.
......
...@@ -6,6 +6,7 @@ from argparse import Namespace ...@@ -6,6 +6,7 @@ from argparse import Namespace
from importlib.resources import files from importlib.resources import files
from os import path from os import path
import pinocchio as pin import pinocchio as pin
import numpy as np
class AbstractYuMiRobotManager(DualArmInterface): class AbstractYuMiRobotManager(DualArmInterface):
...@@ -19,6 +20,42 @@ class AbstractYuMiRobotManager(DualArmInterface): ...@@ -19,6 +20,42 @@ class AbstractYuMiRobotManager(DualArmInterface):
self._r_ee_frame_id = self.model.getFrameId("robr_tool0") self._r_ee_frame_id = self.model.getFrameId("robr_tool0")
self._MAX_ACCELERATION = 1.7 # const self._MAX_ACCELERATION = 1.7 # const
self._MAX_QD = 3.14 # 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 = ( self._mode: DualArmInterface.control_mode = (
DualArmInterface.control_mode.both_arms DualArmInterface.control_mode.both_arms
......
...@@ -2,6 +2,10 @@ import numpy as np ...@@ -2,6 +2,10 @@ import numpy as np
from smc.visualization.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer from smc.visualization.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat
import meshcat_shapes import meshcat_shapes
import pinocchio as pin
from multiprocessing import Queue
from argparse import Namespace
from typing import Any
# tkinter stuff for later reference # tkinter stuff for later reference
# from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk) # from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk)
...@@ -9,16 +13,26 @@ import meshcat_shapes ...@@ -9,16 +13,26 @@ import meshcat_shapes
# from tkinter.ttk import * # 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( viz = MeshcatVisualizer(
model=model, collision_model=collision_model, visual_model=visual_model model=model, collision_model=collision_model, visual_model=visual_model
) )
viz.loadViewerModel() viz.loadViewerModel()
# display the initial pose # display the initial pose
viz.display(cmd["q"]) 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["Mgoal"], opacity=0.5)
meshcat_shapes.frame(viz.viewer["T_w_e"], 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) meshcat_shapes.frame(viz.viewer["T_base"], opacity=0.5)
print("MANIPULATORVISUALIZER: FULLY ONLINE") print("MANIPULATORVISUALIZER: FULLY ONLINE")
try: try:
...@@ -37,6 +51,10 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue ...@@ -37,6 +51,10 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous) viz.viewer["Mgoal"].set_transform(cmd["Mgoal"].homogeneous)
if key == "T_w_e": if key == "T_w_e":
viz.viewer["T_w_e"].set_transform(cmd["T_w_e"].homogeneous) 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": if key == "T_base":
viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous) viz.viewer["T_base"].set_transform(cmd["T_base"].homogeneous)
if key == "q": if key == "q":
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment