From b57fe5e3010368184d255570f5552afe0e1774c0 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 7 Feb 2025 11:10:28 +0100
Subject: [PATCH] p
---
python/smc/__init__.py | 19 +++++++++++++++++--
python/smc/{util => robots}/get_model.py | 20 ++++++++++----------
python/smc/robots/implementations/ur5e.py | 2 +-
python/smc/robots/robotmanager_abstract.py | 7 +++++--
4 files changed, 33 insertions(+), 15 deletions(-)
rename python/smc/{util => robots}/get_model.py (97%)
diff --git a/python/smc/__init__.py b/python/smc/__init__.py
index 581c607..80bfd46 100644
--- a/python/smc/__init__.py
+++ b/python/smc/__init__.py
@@ -1,4 +1,19 @@
-from ur_simple_control import robots, control, multiprocessing, util, visualize, vision, path_generation
+from smc import (
+ robots,
+ control,
+ multiprocessing,
+ util,
+ visualize,
+ vision,
+ path_generation,
+)
+
__all__ = [
-"robots", "control","multiprocessing", "util", "visualize", "vision", "path_generation"
+ "robots",
+ "control",
+ "multiprocessing",
+ "util",
+ "visualize",
+ "vision",
+ "path_generation",
]
diff --git a/python/smc/util/get_model.py b/python/smc/robots/get_model.py
similarity index 97%
rename from python/smc/util/get_model.py
rename to python/smc/robots/get_model.py
index 23d4525..87a285d 100644
--- a/python/smc/util/get_model.py
+++ b/python/smc/robots/get_model.py
@@ -16,7 +16,7 @@ import example_robot_data
# can't get the urdf reading with these functions to save my life, idk what or why
#############################################################
-# PACKAGE_DIR IS THE WHOLE UR_SIMPLE_CONTROL FOLDER (cos that's accessible from anywhere it's installed)
+# PACKAGE_DIR IS THE WHOLE smc FOLDER (cos that's accessible from anywhere it's installed)
# PACKAGE:// IS WHAT'S BEING REPLACED WITH THE PACKAGE_DIR ARGUMENT IN THE URDF.
# YOU GIVE ABSOLUTE PATH TO THE URDF THO.
#############################################################
@@ -38,12 +38,14 @@ NOTE: this should be fixed for a proper release
def get_model():
- urdf_path_relative = files("ur_simple_control.robot_descriptions.urdf").joinpath(
+ urdf_path_relative = files("smc.robots.robot_descriptions.urdf").joinpath(
"ur5e_with_robotiq_hande_FIXED_PATHS.urdf"
)
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- mesh_dir = files("ur_simple_control")
+ mesh_dir = files("smc")
+ mesh_dir = mesh_dir.joinpath("robots/robot_descriptions")
mesh_dir_absolute = os.path.abspath(mesh_dir)
+ print(mesh_dir_absolute)
shoulder_trans = np.array([0, 0, 0.1625134425523304])
shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
@@ -116,7 +118,7 @@ def get_model():
model.jointPlacements[5] = wrist_2_se3
model.jointPlacements[6] = wrist_3_se3
# TODO: fix where the fingers end up by setting a better position here (or maybe not here idk)
- #model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
+ # model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq))
data = pin.Data(model)
return model, collision_model, visual_model, data
@@ -173,9 +175,9 @@ def getGripperlessUR5e():
# after you've removed camera joint and similar.
def get_heron_model():
- # urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
+ # urdf_path_relative = files('smc.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
urdf_path_absolute = "/home/gospodar/home2/gospodar/lund/praxis/software/ros/ros-containers/home/model.urdf"
- # mesh_dir = files('ur_simple_control')
+ # mesh_dir = files('smc')
# mesh_dir_absolute = os.path.abspath(mesh_dir)
mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
@@ -200,11 +202,9 @@ def get_heron_model():
def get_yumi_model():
- urdf_path_relative = files("ur_simple_control.robot_descriptions").joinpath(
- "yumi.urdf"
- )
+ urdf_path_relative = files("smc.robot_descriptions").joinpath("yumi.urdf")
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- # mesh_dir = files('ur_simple_control')
+ # mesh_dir = files('smc')
# mesh_dir_absolute = os.path.abspath(mesh_dir)
# mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
mesh_dir_absolute = None
diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py
index ca67340..2f6404c 100644
--- a/python/smc/robots/implementations/ur5e.py
+++ b/python/smc/robots/implementations/ur5e.py
@@ -115,7 +115,7 @@ class RobotManagerU5eReal(RobotManagerUR5e):
self._wrench = mapping.T @ self._wrench_base
def sendVelocityCommand(self, v):
- v = super().sendVelocityCommand(v)
+ jdflkdahfjkdlsa nkjdf;jnsao
# speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
self._rtde_control.speedJ(v, self._acceleration, self._dt)
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 32aee3b..16f8ce1 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -149,7 +149,6 @@ class RobotManagerAbstract(abc.ABC):
- use getters to get copies of the variables you want
"""
- @abc.abstractmethod
def sendVelocityCommand(self, v):
"""
sendVelocityCommand
@@ -162,7 +161,11 @@ class RobotManagerAbstract(abc.ABC):
assert type(v) == np.ndarray
assert len(v) == self.model.nv
v = np.clip(v, -1 * self._max_v, self._max_v)
- return v
+ self.actualSendCommand(v)
+
+ @abc.abstractmethod
+ def actualSendCommand(self, v):
+ raise NotImplementedError()
# TODO: make faux gripper class to avoid this bullshit here
def openGripper(self):
--
GitLab