diff --git a/python/smc/control/clik/clik.py b/python/smc/control/clik/clik.py
index e8e9a5810a80fbabaaa657917365a30b3abc166a..a65d2ed072c002ee1fc48390a465f9dd10569191 100644
--- a/python/smc/control/clik/clik.py
+++ b/python/smc/control/clik/clik.py
@@ -1,5 +1,5 @@
from smc.control.control_loop_manager import ControlLoopManager
-from smc.robots.robotmanager_abstract import RobotManagerAbstract
+from smc.robots.robotmanager_abstract import AbstractRobotManager
from smc.multiprocessing.process_manager import ProcessManager
import pinocchio as pin
@@ -358,7 +358,7 @@ def getClikController(args, robot):
return partial(dampedPseudoinverse, args.tikhonov_damp)
-def controlLoopClik(robot: RobotManagerAbstract, clik_controller, i, past_data):
+def controlLoopClik(robot: AbstractRobotManager, clik_controller, i, past_data):
"""
controlLoopClik
---------------
@@ -396,7 +396,7 @@ def controlLoopClik(robot: RobotManagerAbstract, clik_controller, i, past_data):
def controlLoopClikDualArm(
- robot: RobotManagerAbstract, clik_controller, goal_transform, i, past_data
+ robot: AbstractRobotManager, clik_controller, goal_transform, i, past_data
):
"""
controlLoopClikDualArm
@@ -499,7 +499,7 @@ def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
def moveUntilContactControlLoop(
- args, robot: RobotManagerAbstract, speed, clik_controller, i, past_data
+ args, robot: AbstractRobotManager, speed, clik_controller, i, past_data
):
"""
moveUntilContactControlLoop
@@ -561,7 +561,7 @@ def moveUntilContact(args, robot, speed):
print("Collision detected!!")
-def moveL(args, robot: RobotManagerAbstract, goal_point):
+def moveL(args, robot: AbstractRobotManager, goal_point):
"""
moveL
-----
@@ -590,7 +590,7 @@ def moveL(args, robot: RobotManagerAbstract, goal_point):
def moveLDualArm(
- args, robot: RobotManagerAbstract, goal_point, goal_transform, run=True
+ args, robot: AbstractRobotManager, goal_point, goal_transform, run=True
):
"""
moveL
@@ -635,7 +635,7 @@ def moveLFollowingLine(args, robot, goal_point):
def cartesianPathFollowingWithPlannerControlLoop(
- args, robot: RobotManagerAbstract, path_planner: ProcessManager, i, past_data
+ args, robot: AbstractRobotManager, path_planner: ProcessManager, i, past_data
):
"""
cartesianPathFollowingWithPlanner
@@ -698,7 +698,7 @@ def cartesianPathFollowingWithPlannerControlLoop(
def cartesianPathFollowingWithPlanner(
- args, robot: RobotManagerAbstract, path_planner: ProcessManager
+ args, robot: AbstractRobotManager, path_planner: ProcessManager
):
controlLoop = partial(
cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner
@@ -711,7 +711,7 @@ def cartesianPathFollowingWithPlanner(
loop_manager.run()
-def controlLoopCompliantClik(args, robot: RobotManagerAbstract, i, past_data):
+def controlLoopCompliantClik(args, robot: AbstractRobotManager, i, past_data):
"""
controlLoopClik
---------------
@@ -835,7 +835,7 @@ def clikCartesianPathIntoJointPath(
sim_args.visualizer = False
sim_args.save_log = False # we're not using sim robot outside of this
sim_args.max_iterations = 10000 # more than enough
- sim_robot = RobotManagerAbstract(sim_args)
+ sim_robot = AbstractRobotManager(sim_args)
sim_robot.q = q_init.copy()
sim_robot._step()
for pose in path:
@@ -870,7 +870,7 @@ def clikCartesianPathIntoJointPath(
def controlLoopClikDualArmsOnly(
- robot: RobotManagerAbstract, clik_controller, goal_transform, i, past_data
+ robot: AbstractRobotManager, clik_controller, goal_transform, i, past_data
):
"""
controlLoopClikDualArmsOnly
@@ -918,7 +918,7 @@ def controlLoopClikDualArmsOnly(
def moveLDualArmsOnly(
- args, robot: RobotManagerAbstract, goal_point, goal_transform, run=True
+ args, robot: AbstractRobotManager, goal_point, goal_transform, run=True
):
"""
moveL
@@ -948,7 +948,7 @@ def moveLDualArmsOnly(
if __name__ == "__main__":
args = get_args()
- robot = RobotManagerAbstract(args)
+ robot = AbstractRobotManager(args)
Mgoal = robot.defineGoalPointCLI()
clik_controller = getClikController(args, robot)
controlLoop = partial(controlLoopClik, robot, clik_controller)
diff --git a/python/smc/multiprocessing/networking/TODO b/python/smc/multiprocessing/networking/TODO
new file mode 100644
index 0000000000000000000000000000000000000000..9e843d64cb62df340ae9ec61d72e2057a7d6bcf7
--- /dev/null
+++ b/python/smc/multiprocessing/networking/TODO
@@ -0,0 +1 @@
+rename receiver and sender to subscriber and publisher
diff --git a/python/smc/robots/get_model.py b/python/smc/robots/get_model.py
index 87a285da828a530f9e36aa9bca8fb850b59d15a3..e416db92db9431842d2bbd070056285fe6365d43 100644
--- a/python/smc/robots/get_model.py
+++ b/python/smc/robots/get_model.py
@@ -43,9 +43,8 @@ def get_model():
)
urdf_path_absolute = os.path.abspath(urdf_path_relative)
mesh_dir = files("smc")
- mesh_dir = mesh_dir.joinpath("robots/robot_descriptions")
+ mesh_dir = mesh_dir.joinpath("robots")
mesh_dir_absolute = os.path.abspath(mesh_dir)
- print(mesh_dir_absolute)
shoulder_trans = np.array([0, 0, 0.1625134425523304])
shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
@@ -202,7 +201,7 @@ def get_heron_model():
def get_yumi_model():
- urdf_path_relative = files("smc.robot_descriptions").joinpath("yumi.urdf")
+ urdf_path_relative = files("smc.robots.robot_descriptions").joinpath("yumi.urdf")
urdf_path_absolute = os.path.abspath(urdf_path_relative)
# mesh_dir = files('smc')
# mesh_dir_absolute = os.path.abspath(mesh_dir)
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index 8d3a04e0e7ceb8eac31b12db17bf27424dee8897..e9b0d605394cf669a3aa2c03387b4012e159f6e7 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -1,11 +1,11 @@
-from smc.robots.robotmanager_abstract import RobotManagerAbstract
+from smc.robots.robotmanager_abstract import AbstractRobotManager
import abc
import numpy as np
import pinocchio as pin
-class DualArmInterface(RobotManagerAbstract):
+class DualArmInterface(AbstractRobotManager):
"""
DualArmInterface
----------------
@@ -14,42 +14,58 @@ class DualArmInterface(RobotManagerAbstract):
- r stands for right
"""
+ # NOTE: you need to fill in the specific names from the urdf here for your robot
+ # (better than putting in a magic number)
+ # self._l_ee_frame_id = self.model.getFrameId("left_ee_name")
+ # self._r_ee_frame_id = self.model.getFrameId("right_ee_name")
+ @property
@abc.abstractmethod
- 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")
+ def l_ee_frame_id(self) -> int: ...
+ @property
+ @abc.abstractmethod
+ def r_ee_frame_id(self) -> int: ...
- self._l_ee_frame_id: int
- self._r_ee_frame_id: int
+ # T_abs_l and T_abs_r are relative transformations between the absolute dual-arm frame,
+ # and the left and right end-effector frames respectively
+ @property
+ @abc.abstractmethod
+ def T_abs_r(self) -> pin.SE3: ...
+ @property
+ @abc.abstractmethod
+ def T_abs_l(self) -> pin.SE3: ...
- self._abs_T_w_e: pin.SE3
- self._l_T_w_e: pin.SE3
- self._r_T_w_e: pin.SE3
+ def __init__(self):
+ self._T_w_abs: pin.SE3
+ self._T_w_l: pin.SE3
+ self._T_w_r: 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):
+ def constructT_w_abs(self):
+ raise NotImplementedError
+
+ @property
+ def T_w_abs(self, q=None):
"""
- getAbsT_w_e
+ getT_w_abs
-----------
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
+ raise NotImplementedError("need to add the case where you construct it from q")
+ return self._T_w_abs.copy()
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,
+ returns a tuple (T_w_l, T_w_r), 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())
+ return (self._T_w_l.copy(), self._T_w_r.copy())
assert type(q) is np.ndarray
# NOTE:
# calling forward kinematics and updateFramePlacements here is ok
@@ -64,17 +80,17 @@ class DualArmInterface(RobotManagerAbstract):
)
# 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)
+ 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(),
+ 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()
- self.getAbsT_w_e()
+ 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_l = self.data.oMf[self.l_ee_frame_id].copy()
+ self._T_w_r = self.data.oMf[self.r_ee_frame_id].copy()
+ self.constructT_w_abs()
raise NotImplementedError
diff --git a/python/smc/robots/interfaces/force_torque_sensor_interface.py b/python/smc/robots/interfaces/force_torque_sensor_interface.py
index 89f1454c08f61b605fe9d178264966e1e9d7f7a0..268e9485abf8fa3f7a9c223e880bcb8d87df2693 100644
--- a/python/smc/robots/interfaces/force_torque_sensor_interface.py
+++ b/python/smc/robots/interfaces/force_torque_sensor_interface.py
@@ -11,12 +11,13 @@ class ForceTorqueSensorInterface(abc.ABC):
# NOTE: wrench bias will be defined in the frame your sensor's gives readings
self._wrench_bias = np.zeros(6)
+ # get this via model.getFrameId("frame_name")
@property
- def _ft_sensor_frame_id(self):
- # whatever you set to be the frame
- ...
+ @abc.abstractmethod
+ def ft_sensor_frame_id(self) -> int: ...
- def getWrench(self):
+ @property
+ def wrench(self):
"""
getWrench
---------
@@ -26,7 +27,8 @@ class ForceTorqueSensorInterface(abc.ABC):
"""
return self._wrench.copy()
- def getWrenchBase(self):
+ @property
+ def wrench_base(self):
"""
getWrench
---------
@@ -36,7 +38,7 @@ class ForceTorqueSensorInterface(abc.ABC):
return self._wrench_base.copy()
@abc.abstractmethod
- def _getWrench(self):
+ def getWrenchFromReal(self):
"""
get wrench reading from whatever interface you have.
NOTE:
@@ -52,10 +54,8 @@ class ForceTorqueSensorInterface(abc.ABC):
"""
pass
- def _getWrenchInEE(self, step_called=False):
- if self.robot_name != "yumi":
- else:
- self.wrench = np.zeros(6)
+ def _getWrenchInEE(self):
+ raise NotImplementedError
def calibrateFT(self, robot):
"""
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 16f8ce149161abf50c5e68dddba642bac8c54bff..28d4c8692dd1605d721b16f8c99be509d5fa71d3 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -8,7 +8,7 @@ import numpy as np
from functools import partial
-class RobotManagerAbstract(abc.ABC):
+class AbstractRobotManager(abc.ABC):
"""
RobotManager:
---------------
@@ -20,32 +20,52 @@ class RobotManagerAbstract(abc.ABC):
- provide functions every robot has to have like getQ
"""
- # just pass all of the arguments here and store them as is
- # so as to minimize the amount of lines.
- # might be changed later if that seems more appropriate
+ # weird ass pythonic way to declare an abstract atribute:
+ # you make the property function abstract,
+ # and then implement that.
+ # the property function consists of:
+ # - the class attribute
+ # - setter of the class attribute
+ # - getter of the class attribute
+ # - deleter of the class attribute
+ # since we only want the class attribute which won't be changed,
+ # we don't explicitely declare @property.setter and the rest.
+ # if you don't implement the following for attributes declared this way:
+ # @property
+ # def attribute(self):
+ # return self.attribute
+ # then you will get a TypeError, and that's what we want -
+ # errors if you didn't implement these attributes.
+
+ # NOTE: these arguments you need to implement yourself
+ @property
@abc.abstractmethod
+ def args(self) -> argparse.Namespace: ...
+
+ @property
+ @abc.abstractmethod
+ def model(self) -> pin.Model: ...
+
+ @property
+ @abc.abstractmethod
+ def data(self) -> pin.Data: ...
+ @property
+ @abc.abstractmethod
+ def visual_model(self) -> pin.pinocchio_pywrap_default.GeometryModel: ...
+ @property
+ @abc.abstractmethod
+ def collision_model(self) -> pin.pinocchio_pywrap_default.GeometryModel: ...
+
+ # NOTE: call this via super().__init__() after you've implemented the above abstract properties
def __init__(self, args):
- self.args: argparse.Namespace # const
####################################################################
# robot-related attributes #
####################################################################
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
-
self._dt = 1 / self.args.ctrl_freq # [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
- # )
- # NOTE: probably should be an array, but since UR robots only accept a float,
- # we go for a float
+ # NOTE: _MAX_ACCELERATION probably should be an array, but since UR robots only accept a float,
+ # we go for a float and pretend the every vector element is this 1 number
# NOTE: this makes sense only for velocity-controlled robots.
# when torque control robot support will be introduce, we'll do something else here
self._MAX_ACCELERATION: float | np.ndarray # in joint space
@@ -55,25 +75,25 @@ class RobotManagerAbstract(abc.ABC):
)
self._acceleration = self.args.acceleration
# _MAX_V is the robot's hardware joint velocity limit
- self._MAX_V: np.ndarray
- self._MAX_V = self.model.velocityLimit
+ self._MAX_V: np.ndarray = self.model.velocityLimit
# but you usually want to run at lower speeds for safety reasons
assert self.args.max_v_percentage <= 1.0 and self.args.max_v_pecentage > 0.0
# so velocity commands are internally clipped to self._max_v
self._max_v = np.clip(self._MAX_V, 0.0, args.max_v_percentage * self._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 = np.zeros(self.model.nv)
- self.a = np.zeros(self.model.nv)
+ self._q = np.zeros(self.model.nq)
+ self._v = np.zeros(self.model.nv)
+ self._a = np.zeros(self.model.nv)
+ # TODO: each robot should know which grippers are available
+ # and set this there.
self.gripper = None
self._step()
####################################################################
# processes and utilities robotmanager owns #
####################################################################
-
# TODO: make a default
if args.save_log:
self._log_manager = LogManager(args)
@@ -90,7 +110,7 @@ class RobotManagerAbstract(abc.ABC):
self.visual_model,
)
self.visualizer_manager = ProcessManager(
- args, side_function, {"q": self.q.copy()}, 0
+ args, side_function, {"q": self.q}, 0
)
# TODO: move this bs to visualizer, there are 0 reasons to keep it here
if args.visualize_collision_approximation:
@@ -101,7 +121,7 @@ class RobotManagerAbstract(abc.ABC):
)
@abc.abstractmethod
- def setInitialPose(self):
+ def setInitialPose(self) -> None:
"""
setInitialPose
--------------
@@ -114,11 +134,13 @@ class RobotManagerAbstract(abc.ABC):
"""
pass
- def getQ(self):
- return self.q.copy()
+ @property
+ def q(self):
+ return self._q.copy()
- def getV(self):
- return self.v.copy()
+ @property
+ def v(self):
+ return self._v.copy()
@abc.abstractmethod
def forwardKinematics(self):
@@ -130,6 +152,14 @@ class RobotManagerAbstract(abc.ABC):
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.
+
+ This should be implemented by an interface.
+ For example, for the SingleArmInterface you would do:
+
+ def forwardKinematics(self):
+ pin.forwardKinematics(self.model, self.data, self._q)
+ pin.updateFramePlacement(self.model, self.data, self._ee_frame_id)
+ self.T_w_e = self.data.oMf[self._ee_frame_id]
"""
pass
@@ -147,25 +177,24 @@ class RobotManagerAbstract(abc.ABC):
Usage:
- don't call this method yourself! this is CommandLoopManager's job
- use getters to get copies of the variables you want
+
+ Look at some interfaces to see particular implementations.
"""
- def sendVelocityCommand(self, v):
+ def sendVelocityCommand(self, v) -> None:
"""
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)
+ -------------------
+ 1) saturate the command to comply with hardware limits or smaller limits you've set
+ 2) send it via the particular robot's particular communication interface
"""
assert type(v) == np.ndarray
assert len(v) == self.model.nv
v = np.clip(v, -1 * self._max_v, self._max_v)
- self.actualSendCommand(v)
+ self.sendVelocityCommandToReal(v)
@abc.abstractmethod
- def actualSendCommand(self, v):
- raise NotImplementedError()
+ def sendVelocityCommandToReal(self, v): ...
# TODO: make faux gripper class to avoid this bullshit here
def openGripper(self):
@@ -222,7 +251,7 @@ class RobotManagerAbstract(abc.ABC):
print("you didn't select viz")
-class RobotManagerRealAbstract(RobotManagerAbstract, abc.ABC):
+class AbstractRobotManagerReal(AbstractRobotManager, abc.ABC):
"""
RobotManagerRealAbstract
------------------------
@@ -276,15 +305,15 @@ class RobotManagerRealAbstract(RobotManagerAbstract, abc.ABC):
"""
pass
- @abc.abstractmethod
- 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()
+ # @abc.abstractmethod
+ # 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()
@abc.abstractmethod
def stopRobot(self):