diff --git a/python/smc/__init__.py b/python/smc/__init__.py index 581c6076306a0e96d4483a397e7a423e9c86243c..80bfd4616a268b4b234dd7f7f0b6e0311a547422 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 23d4525d7a623f7837f35ecd02446d954eca14fa..87a285da828a530f9e36aa9bca8fb850b59d15a3 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 ca67340cac1815188ab758cb51098300967f8cab..2f6404c9dfc8bde73097151e7dccbc9bdafffeb7 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 32aee3ba47d33fe0c701333e502c2c4610c2d334..16f8ce149161abf50c5e68dddba642bac8c54bff 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):