diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index a442dff4d7668a4df729fb1036c21f76ede319ca..97057c8041c533926c4dc301dcf62c471326ec28 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/robots/robotmanager_abstract.py b/python/ur_simple_control/robots/robotmanager_abstract.py
index c3e60b2b1b72d7dca089591004ce238dd21f9453..06de3ef2359fe8c108143e7e1b585efa313b0de8 100644
--- a/python/ur_simple_control/robots/robotmanager_abstract.py
+++ b/python/ur_simple_control/robots/robotmanager_abstract.py
@@ -48,8 +48,8 @@ class RobotManagerAbstract(abc.ABC):
self._log_manager = LogManager(args)
# TODO: add -1 option here meaning as fast as possible
- self._update_rate: int = self.args.ctrl_freq # Hz
- self._dt = 1 / self._update_rate
+ self._update_rate: int = self.args.ctrl_freq # [Hz]
+ self._dt = 1 / self._update_rate # [s]
self._t: float = 0.0
# defined per robot - hardware limit
self._MAX_ACCELERATION: float | np.ndarray # in joint space
@@ -64,6 +64,12 @@ class RobotManagerAbstract(abc.ABC):
# internally clipped to this
self._max_qd = args.max_qd
+ # 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_q = np.zeros(self.model.nv)
+ self.a_q = np.zeros(self.model.nv)
+
self.gripper = None
if (self.args.gripper != "none") and self.args.real:
if self.args.gripper == "robotiq":
@@ -73,12 +79,6 @@ class RobotManagerAbstract(abc.ABC):
if self.args.gripper == "onrobot":
self.gripper = TWOFG()
- # 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_q = np.zeros(self.model.nv)
- self.a_q = np.zeros(self.model.nv)
-
# start visualize manipulator process if selected.
# since there is only one robot and one visualizer, this is robotmanager's job
self.visualizer_manager: None | ProcessManager
diff --git a/python/ur_simple_control/robots/simulated_robot.py b/python/ur_simple_control/robots/simulated_robot.py
index 9adcb661f489096b846bd60e6522f9f6b52e0a0c..066af4db934da8db98b7ae5bf846c0ad980d803d 100644
--- a/python/ur_simple_control/robots/simulated_robot.py
+++ b/python/ur_simple_control/robots/simulated_robot.py
@@ -1,4 +1,5 @@
from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+from ur_simple_control.util.get_model import *
class RobotManagerSimulated(RobotManagerAbstract):
diff --git a/python/ur_simple_control/robots/ur5e.py b/python/ur_simple_control/robots/ur5e.py
index 311314e2e04bd47ae1794a2615655df8baba045c..32e2b3ffa7b2973c031f2869d7ee42403034c22d 100644
--- a/python/ur_simple_control/robots/ur5e.py
+++ b/python/ur_simple_control/robots/ur5e.py
@@ -3,6 +3,9 @@ from ur_simple_control.robots.single_arm_interface import SingleArmInterface
from ur_simple_control.util.get_model import get_model
import numpy as np
import pinocchio as pin
+from rtde_control import RTDEControlInterface
+from rtde_receive import RTDEReceiveInterface
+from rtde_io import RTDEIOInterface
# NOTE: this one assumes a jaw gripper
@@ -16,34 +19,13 @@ class RobotManagerUR5e(SingleArmInterface):
# you really should control the acceleration via the acceleration argument.
# we need to set it to 1.0 with ur_rtde so that's why it's here and explicitely named
self._speed_slider = 1.0 # const
- # TODO: remove gripper degrees of freedom from the model, it just ruins stuff
- if self.robot_name == "ur5e":
- self.q[-1] = 0.0
- self.q[-2] = 0.0
- if self.args.real:
- # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
- # if this produces errors like "already in use", and it's not already in use,
- # try just running your new program again. it could be that the socket wasn't given
- # back to the os even though you've shut off the previous program.
- print("CONNECTING TO UR5e!")
- self.rtde_control = RTDEControlInterface(self.args.robot_ip)
- self.rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
- self.rtde_io = RTDEIOInterface(self.args.robot_ip)
- self.rtde_io.setSpeedSlider(self.args.speed_slider)
- # NOTE: the force/torque sensor just has large offsets for no reason,
- # and you need to minus them to have usable readings.
- # we provide this with calibrateFT
- self.wrench_offset = self.calibrateFT()
- else:
- self.wrench_offset = np.zeros(6)
+ self.connectToRobot()
self.setInitialPose()
- if not args.real and args.start_from_current_pose:
+ if not self.args.real and self.args.start_from_current_pose:
self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
q = self.rtde_receive.getActualQ()
- q.append(0.0)
- q.append(0.0)
q = np.array(q)
self.q = q
if args.visualize_manipulator:
@@ -61,3 +43,21 @@ class RobotManagerUR5e(SingleArmInterface):
)
if self.args.real:
self.q = np.array(self.rtde_receive.getActualQ())
+
+ def connectToRobot(self):
+ if self.args.real:
+ # NOTE: you can't connect twice, so you can't have more than one RobotManager per robot.
+ # if this produces errors like "already in use", and it's not already in use,
+ # try just running your new program again. it could be that the socket wasn't given
+ # back to the os even though you've shut off the previous program.
+ print("CONNECTING TO UR5e!")
+ self.rtde_control = RTDEControlInterface(self.args.robot_ip)
+ self.rtde_receive = RTDEReceiveInterface(self.args.robot_ip)
+ self.rtde_io = RTDEIOInterface(self.args.robot_ip)
+ self.rtde_io.setSpeedSlider(self.args.speed_slider)
+ # NOTE: the force/torque sensor just has large offsets for no reason,
+ # and you need to minus them to have usable readings.
+ # we provide this with calibrateFT
+ self.wrench_offset = self.calibrateFT()
+ else:
+ self.wrench_offset = np.zeros(6)
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 bfffa078c75645aea48a88d67231247adeaa2e8c..434ecea554d17d59d10efe9ced81a04145265c2d 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/get_model.py b/python/ur_simple_control/util/get_model.py
index 351790f769dc0a74fcfb4eeb4cf5189ea4d4a227..23d4525d7a623f7837f35ecd02446d954eca14fa 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -115,6 +115,8 @@ def get_model():
model.jointPlacements[4] = wrist_1_se3
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))
data = pin.Data(model)
return model, collision_model, visual_model, data