diff --git a/python/ur_simple_control/robots/dual_arm_interface.py b/python/ur_simple_control/robots/dual_arm_interface.py
new file mode 100644
index 0000000000000000000000000000000000000000..023cb82542af81f3c06630b1c521ae0f1a28c5d2
--- /dev/null
+++ b/python/ur_simple_control/robots/dual_arm_interface.py
@@ -0,0 +1,71 @@
+import numpy as np
+import pinocchio as pin
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+
+
+class DualArmInterface(RobotManagerAbstract):
+ def __init__(self):
+ self._abs_frame: pin.SE3
+ # NOTE: you need to fill in the specific names from the urdf here for your robot
+ # self._l_ee_frame_id = self.model.getFrameId("left_ee_name")
+ # self._r_ee_frame_id = self.model.getFrameId("right_ee_name")
+
+ # l stands for left
+ # r stands for right
+ self._l_ee_frame_id: int
+ self._r_ee_frame_id: int
+
+ self._abs_T_w_e: pin.SE3
+ self._l_T_w_e: pin.SE3
+ self._r_T_w_e: pin.SE3
+
+ # TODO: you have to have relative frame transformations here
+ # (from absolute frame to left frame and to the right frame)
+ raise NotImplementedError
+
+ def getAbsT_w_e(self, q=None):
+ """
+ getAbsT_w_e
+ -----------
+ get absolute frame, as seen in base frame, based on the current, or provident joint configuration
+ TODO: figure out what's the best way to get this
+ """
+ raise NotImplementedError
+
+ def getLeftRightT_w_e(self, q=None):
+ """
+ getLeftRightT_w_e
+ -----------
+ returns a tuple (l_T_w_e, r_T_w_e), i.e. left and right end-effector frames in the base frame,
+ based on the current, or provident joint configuration
+ """
+ if q is None:
+ return (self._l_T_w_e.copy(), self._r_T_w_e.copy())
+ assert type(q) is np.ndarray
+ # NOTE:
+ # calling forward kinematics and updateFramePlacements here is ok
+ # because we rely on robotmanager attributes instead of model.something in the rest of the code,
+ # i.e. this won't update this class' atribute
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ q,
+ # np.zeros(self.model.nv),
+ # np.zeros(self.model.nv),
+ )
+ # NOTE: this also returns the frame, so less copying possible
+ # pin.updateFramePlacements(self.model, self.data)
+ pin.updateFramePlacement(self.model, self.data, self._l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._r_ee_frame_id)
+ return (
+ self.data.oMf[self._l_ee_frame_id].copy(),
+ self.data.oMf[self._r_ee_frame_id].copy(),
+ )
+
+ def forwardKinematics(self):
+ pin.updateFramePlacement(self.model, self.data, self._l_ee_frame_id)
+ pin.updateFramePlacement(self.model, self.data, self._r_ee_frame_id)
+ self.l_T_w_e = self.data.oMf[self._l_ee_frame_id].copy()
+ self.r_T_w_e = self.data.oMf[self._r_ee_frame_id].copy()
+ # TODO: contruct the absolute frame from the relative frames here!!!
+ raise NotImplementedError
diff --git a/python/ur_simple_control/robots/force_torque_sensor_interface.py b/python/ur_simple_control/robots/force_torque_sensor_interface.py
new file mode 100644
index 0000000000000000000000000000000000000000..e1ce9feec98bcc644b94397ceb64293f7488aef8
--- /dev/null
+++ b/python/ur_simple_control/robots/force_torque_sensor_interface.py
@@ -0,0 +1,92 @@
+import abc
+import numpy as np
+import time
+
+
+class ForceTorqueSensorInterface(abc.ABC):
+ def __init__(self):
+ # in base frame
+ self._wrench_base = np.zeros(6)
+ # whatever you set to be the frame
+ self._ft_sensor_frame_id: int
+ self._wrench = np.zeros(6)
+ # NOTE: wrench bias will be defined in the frame your sensor's gives readings
+ self._wrench_bias = np.zeros(6)
+
+ def getWrench(self):
+ """
+ getWrench
+ ---------
+ returns UNFILTERED! force-torque sensor reading, i.e. wrench,
+ in the your prefered frame
+ NOTE: it is given in the end-effector frame by default
+ """
+ return self._wrench.copy()
+
+ def getWrenchBase(self):
+ """
+ getWrench
+ ---------
+ returns UNFILTERED! force-torque sensor reading, i.e. wrench,
+ in the base frame of the robot
+ """
+ return self._wrench_base.copy()
+
+ @abc.abstractmethod
+ def _getWrench(self):
+ """
+ get wrench reading from whatever interface you have.
+ NOTE:
+ 1) it is YOUR job to know in which frame the reading is, and to map this
+ BOTH to the base frame of the robot AND your prefered frame.
+ this is because it is impossible to know what's the default on your sensor or where it is.
+ this way you are forced to verify and know in which frame you get your readings from.
+ 2) do NOT do any filtering in this method - that happens elsewhere - here we just want the reading
+ in correct frames
+ 3) you can count on your RobotManager to have already called forward kinematics before this function, i.e.
+ you can use frames stored in your RobotManager's attributes
+ 4) if self.args.real == False, provide noise for better simulation testing
+ """
+ pass
+
+ def _getWrenchInEE(self, step_called=False):
+ if self.robot_name != "yumi":
+ else:
+ self.wrench = np.zeros(6)
+
+ def calibrateFT(self, robot):
+ """
+ calibrateFT
+ -----------
+ TODO: make generic
+ Read from the f/t sensor a bit, average the results
+ and return the result.
+ This can be used to offset the bias of the f/t sensor.
+ NOTE: this is not an ideal solution.
+ ALSO TODO: test whether the offset changes when
+ the manipulator is in different poses.
+ """
+ ft_readings = []
+ print("Will read from f/t sensors for a some number of seconds")
+ print("and give you the average.")
+ print("Use this as offset.")
+ # NOTE: zeroFtSensor() needs to be called frequently because it drifts
+ # by quite a bit in a matter of minutes.
+ # if you are running something on the robot for a long period of time, you need
+ # to reapply zeroFtSensor() to get reasonable results.
+ # because the robot needs to stop for the zeroing to make sense,
+ # this is the responsibility of the user!!!
+ robot.rtde_control.zeroFtSensor()
+ for i in range(2000):
+ start = time.time()
+ ft = robot.rtde_receive.getActualTCPForce()
+ ft_readings.append(ft)
+ end = time.time()
+ diff = end - start
+ if diff < self.dt:
+ time.sleep(self.dt - diff)
+
+ ft_readings = np.array(ft_readings)
+ self.wrench_offset = np.average(ft_readings, axis=0)
+ print(self.wrench_offset)
+ return self.wrench_offset.copy()
diff --git a/python/ur_simple_control/robots/heron.py b/python/ur_simple_control/robots/heron.py
deleted file mode 100644
index beb6909144d235ce416c1647bcd7f856daa075e3..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/robots/heron.py
+++ /dev/null
@@ -1,7 +0,0 @@
-class RobotManagerHeron(RobotManagerAbstract):
- def __init__(self, args):
- self.args = args
- 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/implementations/heron.py b/python/ur_simple_control/robots/implementations/heron.py
new file mode 100644
index 0000000000000000000000000000000000000000..67b3223d7a4bd2ebe21cd62ba6d3e33153b5c427
--- /dev/null
+++ b/python/ur_simple_control/robots/implementations/heron.py
@@ -0,0 +1,62 @@
+import time
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+
+
+class RobotManagerHeron(RobotManagerAbstract):
+ def __init__(self, args):
+ self.args = args
+ self.model, self.collision_model, self.visual_model, self.data = (
+ heron_approximation()
+ )
+ self.ee_frame_id = self.model.getFrameId("tool0")
+
+
+class RobotManagerHeronReal(RobotManagerHeron):
+ def __init__(self, args):
+ super().__init__(args)
+
+ # TODO: here assert you need to have ros2 installed to run on real heron
+ # and then set all this bullshit here instead of elsewhere
+ def set_publisher_vel_base(self, publisher_vel_base):
+ self.publisher_vel_base = publisher_vel_base
+ print("set vel_base_publisher into robotmanager")
+
+ def set_publisher_vel_left(self, publisher_vel_left):
+ self.publisher_vel_left = publisher_vel_left
+ print("set vel_left_publisher into robotmanager")
+
+ def set_publisher_vel_right(self, publisher_vel_right):
+ self.publisher_vel_right = publisher_vel_right
+ print("set vel_right_publisher into robotmanager")
+
+ def sendVelocityCommand(self, v):
+ v = super().sendVelocityCommand(v)
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ self.rtde_control.speedJ(v[3:], self._acceleration, self._dt)
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = v[0]
+ # already clipped away, but no cost in being more explicit
+ # about underactuation
+ cmd_msg.linear.y = 0.0
+ cmd_msg.angular.z = v[2]
+ self.publisher_vel_base.publish(cmd_msg)
+ # good to keep because updating is slow otherwise
+ # it's not correct, but it's more correct than not updating
+ # self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+ def stopRobot(self):
+ print("stopping via freedrive lel")
+ self._rtde_control.freedriveMode()
+ time.sleep(0.5)
+ self._rtde_control.endFreedriveMode()
+ raise NotImplementedError(
+ "idk how to stop the base lmao. the arm should be the same as for ur5e"
+ )
+
+ def setFreedrive(self):
+ # NOTE: only works for the arm
+ self._rtde_control.FreedriveMode()
+
+ def unSetFreedrive(self):
+ # NOTE: only works for the arm
+ self._rtde_control.endFreedriveMode()
diff --git a/python/ur_simple_control/robots/implementations/mir.py b/python/ur_simple_control/robots/implementations/mir.py
new file mode 100644
index 0000000000000000000000000000000000000000..ae5a9c394a2ef89b3537175c5c1deacd33950493
--- /dev/null
+++ b/python/ur_simple_control/robots/implementations/mir.py
@@ -0,0 +1,35 @@
+ sendVelocityCommand(self, v):
+ """
+ sendVelocityCommand
+ -----
+ different things need to be send depending on whether you're running a simulation,
+ you're on a real robot, you're running some new simulator bla bla. this is handled
+ here because this things depend on the arguments which are manager here (hence the
+ class name RobotManager)
+ """
+ # y-direction is not possible on heron
+ qd[1] = 0
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = qd[0]
+ cmd_msg.angular.z = qd[2]
+ # print("about to publish", cmd_msg)
+ self.publisher_vel_base.publish(cmd_msg)
+ # good to keep because updating is slow otherwise
+ # it's not correct, but it's more correct than not updating
+ # self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+
+
+ def stopRobot(self):
+ print("stopping via freedrive lel")
+ self._rtde_control.freedriveMode()
+ time.sleep(0.5)
+ self._rtde_control.endFreedriveMode()
+
+ def setFreedrive(self):
+ self._rtde_control.freedriveMode()
+ raise NotImplementedError("freedrive function only written for ur5e")
+
+ def unSetFreedrive(self):
+ self._rtde_control.endFreedriveMode()
+ raise NotImplementedError("freedrive function only written for ur5e")
diff --git a/python/ur_simple_control/robots/implementations/mobile_yumi.py b/python/ur_simple_control/robots/implementations/mobile_yumi.py
new file mode 100644
index 0000000000000000000000000000000000000000..7cf3176e3349d9cda9d65cc86c5beeaa49305085
--- /dev/null
+++ b/python/ur_simple_control/robots/implementations/mobile_yumi.py
@@ -0,0 +1,41 @@
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+
+
+class RobotManagerMobileYumi(RobotManagerAbstract, DualArmInterface):
+ def __init__(self, args):
+ 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")
+
+
+class RobotManagerMobileYumiReal(RobotManagerMobileYumi):
+ def __init__(self, args):
+ super().__init__(args)
+
+ # TODO: here assert you need to have ros2 installed to run on real heron
+ # and then set all this bullshit here instead of elsewhere
+ def set_publisher_joints_cmd(self, publisher_joints_cmd):
+ self.publisher_joints_cmd = publisher_joints_cmd
+ print("set publisher_joints_cmd into RobotManager")
+
+ def sendVelocityCommand(self, v):
+ # qd_base = qd[:3]
+ # qd_left = qd[3:10]
+ # qd_right = qd[10:]
+ # self.publisher_vel_base(qd_base)
+ # self.publisher_vel_left(qd_left)
+ # self.publisher_vel_right(qd_right)
+ empty_msg = JointState()
+ for i in range(29):
+ empty_msg.velocity.append(0.0)
+ msg = empty_msg
+ msg.header.stamp = Time().to_msg()
+ for i in range(3):
+ msg.velocity[i] = qd_cmd[i]
+ for i in range(15, 29):
+ msg.velocity[i] = qd_cmd[i - 12]
+
+ self.publisher_joints_cmd.publish(msg)
diff --git a/python/ur_simple_control/robots/implementations/simulated_robot.py b/python/ur_simple_control/robots/implementations/simulated_robot.py
new file mode 100644
index 0000000000000000000000000000000000000000..43bcb015976f59af1c78f9b95d9551f97e636b19
--- /dev/null
+++ b/python/ur_simple_control/robots/implementations/simulated_robot.py
@@ -0,0 +1,18 @@
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+from ur_simple_control.util.get_model import *
+
+
+class RobotManagerSimulated(RobotManagerAbstract):
+ # TODO: find a way for this to inherit from the particular robot manager (the non-real part)
+ def __init__(self, args):
+ pass
+
+ def sendVelocityCommand(self, v):
+ """
+ sendVelocityCommand
+ -----
+ in simulation we just integrate the velocity for a dt and that's it
+ """
+ v = super().sendVelocityCommand(v)
+ self._v = v
+ self._q = pin.integrate(self.model, self._q, v * self._dt)
diff --git a/python/ur_simple_control/robots/implementations/ur5e.py b/python/ur_simple_control/robots/implementations/ur5e.py
new file mode 100644
index 0000000000000000000000000000000000000000..ae1c74822927ed148f74685f20a67ee30d844c38
--- /dev/null
+++ b/python/ur_simple_control/robots/implementations/ur5e.py
@@ -0,0 +1,134 @@
+from ur_simple_control.robots.single_arm_interface import SingleArmInterface
+from ur_simple_control.robots.force_torque_sensor_interface import (
+ ForceTorqueSensorInterface,
+)
+from ur_simple_control.util.get_model import get_model
+import numpy as np
+import pinocchio as pin
+
+# TODO: make rtde optional - ofc assert it has to exist in UR5e real constructor
+from rtde_control import RTDEControlInterface
+from rtde_receive import RTDEReceiveInterface
+from rtde_io import RTDEIOInterface
+from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
+import time
+
+
+# NOTE: this one assumes a jaw gripper
+class RobotManagerUR5e(SingleArmInterface, ForceTorqueSensorInterface):
+ 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
+
+
+class RobotManagerU5eReal(RobotManagerUR5e):
+ def __init__(self, args):
+ super().__init__(args)
+ # NOTE: UR's sleep slider is evil and nothing works unless if it's set to 1.0!!!
+ # you have to 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
+ self._rtde_control: RTDEControlInterface
+ self._rtde_receive: RTDEReceiveInterface
+ self._rtde_io: RTDEIOInterface
+ self.connectToRobot()
+ self.connectToGripper()
+ self.setInitialPose()
+
+ def connectToGripper(self):
+ if (self.args.gripper != "none") and self.args.real:
+ if self.args.gripper == "robotiq":
+ self.gripper = RobotiqGripper()
+ self.gripper.connect(self.args.robot_ip, 63352)
+ self.gripper.activate()
+ if self.args.gripper == "onrobot":
+ self.gripper = TWOFG()
+ if self.args.gripper == "none":
+ self.gripper = None
+
+ def setInitialPose(self):
+ if not self.args.real and self.args.start_from_current_pose:
+ self._rtde_receive = RTDEReceiveInterface(self.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())
+
+ 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)
+
+ def setSpeedSlider(self, value):
+ """
+ setSpeedSlider
+ ---------------
+ update in all places
+ """
+ assert value <= 1.0 and value > 0.0
+ if not self.args.pinocchio_only:
+ self._rtde_io.setSpeedSlider(value)
+ self.speed_slider = value
+
+ def _getQ(self):
+ q = self._rtde_receive.getActualQ()
+ self._q = np.array(q)
+
+ def _getV(self):
+ v = self._rtde_receive.getActualQd()
+ self._v = np.array(v)
+
+ def _getWrench(self):
+ # NOTE: UR5e's ft-sensors gives readings in robot's base frame
+ if not self.args.real:
+ self._wrench_base = (
+ np.array(self._rtde_receive.getActualTCPForce()) - self._wrench_bias
+ )
+ else:
+ # TODO: do something better here (at least a better distribution)
+ self._wrench_base = np.random.random(6)
+ # NOTE: we define the default wrench to be given in the end-effector frame
+ mapping = np.zeros((6, 6))
+ mapping[0:3, 0:3] = self._T_w_e.rotation
+ mapping[3:6, 3:6] = self._T_w_e.rotation
+ self._wrench = mapping.T @ self._wrench_base
+
+ def sendVelocityCommand(self, v):
+ v = super().sendVelocityCommand(v)
+ # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
+ self._rtde_control.speedJ(v, self._acceleration, self._dt)
+
+ def stopRobot(self):
+ print("stopping via freedrive lel")
+ self._rtde_control.freedriveMode()
+ time.sleep(0.5)
+ self._rtde_control.endFreedriveMode()
+
+ def setFreedrive(self):
+ self._rtde_control.freedriveMode()
+ raise NotImplementedError("freedrive function only written for ur5e")
+
+ def unSetFreedrive(self):
+ self._rtde_control.endFreedriveMode()
+ raise NotImplementedError("freedrive function only written for ur5e")
diff --git a/python/ur_simple_control/robots/mobile_base_interface.py b/python/ur_simple_control/robots/mobile_base_interface.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/python/ur_simple_control/robots/mobile_yumi.py b/python/ur_simple_control/robots/mobile_yumi.py
deleted file mode 100644
index 8abda3f0a984564f3ad277a1c077cea9ec2356c6..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/robots/mobile_yumi.py
+++ /dev/null
@@ -1,8 +0,0 @@
-
-class RobotManagerMobileYumi(RobotManagerAbstract):
- def __init__(self, args):
- 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 06de3ef2359fe8c108143e7e1b585efa313b0de8..d097fadd55c627e9207493a603c8b445c2a655f7 100644
--- a/python/ur_simple_control/robots/robotmanager_abstract.py
+++ b/python/ur_simple_control/robots/robotmanager_abstract.py
@@ -2,19 +2,11 @@ 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!)
-############################################
-
class RobotManagerAbstract(abc.ABC):
"""
@@ -22,14 +14,10 @@ class RobotManagerAbstract(abc.ABC):
---------------
- design goal: rely on pinocchio as much as possible while
concealing obvious bookkeeping
- - 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.
- - 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
+ - this class serves to:
+ - store the non-interfacing information
+ about the robot like maximum allowed velocity, pin.model etc
+ - provide functions every robot has to have like getQ
"""
# just pass all of the arguments here and store them as is
@@ -52,6 +40,12 @@ class RobotManagerAbstract(abc.ABC):
self._dt = 1 / self._update_rate # [s]
self._t: float = 0.0
# defined per robot - hardware limit
+ # TODO TODO TODO:
+ # these max things need to be arrays
+ # and if nothing is set via arguments, set it to model.velocityLimit by default!!!!!!!
+ # qd_cmd = np.clip(
+ # qd, -1 * self.model.velocityLimit, self.model.velocityLimit
+ # )
self._MAX_ACCELERATION: float | np.ndarray # in joint space
assert (
self.args.acceleration <= self._MAX_ACCELERATION
@@ -59,30 +53,21 @@ class RobotManagerAbstract(abc.ABC):
)
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
+ self._MAX_V: float | np.ndarray
+ assert self.args.max_v <= self._MAX_V and self.args.max_v > 0.0
# internally clipped to this
- self._max_qd = args.max_qd
+ self._max_v = args.max_v
# 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.v = np.zeros(self.model.nv)
+ self.a = np.zeros(self.model.nv)
- self.gripper = None
- if (self.args.gripper != "none") and self.args.real:
- if self.args.gripper == "robotiq":
- self.gripper = RobotiqGripper()
- self.gripper.connect(args.robot_ip, 63352)
- self.gripper.activate()
- if self.args.gripper == "onrobot":
- self.gripper = TWOFG()
-
- # 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
- if args.visualizer:
+ self.visualizer_manager = None
+ if self.args.visualizer:
side_function = partial(
manipulatorVisualizer,
self.model,
@@ -92,36 +77,22 @@ class RobotManagerAbstract(abc.ABC):
self.visualizer_manager = ProcessManager(
args, side_function, {"q": self.q.copy()}, 0
)
+ # TODO: move this bs to visualizer, there are 0 reasons to keep it here
if args.visualize_collision_approximation:
# 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.visualizer_manager = None
-
- self.connectToRobot()
- # wrench being the force-torque sensor reading, if any
- self.wrench_offset = np.zeros(6)
-
- self.setInitialPose()
+ self.gripper = None
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):
"""
+ setInitialPose
+ --------------
for example, if just integrating, do:
self.q = pin.randomConfiguration(
self.model, -1 * np.ones(self.model.nq), np.ones(self.model.nq)
@@ -134,418 +105,54 @@ class RobotManagerAbstract(abc.ABC):
def getQ(self):
return self.q.copy()
- def getQd(self):
- return self.v_q.copy()
-
- def getT_w_e(self, q_given=None):
- if self.robot_name != "yumi":
- if q_given is None:
- return self.T_w_e.copy()
- else:
- assert type(q_given) is np.ndarray
- # calling these here is ok because we rely
- # on robotmanager attributes instead of model.something
- # (which is copying data, but fully separates state and computation,
- # which is important in situations like this)
- pin.forwardKinematics(
- self.model,
- self.data,
- q_given,
- np.zeros(self.model.nv),
- np.zeros(self.model.nv),
- )
- # NOTE: this also returns the frame, so less copying possible
- # pin.updateFramePlacements(self.model, self.data)
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- return self.data.oMf[self.ee_frame_id].copy()
- else:
- if q_given is None:
- return self.T_w_e_left.copy(), self.T_w_e_right.copy().copy()
- else:
- assert type(q_given) is np.ndarray
- # calling these here is ok because we rely
- # on robotmanager attributes instead of model.something
- # (which is copying data, but fully separates state and computation,
- # which is important in situations like this)
- pin.forwardKinematics(
- self.model,
- self.data,
- q_given,
- np.zeros(self.model.nv),
- np.zeros(self.model.nv),
- )
- # NOTE: this also returns the frame, so less copying possible
- # pin.updateFramePlacements(self.model, self.data)
- pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
- pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
- return (
- self.data.oMf[self.l_ee_frame_id].copy(),
- self.data.oMf[self.r_ee_frame_id].copy(),
- )
-
- # this is in EE frame by default (handled in step which
- # is assumed to be called before this)
- def getWrench(self):
- return self.wrench.copy()
+ def getV(self):
+ return self.v.copy()
- def calibrateFT(self):
+ @abc.abstractmethod
+ def forwardKinematics(self):
"""
- calibrateFT
- -----------
- Read from the f/t sensor a bit, average the results
- and return the result.
- This can be used to offset the bias of the f/t sensor.
- NOTE: this is not an ideal solution.
- ALSO TODO: test whether the offset changes when
- the manipulator is in different poses.
+ forwardKinematics
+ -----------------
+ compute all the frames you care about based on current configuration
+ and put them into class attributes.
+ for a single manipulator with just the end-effector frame,
+ this is a pointless function, but there is more to do for dual
+ arm robots for example.
"""
- ft_readings = []
- print("Will read from f/t sensors for a some number of seconds")
- print("and give you the average.")
- print("Use this as offset.")
- # NOTE: zeroFtSensor() needs to be called frequently because it drifts
- # by quite a bit in a matter of minutes.
- # if you are running something on the robot for a long period of time, you need
- # to reapply zeroFtSensor() to get reasonable results.
- # because the robot needs to stop for the zeroing to make sense,
- # this is the responsibility of the user!!!
- self.rtde_control.zeroFtSensor()
- for i in range(2000):
- start = time.time()
- ft = self.rtde_receive.getActualTCPForce()
- ft_readings.append(ft)
- end = time.time()
- diff = end - start
- if diff < self.dt:
- time.sleep(self.dt - diff)
-
- ft_readings = np.array(ft_readings)
- self.wrench_offset = np.average(ft_readings, axis=0)
- print(self.wrench_offset)
- return self.wrench_offset.copy()
+ pass
+ @abc.abstractmethod
def _step(self):
"""
_step
----
- - the idea is to update everything that should be updated
- on a step-by-step basis
- - the actual problem this is solving is that you're not calling
+ Purpose:
+ - update everything that should be updated on a step-by-step basis
+ Reason for existance:
+ - the problem this is solving is that you're not calling
forwardKinematics, an expensive call, more than once per step.
- - within the TODO is to make all (necessary) variable private
- so that you can rest assured that everything is handled the way
- it's supposed to be handled. then have getters for these
- private variables which return deepcopies of whatever you need.
- that way the computations done in the control loop
- can't mess up other things. this is important if you want
- to switch between controllers during operation and have a completely
- painless transition between them.
- TODO: make the getQ, getQd and the rest here do the actual communication,
- and make these functions private.
- then have the deepcopy getters public.
- also TODO: make ifs for the simulation etc.
- this is less ifs overall right.
+ - you also don't need to remember to do call forwardKinematics
+ Usage:
+ - don't call this method yourself! this is CommandLoopManager's job
+ - use getters to get copies of the variables you want
"""
- self._getQ()
- self._getQd()
- # self._getWrench()
- # computeAllTerms is certainly not necessary btw
- # but if it runs on time, does it matter? it makes everything available...
- # (includes forward kinematics, all jacobians, all dynamics terms, energies)
- # pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
- pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
- if self.robot_name != "yumi":
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
- else:
- pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
- pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
- self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
- self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
- # wrench in EE should obviously be the default
- self._getWrenchInEE(step_called=True)
- # this isn't real because we're on a velocity-controlled robot,
- # so this is actually None (no tau, no a_q, as expected)
- self.a_q = self.data.ddq
- # TODO NOTE: you'll want to do the additional math for
- # torque controlled robots here, but it's ok as is rn
- def setSpeedSlider(self, value):
- """
- setSpeedSlider
- ---------------
- update in all places
- """
- assert value <= 1.0 and value > 0.0
- if not self.args.pinocchio_only:
- self.rtde_io.setSpeedSlider(value)
- self.speed_slider = value
-
- def _getQ(self):
- """
- _getQ
- -----
- NOTE: private function for use in _step(), use the getter getQ()
- urdf treats gripper as two prismatic joints,
- but they do not affect the overall movement
- of the robot, so we add or remove 2 items to the joint list.
- also, the gripper is controlled separately so we'd need to do this somehow anyway
- NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
- TODO: make work for new gripper
- """
- if not self.pinocchio_only:
- q = self.rtde_receive.getActualQ()
- if self.args.gripper == "robotiq":
- # TODO: make it work or remove it
- # self.gripper_past_pos = self.gripper_pos
- # this is pointless by itself
- self.gripper_pos = self.gripper.get_current_position()
- # the /255 is to get it dimensionless.
- # the gap is 5cm,
- # thus half the gap is 0.025m (and we only do si units here).
- q.append((self.gripper_pos / 255) * 0.025)
- q.append((self.gripper_pos / 255) * 0.025)
- else:
- # just fill it with zeros otherwise
- if self.robot_name == "ur5e":
- q.append(0.0)
- q.append(0.0)
- # let's just have both options for getting q, it's just a 8d float list
- # readability is a somewhat subjective quality after all
- q = np.array(q)
- self.q = q
-
- # TODO remove evil hack
- def _getT_w_e(self, q_given=None):
- """
- _getT_w_e
- -----
- NOTE: private function, use the getT_w_e() getter
- urdf treats gripper as two prismatic joints,
- but they do not affect the overall movement
- of the robot, so we add or remove 2 items to the joint list.
- also, the gripper is controlled separately so we'd need to do this somehow anyway
- NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO.
- NOTE: don't use this if use called _step() because it repeats forwardKinematics
- """
- test = True
- try:
- test = q_given.all() == None
- print(test)
- print(q_given)
- except AttributeError:
- test = True
-
- if test:
- if not self.pinocchio_only:
- q = self.rtde_receive.getActualQ()
- if self.args.gripper == "robotiq":
- # TODO: make it work or remove it
- # self.gripper_past_pos = self.gripper_pos
- # this is pointless by itself
- self.gripper_pos = self.gripper.get_current_position()
- # the /255 is to get it dimensionless.
- # the gap is 5cm,
- # thus half the gap is 0.025m (and we only do si units here).
- q.append((self.gripper_pos / 255) * 0.025)
- q.append((self.gripper_pos / 255) * 0.025)
- else:
- # just fill it with zeros otherwise
- q.append(0.0)
- q.append(0.0)
- else:
- q = self.q
- else:
- q = copy.deepcopy(q_given)
- q = np.array(q)
- self.q = q
- pin.forwardKinematics(self.model, self.data, q)
- if self.robot_name != "yumi":
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
- else:
- pin.updateFramePlacement(self.model, self.data, self.l_ee_frame_id)
- pin.updateFramePlacement(self.model, self.data, self.r_ee_frame_id)
- self.T_w_e_left = self.data.oMf[self.l_ee_frame_id].copy()
- self.T_w_e_right = self.data.oMf[self.r_ee_frame_id].copy()
- # NOTE: VERY EVIL, to bugfix things that depend on this like wrench (which we don't have on yumi)
- # self.T_w_e = self.data.oMf[self.l_ee_frame_id].copy()
-
- def _getQd(self):
- """
- _getQd
- -----
- NOTE: private function, use the _getQd() getter
- same note as _getQ.
- TODO NOTE: atm there's no way to get current gripper velocity.
- this means you'll probably want to read current positions and then finite-difference
- to get the velocity.
- as it stands right now, we'll just pass zeros in because I don't need this ATM
- """
- if not self.pinocchio_only:
- qd = self.rtde_receive.getActualQd()
- if self.args.gripper:
- # TODO: this doesn't work because we're not ensuring stuff is called
- # at every timestep
- # self.gripper_vel = (gripper.get_current_position() - self.gripper_pos) / self.dt
- # so it's just left unused for now - better give nothing than wrong info
- self.gripper_vel = 0.0
- # the /255 is to get it dimensionless
- # the gap is 5cm
- # thus half the gap is 0.025m and we only do si units here
- # no need to deepcopy because only literals are passed
- qd.append(self.gripper_vel)
- qd.append(self.gripper_vel)
- else:
- # just fill it with zeros otherwise
- qd.append(0.0)
- qd.append(0.0)
- # let's just have both options for getting q, it's just a 8d float list
- # readability is a somewhat subjective quality after all
- qd = np.array(qd)
- self.v_q = qd
-
- def _getWrenchRaw(self):
- """
- _getWrench
- -----
- different things need to be send depending on whether you're running a simulation,
- you're on a real robot, you're running some new simulator bla bla. this is handled
- here because this things depend on the arguments which are manager here (hence the
- class name RobotManager)
- """
- if not self.pinocchio_only:
- wrench = np.array(self.rtde_receive.getActualTCPForce())
- else:
- raise NotImplementedError("Don't have time to implement this right now.")
-
- def _getWrench(self):
- if not self.pinocchio_only:
- self.wrench = (
- np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
- )
- else:
- # TODO: do something better here (at least a better distribution)
- self.wrench = np.random.random(self.n_arm_joints)
-
- def _getWrenchInEE(self, step_called=False):
- if self.robot_name != "yumi":
- if not self.pinocchio_only:
- self.wrench = (
- np.array(self.rtde_receive.getActualTCPForce()) - self.wrench_offset
- )
- else:
- # TODO: do something better here (at least a better distribution)
- self.wrench = np.random.random(self.n_arm_joints)
- if not step_called:
- self._getT_w_e()
- # NOTE: this mapping is equivalent to having a purely rotational action
- # this is more transparent tho
- mapping = np.zeros((6, 6))
- mapping[0:3, 0:3] = self.T_w_e.rotation
- mapping[3:6, 3:6] = self.T_w_e.rotation
- self.wrench = mapping.T @ self.wrench
- else:
- self.wrench = np.zeros(6)
-
- def sendQd(self, qd):
+ @abc.abstractmethod
+ def sendVelocityCommand(self, v):
"""
- sendQd
+ sendVelocityCommand
-----
different things need to be send depending on whether you're running a simulation,
you're on a real robot, you're running some new simulator bla bla. this is handled
here because this things depend on the arguments which are manager here (hence the
class name RobotManager)
"""
- # we're hiding the extra 2 prismatic joint shenanigans from the control writer
- # because there you shouldn't need to know this anyway
- if self.robot_name == "ur5e":
- qd_cmd = qd[:6]
- # np.clip is ok with bounds being scalar, it does what it should
- # (but you can also give it an array)
- qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
- if not self.pinocchio_only:
- # speedj(qd, scalar_lead_axis_acc, hangup_time_on_command)
- self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
- else:
- # this one takes all 8 elements of qd since we're still in pinocchio
- # this is ugly, todo: fix
- qd = qd[:6]
- qd = qd_cmd.reshape((6,))
- qd = list(qd)
- qd.append(0.0)
- qd.append(0.0)
- qd = np.array(qd)
- self.v_q = qd
- self.q = pin.integrate(self.model, self.q, qd * self.dt)
-
- if self.robot_name == "heron":
- # y-direction is not possible on heron
- qd_cmd = np.clip(
- qd, -1 * self.model.velocityLimit, self.model.velocityLimit
- )
- # qd[1] = 0
- self.v_q = qd_cmd
- self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
-
- if self.robot_name == "heronros":
- # y-direction is not possible on heron
- qd[1] = 0
- cmd_msg = msg.Twist()
- cmd_msg.linear.x = qd[0]
- cmd_msg.angular.z = qd[2]
- # print("about to publish", cmd_msg)
- self.publisher_vel_base.publish(cmd_msg)
- # good to keep because updating is slow otherwise
- # it's not correct, but it's more correct than not updating
- # self.q = pin.integrate(self.model, self.q, qd * self.dt)
-
- if self.robot_name == "mirros":
- # y-direction is not possible on heron
- qd[1] = 0
- cmd_msg = msg.Twist()
- cmd_msg.linear.x = qd[0]
- cmd_msg.angular.z = qd[2]
- # print("about to publish", cmd_msg)
- self.publisher_vel_base.publish(cmd_msg)
- # good to keep because updating is slow otherwise
- # it's not correct, but it's more correct than not updating
- # self.q = pin.integrate(self.model, self.q, qd * self.dt)
-
- if self.robot_name == "gripperlessur5e":
- qd_cmd = np.clip(qd, -1 * self.max_qd, self.max_qd)
- if not self.pinocchio_only:
- self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
- 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, -0.01 * self.model.velocityLimit, 0.01 * self.model.velocityLimit
- )
- self.v_q = qd_cmd
- # if self.args.pinocchio_only:
- # self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
- # else:
- # qd_base = qd[:3]
- # qd_left = qd[3:10]
- # qd_right = qd[10:]
- # self.publisher_vel_base(qd_base)
- # self.publisher_vel_left(qd_left)
- # self.publisher_vel_right(qd_right)
- empty_msg = JointState()
- for i in range(29):
- empty_msg.velocity.append(0.0)
- msg = empty_msg
- msg.header.stamp = Time().to_msg()
- for i in range(3):
- msg.velocity[i] = qd_cmd[i]
- for i in range(15, 29):
- msg.velocity[i] = qd_cmd[i - 12]
-
- self.publisher_joints_cmd.publish(msg)
+ assert type(v) == np.ndarray
+ assert len(v) == self.model.nv
+ v = np.clip(v, -1 * self._max_v, self._max_v)
+ return v
+ # TODO: make faux gripper class to avoid this bullshit here
def openGripper(self):
if self.gripper is None:
if self.args.debug_prints:
@@ -558,6 +165,7 @@ class RobotManagerAbstract(abc.ABC):
else:
print("not implemented yet, so nothing is going to happen!")
+ # TODO: make faux gripper class to avoid this bullshit here
def closeGripper(self):
if self.gripper is None:
if self.args.debug_prints:
@@ -570,78 +178,6 @@ class RobotManagerAbstract(abc.ABC):
else:
print("not implemented yet, so nothing is going to happen!")
- #######################################################################
- # utility functions #
- #######################################################################
-
- def defineGoalPointCLI(self):
- """
- defineGoalPointCLI
- ------------------
- NOTE: this assume _step has not been called because it's run before the controlLoop
- --> best way to handle the goal is to tell the user where the gripper is
- in both UR tcp frame and with pinocchio and have them
- manually input it when running.
- this way you force the thinking before the moving,
- but you also get to view and analyze the information first
- TODO get the visual thing you did in ivc project with sliders also.
- it's just text input for now because it's totally usable, just not superb.
- but also you do want to have both options. obviously you go for the sliders
- in the case you're visualizing, makes no sense otherwise.
- """
- self._getQ()
- q = self.getQ()
- # define goal
- pin.forwardKinematics(self.model, self.data, np.array(q))
- pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
- T_w_e = self.data.oMf[self.ee_frame_id]
- print("You can only specify the translation right now.")
- if not self.pinocchio_only:
- print(
- "In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles"
- )
- print(
- "Here's where the robot is currently. Ensure you know what the base frame is first."
- )
- print(
- "base frame end-effector pose from pinocchio:\n",
- *self.data.oMi[6].translation.round(4),
- *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4)
- )
- print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
- # remain with the current orientation
- # TODO: add something, probably rpy for orientation because it's the least number
- # of numbers you need to type in
- Mgoal = T_w_e.copy()
- # this is a reasonable way to do it too, maybe implement it later
- # Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
- # do a while loop until this is parsed correctly
- while True:
- goal = input(
- "Please enter the target end-effector position in the x.x,y.y,z.z format: "
- )
- try:
- e = "ok"
- goal_list = goal.split(",")
- for i in range(len(goal_list)):
- goal_list[i] = float(goal_list[i])
- except:
- e = exc_info()
- print("The input is not in the expected format. Try again.")
- print(e)
- if e == "ok":
- Mgoal.translation = np.array(goal_list)
- break
- print("this is goal pose you defined:\n", Mgoal)
-
- # NOTE i'm not deepcopying this on purpose
- # but that might be the preferred thing, we'll see
- self.Mgoal = Mgoal
- if self.args.visualize_manipulator:
- # TODO document this somewhere
- self.visualizer_manager.sendCommand({"Mgoal": Mgoal})
- return Mgoal
-
def killManipulatorVisualizer(self):
"""
killManipulatorVisualizer
@@ -656,25 +192,6 @@ class RobotManagerAbstract(abc.ABC):
"""
self.visualizer_manager.terminateProcess()
- def stopRobot(self):
- if not self.args.pinocchio_only:
- print("stopping via freedrive lel")
- self.rtde_control.freedriveMode()
- time.sleep(0.5)
- self.rtde_control.endFreedriveMode()
-
- def setFreedrive(self):
- if self.robot_name in ["ur5e", "gripperlessur5e"]:
- self.rtde_control.freedriveMode()
- else:
- raise NotImplementedError("freedrive function only written for ur5e")
-
- def unSetFreedrive(self):
- if self.robot_name in ["ur5e", "gripperlessur5e"]:
- self.rtde_control.endFreedriveMode()
- else:
- raise NotImplementedError("freedrive function only written for ur5e")
-
def updateViz(self, viz_dict: dict):
"""
updateViz
@@ -689,18 +206,114 @@ class RobotManagerAbstract(abc.ABC):
if self.args.debug_prints:
print("you didn't select viz")
- def set_publisher_vel_base(self, publisher_vel_base):
- self.publisher_vel_base = publisher_vel_base
- print("set vel_base_publisher into robotmanager")
- def set_publisher_vel_left(self, publisher_vel_left):
- self.publisher_vel_left = publisher_vel_left
- print("set vel_left_publisher into robotmanager")
+class RobotManagerRealAbstract(RobotManagerAbstract, abc.ABC):
+ """
+ RobotManagerRealAbstract
+ ------------------------
+ enumerate the templates that the interface for your real robot
+ has to have, connectToRobot
+ """
- def set_publisher_vel_right(self, publisher_vel_right):
- self.publisher_vel_right = publisher_vel_right
- print("set vel_right_publisher into robotmanager")
+ def __init__(self, args):
+ super().__init__(args)
+ self.connectToRobot()
+ self.connectToGripper()
+ self.setInitialPose()
- def set_publisher_joints_cmd(self, publisher_joints_cmd):
- self.publisher_joints_cmd = publisher_joints_cmd
- print("set publisher_joints_cmd into RobotManager")
+ @abc.abstractmethod
+ def connectToRobot(self):
+ """
+ connectToRobot
+ --------------
+ create whatever is necessary to:
+ 1) send commands to the robot
+ 2) receive state information
+ 3) set options if applicable
+ Setting up a gripper, force-torque sensor and similar is handled separately
+ """
+ pass
+
+ @abc.abstractmethod
+ def connectToGripper(self):
+ """
+ connectToGripper
+ --------------
+ create a gripper class based on selected argument.
+ this needs to be made for each robot individually because even if the gripper
+ is the same, the robot is likely to have its own way of interfacing it
+ """
+ pass
+
+ @abc.abstractmethod
+ def _getQ(self):
+ """
+ update internal _q joint angles variable with current angles obtained
+ from your robot's communication interface
+ """
+ pass
+
+ @abc.abstractmethod
+ def _getV(self):
+ """
+ update internal _v joint velocities variable with current velocities obtained
+ from your robot's communication interface
+ """
+ pass
+
+ def _step(self):
+ self._getQ()
+ self._getV()
+ # self._getWrench()
+
+ # NOTE: when implementing an interface for torque-controlled robots,
+ # use computeAllTerms to make life easier
+
+ self.forwardKinematics()
+ # wrench in EE should obviously be the default
+ self._getWrenchInEE(step_called=True)
+ # this isn't real because we're on a velocity-controlled robot,
+ # so this is actually None (no tau, no a_q, as expected)
+ self.a_q = self.data.ddq
+ # TODO NOTE: you'll want to do the additional math for
+ # torque controlled robots here, but it's ok as is rn
+
+ @abc.abstractmethod
+ def stopRobot(self):
+ """
+ stopRobot
+ ---------
+ implement whatever stops the robot as cleanly as possible on your robot.
+ does not need to be emergency option, could be sending zero velocity
+ commands, could be switching to freedrive/leadthrough/whatever.
+
+ depending on the communication interface on your robot,
+ it could happen that it stays in "externally guided motion" mode,
+ i.e. it keeps listening to new velocity commands, while executing the last velocity command.
+ such a scenario is problematic because they last control command might not be
+ all zeros, meaning the robot will keep slowly moving and hit something after 5min
+ (obviously this exact thing happened to the author...)
+ """
+ pass
+
+ @abc.abstractmethod
+ def setFreedrive(self):
+ """
+ setFreedrive
+ ------------
+ set the robot into the "teaching mode", "freedrive mode", "leadthrough mode"
+ or whatever the term is for when you can manually (with your hands) move the robot
+ around
+ """
+ pass
+
+ def unSetFreedrive(self):
+ """
+ setFreedrive
+ ------------
+ unset the robot into the "teaching mode", "freedrive mode", "leadthrough mode"
+ or whatever the term is for when you can manually (with your hands) move the robot
+ around, back to "wait" or "accept commands" or whatever mode the robot has to
+ receive external commands.
+ """
+ pass
diff --git a/python/ur_simple_control/robots/simulated_robot.py b/python/ur_simple_control/robots/simulated_robot.py
deleted file mode 100644
index 066af4db934da8db98b7ae5bf846c0ad980d803d..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/robots/simulated_robot.py
+++ /dev/null
@@ -1,31 +0,0 @@
-from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
-from ur_simple_control.util.get_model import *
-
-
-class RobotManagerSimulated(RobotManagerAbstract):
- def __init__(self, args):
- # TODO: idk where i pass args
- super(RobotManagerAbstract, self).__init__(args)
- if self.robot_name == "ur5e":
- self.model, self.collision_model, self.visual_model, self.data = get_model()
- if self.robot_name == "heron":
- self.model, self.collision_model, self.visual_model, self.data = (
- heron_approximation()
- )
- if self.robot_name == "heronros":
- self.model, self.collision_model, self.visual_model, self.data = (
- heron_approximation()
- )
- if self.robot_name == "mirros":
- self.model, self.collision_model, self.visual_model, self.data = (
- mir_approximation()
- )
- # self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
- # self.publisher_vel_base = publisher_vel_base
- 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()
diff --git a/python/ur_simple_control/robots/single_arm_interface.py b/python/ur_simple_control/robots/single_arm_interface.py
index c12ad68a1c44e1760120bbae6f506f5c536be76e..b3c09792c01ffd70487abe1bb111ed31cd5738bd 100644
--- a/python/ur_simple_control/robots/single_arm_interface.py
+++ b/python/ur_simple_control/robots/single_arm_interface.py
@@ -1,9 +1,34 @@
import abs
+import numpy as np
+import pinocchio as pin
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")
+ self._ee_frame_id = self.model.getFrameId("tool0")
+ self._T_w_e: pin.SE3
+
+ def getT_w_e(self, q=None):
+ if q is None:
+ return self._T_w_e.copy()
+ assert type(q) is np.ndarray
+ # calling these here is ok because we rely
+ # on robotmanager attributes instead of model.something
+ # (which is copying data, but fully separates state and computation,
+ # which is important in situations like this)
+ pin.forwardKinematics(
+ self.model,
+ self.data,
+ q,
+ # np.zeros(self.model.nv),
+ # np.zeros(self.model.nv),
+ )
+ # NOTE: this also returns the frame, so less copying possible
+ # pin.updateFramePlacements(self.model, self.data)
+ pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
+ return self.data.oMf[self._ee_frame_id].copy()
+
+ def forwardKinematics(self):
+ pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
+ self._T_w_e = self.data.oMf[self._ee_frame_id].copy()
diff --git a/python/ur_simple_control/robots/ur5e.py b/python/ur_simple_control/robots/ur5e.py
deleted file mode 100644
index 32e2b3ffa7b2973c031f2869d7ee42403034c22d..0000000000000000000000000000000000000000
--- a/python/ur_simple_control/robots/ur5e.py
+++ /dev/null
@@ -1,63 +0,0 @@
-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
-from rtde_control import RTDEControlInterface
-from rtde_receive import RTDEReceiveInterface
-from rtde_io import RTDEIOInterface
-
-
-# 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
-
- self.connectToRobot()
-
- self.setInitialPose()
- 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 = 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())
-
- 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/robots/utils.py b/python/ur_simple_control/robots/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..de3be244e94ea905a12d44fba23684c52cef1943
--- /dev/null
+++ b/python/ur_simple_control/robots/utils.py
@@ -0,0 +1,72 @@
+import numpy as np
+import pinocchio as pin
+
+# TODO: make robot-independent
+def defineGoalPointCLI(robot. robot):
+ """
+ defineGoalPointCLI
+ ------------------
+ get a nice TUI-type prompt to put in a frame goal for p2p motion.
+ --> best way to handle the goal is to tell the user where the gripper is
+ in both UR tcp frame and with pinocchio and have them
+ manually input it when running.
+ this way you force the thinking before the moving,
+ but you also get to view and analyze the information first
+ TODO get the visual thing you did in ivc project with sliders also.
+ it's just text input for now because it's totally usable, just not superb.
+ but also you do want to have both options. obviously you go for the sliders
+ in the case you're visualizing, makes no sense otherwise.
+ """
+ robot._step()
+ robot._getQ()
+ q = robot.getQ()
+ # define goal
+ pin.forwardKinematics(robot.model, robot.data, np.array(q))
+ pin.updateFramePlacement(robot.model, robot.data, robot.ee_frame_id)
+ T_w_e = robot.data.oMf[robot.ee_frame_id]
+ print("You can only specify the translation right now.")
+ if not robot.pinocchio_only:
+ print(
+ "In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles"
+ )
+ print(
+ "Here's where the robot is currently. Ensure you know what the base frame is first."
+ )
+ print(
+ "base frame end-effector pose from pinocchio:\n",
+ *robot.data.oMi[6].translation.round(4),
+ *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4)
+ )
+ print("UR5e TCP:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ # remain with the current orientation
+ # TODO: add something, probably rpy for orientation because it's the least number
+ # of numbers you need to type in
+ Mgoal = T_w_e.copy()
+ # this is a reasonable way to do it too, maybe implement it later
+ # Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # do a while loop until this is parsed correctly
+ while True:
+ goal = input(
+ "Please enter the target end-effector position in the x.x,y.y,z.z format: "
+ )
+ try:
+ e = "ok"
+ goal_list = goal.split(",")
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+
+ # NOTE i'm not deepcopying this on purpose
+ # but that might be the preferred thing, we'll see
+ robot.Mgoal = Mgoal
+ if robot.args.visualize_manipulator:
+ # TODO document this somewhere
+ robot.visualizer_manager.sendCommand({"Mgoal": Mgoal})
+ return Mgoal