diff --git a/python/examples/graz/clik_client.py b/python/examples/graz/clik_client.py
index f7f29d0cab415101c62e4d6f8455a8b287fc66b2..1615311fde4a19fa890bd9e9dafaa9f55401e167 100644
--- a/python/examples/graz/clik_client.py
+++ b/python/examples/graz/clik_client.py
@@ -44,8 +44,8 @@ def controlLoopClikExternalGoal(
T_w_e = robot.getT_w_e()
data = receiver.getData()
T_goal = data["T_goal"]
- print("translation:", T_goal.translation)
- print("rotation:", pin.Quaternion(T_goal.rotation))
+ #print("translation:", T_goal.translation)
+ #print("rotation:", pin.Quaternion(T_goal.rotation))
SEerror = T_w_e.actInv(T_goal)
err_vector = pin.log6(SEerror).vector
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
diff --git a/python/examples/graz/joint_copier_client.py b/python/examples/graz/joint_copier_client.py
index 0ed3acb8fff8131d216bd7717925a21cfd2abfcc..00bc0af5ec6ad272150b5b4d92fbf535e9be73f7 100644
--- a/python/examples/graz/joint_copier_client.py
+++ b/python/examples/graz/joint_copier_client.py
@@ -70,7 +70,7 @@ if __name__ == "__main__":
if args.save_log:
robot.log_manager.plotAllControlLoops()
- if args.visualize_manipulator:
+ if args.visualizer:
robot.killManipulatorVisualizer()
if args.save_log:
diff --git a/python/examples/graz/message_specs_pb2.py b/python/examples/graz/message_specs_pb2.py
new file mode 120000
index 0000000000000000000000000000000000000000..ef7c007cfe238804ade48e005ff5d63b52bf974c
--- /dev/null
+++ b/python/examples/graz/message_specs_pb2.py
@@ -0,0 +1 @@
+../../ur_simple_control/networking/message_specs_pb2.py
\ No newline at end of file
diff --git a/python/examples/graz/protobuf_to_ros1.py b/python/examples/graz/protobuf_to_ros1.py
new file mode 100644
index 0000000000000000000000000000000000000000..c01d29d1e482fa3929566eed695b9e3a168575fa
--- /dev/null
+++ b/python/examples/graz/protobuf_to_ros1.py
@@ -0,0 +1,74 @@
+import socket
+from google.protobuf.internal.encoder import _VarintBytes
+from google.protobuf.internal.decoder import _DecodeVarint32
+import time
+import argparse
+import message_specs_pb2
+
+def getArgs():
+ parser = argparse.ArgumentParser()
+ parser.description = "get ros1 message, make it a protobuf message, send it via tcp socket"
+ parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1")
+ parser.add_argument("--port", type=int, help="host's port", default=6666)
+
+ args = parser.parse_args()
+ return args
+
+
+def parse_message(buffer):
+ """
+ parse_message
+ -------------
+ here the message is what we got from recv(),
+ and parsing it refers to finding serialized pb2 messages in it
+ NOTE: we only keep the latest message because
+ we're not going to do anything with the missed message.
+ the assumption is that we only get the same kind of message from
+ a sensor or something similar, not files or whatever else needs to be whole
+ """
+ pos, next_pos = 0, 0
+ buffer_len = len(buffer)
+ msg_in_bytes = b""
+ len_size_offset = 0
+ while True:
+ next_pos, pos = _DecodeVarint32(buffer, pos)
+ if pos + next_pos > buffer_len:
+ return msg_in_bytes, pos - len_size_offset
+ msg_in_bytes = _VarintBytes(pos + next_pos) + buffer[pos : pos + next_pos]
+ len_size_offset = len(_VarintBytes(pos + next_pos))
+ pos += next_pos
+ if pos >= buffer_len:
+ return msg_in_bytes, pos
+
+args = getArgs()
+host_addr = (args.host, args.port)
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+buffer = b""
+while True:
+ try:
+ s.connect(host_addr)
+ break
+ except ConnectionRefusedError:
+ time.sleep(0.005)
+print("NETWORKING CLIENT: connected to server")
+
+try:
+ while True:
+ msg_raw = s.recv(1024)
+ buffer += msg_raw
+ msg_in_bytes, pos = parse_message(buffer)
+ buffer = buffer[pos:]
+
+ next_pos, pos = 0, 0
+ next_pos, pos = _DecodeVarint32(msg_in_bytes, pos)
+ pb2_msg_in_bytes_cut = msg_in_bytes[pos : pos + next_pos]
+ pb2_msg = message_specs_pb2.wrench()
+ pb2_msg.ParseFromString(pb2_msg_in_bytes_cut)
+
+ print(pb2_msg.wrench)
+ # here you send construct and send the ros message
+ time.sleep(0.002)
+
+except KeyboardInterrupt:
+ s.close()
+ print("NETWORKING_CLIENT: caught KeyboardInterrupt, i'm out")
diff --git a/python/examples/graz/ros1_to_protobuf.py b/python/examples/graz/ros1_to_protobuf.py
new file mode 100644
index 0000000000000000000000000000000000000000..a9659f99cf790c4e20701fb1256dba2a02d24652
--- /dev/null
+++ b/python/examples/graz/ros1_to_protobuf.py
@@ -0,0 +1,46 @@
+import socket
+from google.protobuf.internal.encoder import _VarintBytes
+import argparse
+import message_specs_pb2
+import numpy as np
+import time
+
+
+
+def getArgs():
+ parser = argparse.ArgumentParser()
+ parser.description = "get ros1 message, make it a protobuf message, send it via tcp socket"
+ parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1")
+ parser.add_argument("--port", type=int, help="host's port", default=7777)
+
+ args = parser.parse_args()
+ return args
+
+args = getArgs()
+host_addr = (args.host, args.port)
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.bind(host_addr)
+
+print("listening on port ", args.port)
+s.listen()
+comm_socket, comm_addr = s.accept()
+# we're only accepting a single connection
+s.close()
+print("NETWORKING_SERVER: accepted a client", comm_addr)
+try:
+ while True:
+ # listen to a pre-determined ros1 message here
+ pb2_msg = message_specs_pb2.joint_angles()
+ pb2_msg.q.extend(np.random.random(8))
+ print(pb2_msg)
+ msg_length = pb2_msg.ByteSize()
+ msg_serialized = pb2_msg.SerializeToString()
+ msg = _VarintBytes(msg_length) + msg_serialized
+ comm_socket.send(msg)
+ time.sleep(0.002)
+except KeyboardInterrupt:
+ if args.debug_prints:
+ print("NETWORKING_SERVER: caugth KeyboardInterrupt, networking server out")
+
+
+comm_socket.close()
diff --git a/python/ur_simple_control/networking/util.py b/python/ur_simple_control/networking/util.py
index a3b5e98aed4f1952615278d2719a78a07f6bfc62..4b3bf8b00e23653740fa4e0aadf0e9918ebd55ca 100644
--- a/python/ur_simple_control/networking/util.py
+++ b/python/ur_simple_control/networking/util.py
@@ -1,8 +1,11 @@
+# TODO: make protobuf an optional import
+# then if it's not there use pickling as default
import ur_simple_control.networking.message_specs_pb2 as message_specs_pb2
from google.protobuf.internal.encoder import _VarintBytes
from google.protobuf.internal.decoder import _DecodeVarint32
import numpy as np
import pinocchio as pin
+import pickle
"""
to make the data API uniform across the library (dictionaries),
@@ -32,7 +35,6 @@ want to look at protobuf in the rest of the code
class DictPb2EncoderDecoder:
def __init__(self):
# TODO: fill this in with more messages
- # int_to_message = {0: message_specs_pb2.joint_angles, 6: message_specs_pb2.wrenches}
self.key_to_index = {
"q": 0,
"wrench": 1,
@@ -43,6 +45,7 @@ class DictPb2EncoderDecoder:
# if you want to embed message codes into messages,
# you'll need to use this
# stupidest possible solution
+
# TODO: implement own bidirectional dict for instead of this
# self.index_to_key = {self.key_to_index[key]: key for key in self.key_to_index}
@@ -76,7 +79,6 @@ class DictPb2EncoderDecoder:
message. ask someone who actually knows network programming
what makes more sense
"""
- # pb2_msg = int_to_message[msg_code]
msg_code = self.dictToMsgCode(dict_msg)
if msg_code == 1:
pb2_msg = message_specs_pb2.joint_angles()
@@ -179,3 +181,16 @@ class DictPb2EncoderDecoder:
dict_msg["v"] = np.array(pb2_msg.velocity)
return dict_msg
+
+
+class DictPickleWithHeaderEncoderDecoder:
+ def __init__(self):
+ self.HEADERSIZE = 10
+ self.buffer = b""
+
+ def dictToSerializedMsg(self, dict_msg : dict):
+ msg = pickle.dumps(dict_msg)
+ return bytes(f"{len(msg)}:<{self.HEADERSIZE}", 'utf-8')+msg
+
+ def what(self):
+ pass
diff --git a/python/ur_simple_control/robots/heron.py b/python/ur_simple_control/robots/heron.py
index 5701db459e244e0d66b43c57e78e24d00ae55bd1..beb6909144d235ce416c1647bcd7f856daa075e3 100644
--- a/python/ur_simple_control/robots/heron.py
+++ b/python/ur_simple_control/robots/heron.py
@@ -4,3 +4,4 @@ class RobotManagerHeron(RobotManagerAbstract):
self.model, self.collision_model, self.visual_model, self.data = (
heron_approximation()
)
+ self.ee_frame_id = self.model.getFrameId("tool0")
diff --git a/python/ur_simple_control/robots/mobile_yumi.py b/python/ur_simple_control/robots/mobile_yumi.py
index 3e087f162d084b2daa8d32decfb192f7cd92a88a..8abda3f0a984564f3ad277a1c077cea9ec2356c6 100644
--- a/python/ur_simple_control/robots/mobile_yumi.py
+++ b/python/ur_simple_control/robots/mobile_yumi.py
@@ -4,3 +4,5 @@ class RobotManagerMobileYumi(RobotManagerAbstract):
self.args = args
self.model, self.collision_model, self.visual_model, self.data = (
get_yumi_model()
+ self.r_ee_frame_id = self.model.getFrameId("robr_joint_7")
+ self.l_ee_frame_id = self.model.getFrameId("robl_joint_7")
diff --git a/python/ur_simple_control/robots/robotmanager_abstract.py b/python/ur_simple_control/robots/robotmanager_abstract.py
index a47a0e4422a87159d9d4f5b37377672ebe4ffda3..c3e60b2b1b72d7dca089591004ce238dd21f9453 100644
--- a/python/ur_simple_control/robots/robotmanager_abstract.py
+++ b/python/ur_simple_control/robots/robotmanager_abstract.py
@@ -1,12 +1,18 @@
import abc
import argparse
import pinocchio as pin
+import numpy as np
+from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
from ur_simple_control.util.logging_utils import LogManager
+from functools import partial
+from ur_simple_control.visualize.visualize import manipulatorVisualizer
###################################
# TODO: make everything different on each robot an abstract method.
# TODO: try to retain non-robot defined functions like _step, getters, setters etc
+# TODO: rename pinocchio_only to real (also toggle the boolean!)
############################################
@@ -19,17 +25,8 @@ class RobotManagerAbstract(abc.ABC):
- right now it is assumed you're running this on UR5e so some
magic numbers are just put to it.
this will be extended once there's a need for it.
- - at this stage it's just a boilerplate reduction class
- but the idea is to inherit it for more complicated things
- with many steps, like dmp.
- or just showe additional things in, this is python after all
- - you write your controller separately,
- and then drop it into this - there is a wrapper function you put
- around the control loop which handles timing so that you
- actually run at 500Hz and not more.
- - this is probably not the most new-user friendly solution,
- but it's designed for fastest idea to implementation rate.
- - if this was a real programming language, all of these would really be private variables.
+ - it's just a boilerplate reduction class
+ - if this was a real programming language, a lot of variables would really be private variables.
as it currently stands, "private" functions have the '_' prefix
while the public getters don't have a prefix.
- TODO: write out default arguments needed here as well
@@ -40,56 +37,35 @@ class RobotManagerAbstract(abc.ABC):
# might be changed later if that seems more appropriate
@abc.abstractmethod
def __init__(self, args):
- self.args: argparse.Namespace
- self.robot_name: str = args.robot
- self.model: pin.Model
+ self.args: argparse.Namespace # const
+ self.robot_name: str = args.robot # const
+ self.model: pin.Model # const
self.data: pin.Data
self.visual_model: pin.pinocchio_pywrap_default.GeometryModel
self.collision_model: pin.pinocchio_pywrap_default.GeometryModel
if args.save_log:
- self.log_manager = LogManager(args)
+ self._log_manager = LogManager(args)
- # last joint because pinocchio adds base frame as 0th joint.
- # and since this is unintuitive, we add the other variable too
- # so that the control designer doesn't need to think about such bs
- # self.JOINT_ID = 6
- self.ee_frame_id = self.model.getFrameId("tool0")
- if self.robot_name == "yumi":
- self.r_ee_frame_id = self.model.getFrameId("robr_joint_7")
- self.l_ee_frame_id = self.model.getFrameId("robl_joint_7")
- # JUST FOR TEST
- # NOTE
- # NOTE
- # NOTE
- self.ee_frame_id = self.model.getFrameId("robr_joint_7")
- # self.ee_frame_id = self.model.getFrameId("hande_right_finger_joint")
# TODO: add -1 option here meaning as fast as possible
- self.update_rate = args.ctrl_freq # Hz
- self.dt = 1 / self.update_rate
- # you better not give me crazy stuff
- # and i'm not clipping it, you're fixing it
- self.MAX_ACCELERATION = 1.7
- assert args.acceleration <= self.MAX_ACCELERATION and args.acceleration > 0.0
- # this is the number passed to speedj
- self.acceleration = args.acceleration
- # NOTE: this is evil and everything only works if it's set to 1
- # you really should control the acceleration via the acceleration argument.
- assert args.speed_slider <= 1.0 and args.acceleration > 0.0
- # TODO: these are almost certainly higher
- # NOTE and TODO: speed slider is evil, put it to 1, handle the rest yourself.
- # NOTE: i have no idea what's the relationship between max_qdd and speed slider
- # self.max_qdd = 1.7 * args.speed_slider
- # NOTE: this is an additional kinda evil speed limitation (by this code, not UR).
- # we're clipping joint velocities with this.
- # if your controllers are not what you expect, you might be commanding a very high velocity,
- # which is clipped, resulting in unexpected movement.
- self.MAX_QD = 3.14
- assert args.max_qd <= self.MAX_QD and args.max_qd > 0.0
- self.max_qd = args.max_qd
+ self._update_rate: int = self.args.ctrl_freq # Hz
+ self._dt = 1 / self._update_rate
+ self._t: float = 0.0
+ # defined per robot - hardware limit
+ self._MAX_ACCELERATION: float | np.ndarray # in joint space
+ assert (
+ self.args.acceleration <= self._MAX_ACCELERATION
+ and self.args.acceleration > 0.0
+ )
+ self._acceleration = self.args.acceleration
+ # defined per robot - hardware limit
+ self._MAX_QD: float | np.ndarray
+ assert self.args.max_qd <= self._MAX_QD and self.args.max_qd > 0.0
+ # internally clipped to this
+ self._max_qd = args.max_qd
self.gripper = None
- if (self.args.gripper != "none") and not self.pinocchio_only:
+ if (self.args.gripper != "none") and self.args.real:
if self.args.gripper == "robotiq":
self.gripper = RobotiqGripper()
self.gripper.connect(args.robot_ip, 63352)
@@ -97,23 +73,16 @@ class RobotManagerAbstract(abc.ABC):
if self.args.gripper == "onrobot":
self.gripper = TWOFG()
- # TODO: specialize for each robot,
- # add reasonable home positions
- self.q = pin.randomConfiguration(
- self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
- )
- if self.robot_name == "ur5e":
- self.q[-1] = 0.0
- self.q[-2] = 0.0
- # v_q is the generalization of qd for every type of joint.
- # for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
+ # 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)
- # same note as v_q, but it's a_q.
self.a_q = np.zeros(self.model.nv)
# start visualize manipulator process if selected.
- # has to be started here because it lives throughout the whole run
- if args.visualize_manipulator:
+ # since there is only one robot and one visualizer, this is robotmanager's job
+ self.visualizer_manager: None | ProcessManager
+ if args.visualizer:
side_function = partial(
manipulatorVisualizer,
self.model,
@@ -124,47 +93,44 @@ class RobotManagerAbstract(abc.ABC):
args, side_function, {"q": self.q.copy()}, 0
)
if args.visualize_collision_approximation:
- pass
- # TODO: import the ellipses here, and write an update ellipse function
- # then also call that in controlloopmanager
-
- # initialize and connect the interfaces
- if not args.pinocchio_only:
- # NOTE: you can't connect twice, so you can't have more than one RobotManager.
- # 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(args.robot_ip)
- self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
- self.rtde_io = RTDEIOInterface(args.robot_ip)
- self.rtde_io.setSpeedSlider(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()
+ # TODO: import the ellipses here, and write an update ellipse function
+ # then also call that in controlloopmanager
+ raise NotImplementedError(
+ "sorry, almost done - look at utils to get 90% of the solution!"
+ )
else:
- self.wrench_offset = np.zeros(6)
+ self.visualizer_manager = None
- self.speed_slider = args.speed_slider
+ self.connectToRobot()
- if args.pinocchio_only and 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:
- self.visualizer_manager.sendCommand({"q": q})
+ # wrench being the force-torque sensor reading, if any
+ self.wrench_offset = np.zeros(6)
- # do it once to get T_w_e
+ self.setInitialPose()
self._step()
#######################################################################
# getters which assume you called step() #
#######################################################################
+ # NOTE: just do nothing if you're only integrating with pinocchio,
+ # or start a physics simulator if you're using it
+ @abc.abstractmethod
+ def connectToRobot(self):
+ pass
+
+ @abc.abstractmethod
+ def setInitialPose(self):
+ """
+ for example, if just integrating, do:
+ self.q = pin.randomConfiguration(
+ self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
+ )
+ NOTE: you probably want to specialize this for your robot
+ to have it reasonable (no-self collisions, particular home etc)
+ """
+ pass
+
def getQ(self):
return self.q.copy()
diff --git a/python/ur_simple_control/robots/single_arm_interface.py b/python/ur_simple_control/robots/single_arm_interface.py
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..c12ad68a1c44e1760120bbae6f506f5c536be76e 100644
--- a/python/ur_simple_control/robots/single_arm_interface.py
+++ b/python/ur_simple_control/robots/single_arm_interface.py
@@ -0,0 +1,9 @@
+import abs
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+
+
+class SingleArmInterface(RobotManagerAbstract):
+ def __init__(self):
+ # idk if this is correct
+ super().__init__
+ self.ee_frame_id = self.model.getFrameId("tool0")
diff --git a/python/ur_simple_control/robots/ur5e.py b/python/ur_simple_control/robots/ur5e.py
index 710db78556e1db8319c9c110eb5636f3b3ffd209..311314e2e04bd47ae1794a2615655df8baba045c 100644
--- a/python/ur_simple_control/robots/ur5e.py
+++ b/python/ur_simple_control/robots/ur5e.py
@@ -1,8 +1,63 @@
from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+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
-class RobotManagerUR5e(RobotManagerAbstract):
+# NOTE: this one assumes a jaw gripper
+class RobotManagerUR5e(SingleArmInterface):
def __init__(self, args):
self.args = args
self.model, self.collision_model, self.visual_model, self.data = get_model()
+ self._MAX_ACCELERATION = 1.7 # const
+ self._MAX_QD = 3.14 # const
+ # NOTE: this is evil and everything only works if it's set to 1
+ # 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.setInitialPose()
+ if not args.real and 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:
+ self.visualizer_manager.sendCommand({"q": q})
+
+ def setInitialPose(self):
+ if not self.args.real and self.args.start_from_current_pose:
+ self.rtde_receive = RTDEReceiveInterface(args.robot_ip)
+ self.q = np.array(self.rtde_receive.getActualQ())
+ if self.args.visualize_manipulator:
+ self.visualizer_manager.sendCommand({"q": self.q})
+ if not self.args.real and not self.args.start_from_current_pose:
+ self.q = pin.randomConfiguration(
+ self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
+ )
+ if self.args.real:
+ self.q = np.array(self.rtde_receive.getActualQ())