diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc
index e6536f859f51b8b8a17c0032e2d11854161023ad..83e8dc1caa549a0c48d4b1b555bf9f81c9281138 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index 18f3958bb1c315fff9b9f78273c15d57a03f5cf6..a9cb9681827fd5bf80a51b5ad0c1febe78fd9923 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc differ
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc
index 47dfd580761c955ca39175dd03a865d137c0bcb2..22beafda9e5a29c24a3dc6f2ff7234ca612005bd 100644
Binary files a/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/clik/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc
index 94aa9adee31a8c24a5d1149f612b7a8a46762036..dd510be1a758c41f336d6bf9ef6dfa661f87a3c4 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-311.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-311.pyc
index ac6f7a73897befd20cabe6dae3478b5641c2b3b0..c5479ed52b6db36c9b993efa9e233fa29bc93cf1 100644
Binary files a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-311.pyc and b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-311.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 42f12c01a008f8b6368dd109dcdf9c3a8187958f..f3e8679282daff02ff39d10f0920870e402db2a6 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -13,7 +13,7 @@ from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripp
from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
import copy
import signal
-from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e, mir_approximation
+from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e, mir_approximation, get_yumi_model
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
@@ -86,7 +86,7 @@ def getMinimalArgParser():
#################################################
parser.add_argument('--robot', type=str, \
help="which robot you're running or simulating", default="ur5e", \
- choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e', 'mirros'])
+ choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e', 'mirros', 'yumi'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -286,6 +286,9 @@ class ControlLoopManager:
if self.robot_manager.robot_name == "heron":
T_base = self.robot_manager.data.oMi[1]
self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
+# if self.robot_manager.robot_name == "yumi":
+# T_base = self.robot_manager.data.oMi[1]
+# self.robot_manager.visualizer_manager.sendCommand({"T_base" : T_base})
if self.args.real_time_plotting:
# don't put new stuff in if it didn't handle the previous stuff.
@@ -434,6 +437,9 @@ class RobotManager:
if self.robot_name == "gripperlessur5e":
self.model, self.collision_model, self.visual_model, self.data = \
getGripperlessUR5e()
+ if self.robot_name == "yumi":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ get_yumi_model()
# create log manager if we're saving logs
if args.save_log:
@@ -879,6 +885,11 @@ class RobotManager:
else:
self.v_q = qd_cmd
self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+
+ if self.robot_name == "yumi":
+ qd_cmd = np.clip(qd, -1 * self.model.velocityLimit, self.model.velocityLimit)
+ self.v_q = qd_cmd
+ self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc
index 73ad86e2073753e1f862d0c21f332c78f73adcef..dde9762240beb46abb3d77d92ef2770b4465aec4 100644
Binary files a/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/__init__.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 4e86aaacd804f06577e7d2305969dbbea1a0d4c8..5683b3c8a5c1c9547d5161bdd4dbf9898905d83e 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc
index d3487cec8901f7cebb860803e24fe99105eca82e..fbd1ed2ac5e0c105c2a48f4977dd704e6ef03896 100644
Binary files a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc and b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 5383e5bc9957fc8657a91162b3ebbf3922309efd..7d9cdc2da747113b5db62bb1d50db8e3d0248dac 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -169,6 +169,7 @@ def get_heron_model():
def get_yumi_model():
urdf_path_relative = files('ur_simple_control.robot_descriptions').joinpath('yumi.urdf')
+ urdf_path_absolute = os.path.abspath(urdf_path_relative)
#mesh_dir = files('ur_simple_control')
#mesh_dir_absolute = os.path.abspath(mesh_dir)
#mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc
index eecbbf0f43742bce3f6353efdf25edb8bec57e1b..e29f229bd9772a126ab3b1fdef93916ff8259324 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-311.pyc differ