From 5dae9fa6763569b4de6441da5220f34b2f88b09b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 3 Feb 2025 17:30:41 +0100
Subject: [PATCH] before merge
---
.../__pycache__/managers.cpython-311.pyc | Bin 61250 -> 61250 bytes
.../robots/robotmanager_abstract.py | 16 +++---
.../robots/simulated_robot.py | 1 +
python/ur_simple_control/robots/ur5e.py | 46 +++++++++---------
.../__pycache__/get_model.cpython-311.pyc | Bin 18548 -> 18550 bytes
python/ur_simple_control/util/get_model.py | 2 +
6 files changed, 34 insertions(+), 31 deletions(-)
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
GIT binary patch
delta 22
ecmX?fkNMC&X71&@yj%<n3=B#S7Hs4;e-8j#Ne7_-
delta 22
ecmX?fkNMC&X71&@yj%<n3=DROvo>;@zXt$V^#?cr
diff --git a/python/ur_simple_control/robots/robotmanager_abstract.py b/python/ur_simple_control/robots/robotmanager_abstract.py
index c3e60b2..06de3ef 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 9adcb66..066af4d 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 311314e..32e2b3f 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
GIT binary patch
delta 108
zcmew|f$`e}M!w~|yj%<n3=HNE7NpM#-pH56%h<oUl(&SL^(z|#5C7&j0($I>wVS0S
z&ayDB+x$|-n2Yh`W?6MxE=G;b(S~^(jP09GS*mg|-rW4b`75(fFe9Vc2L>cEXYzOV
FS^#76BYyw@
delta 106
zcmex1f$_@(M!w~|yj%<n3=F$2%}D<kw2?23m$7$qDQ^ig^IdMf&F=*C*cq!g%SxPO
zVO+EMwTv+r<MGY%>b6{rYMWyX^Een=H=nUo<zl?P`IGZkW}!evMzaqLNMy$3pYF8)
DhYKVa
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 351790f..23d4525 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
--
GitLab