diff --git a/python/setup.py b/python/setup.py
index 8cce88f2136998ae264b8c47c8d7d52020572f7a..2e663a563a75f54e254b8ba91ddb5b1b82fb9e10 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -1,13 +1,13 @@
from setuptools import setup, find_packages
-setup(name='ur_simple_control',
- version='0.1',
- description='Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.',
+setup(name='smc',
+ version='0.2',
+ description='Simple Manipulator Control (SMC) - simplest possible framework for robot control.',
author='Marko Guberina',
url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
- packages=['ur_simple_control'],
+ packages=['smc'],
# package_dir={"": "ur_simple_control"},
package_data={
- 'ur_simple_control.robot_descriptions': ['*'],
+ 'smc.robots.robot_descriptions': ['*'],
},
zip_safe=False)
# NOTE: if you want to switch to the toml thing,
diff --git a/python/smc.egg-info/PKG-INFO b/python/smc.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..d5f66fa39bbac33c1594fbda257cdf9cba231203
--- /dev/null
+++ b/python/smc.egg-info/PKG-INFO
@@ -0,0 +1,7 @@
+Metadata-Version: 2.1
+Name: smc
+Version: 0.2
+Summary: Simple Manipulator Control (SMC) - simplest possible framework for robot control.
+Home-page: https://gitlab.control.lth.se/marko-g/ur_simple_control
+Author: Marko Guberina
+License-File: LICENSE.txt
diff --git a/python/smc.egg-info/SOURCES.txt b/python/smc.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5772872e2ac62db9911730a91afc02a76bf290ed
--- /dev/null
+++ b/python/smc.egg-info/SOURCES.txt
@@ -0,0 +1,9 @@
+LICENSE.txt
+README.md
+setup.py
+smc/__init__.py
+smc.egg-info/PKG-INFO
+smc.egg-info/SOURCES.txt
+smc.egg-info/dependency_links.txt
+smc.egg-info/not-zip-safe
+smc.egg-info/top_level.txt
\ No newline at end of file
diff --git a/python/smc.egg-info/dependency_links.txt b/python/smc.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/smc.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/python/smc.egg-info/not-zip-safe b/python/smc.egg-info/not-zip-safe
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/python/smc.egg-info/not-zip-safe
@@ -0,0 +1 @@
+
diff --git a/python/smc.egg-info/top_level.txt b/python/smc.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..45496854d8414265f8d3e99019de0424bd206c89
--- /dev/null
+++ b/python/smc.egg-info/top_level.txt
@@ -0,0 +1 @@
+smc
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index 023cb82542af81f3c06630b1c521ae0f1a28c5d2..8d3a04e0e7ceb8eac31b12db17bf27424dee8897 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -1,17 +1,26 @@
+from smc.robots.robotmanager_abstract import RobotManagerAbstract
+
+import abc
import numpy as np
import pinocchio as pin
-from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
class DualArmInterface(RobotManagerAbstract):
+ """
+ DualArmInterface
+ ----------------
+ Provides the template for what the robot needs to have to use dual arm features.
+ - l stands for left
+ - r stands for right
+ """
+
+ @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")
- # l stands for left
- # r stands for right
self._l_ee_frame_id: int
self._r_ee_frame_id: int
@@ -65,7 +74,7 @@ class DualArmInterface(RobotManagerAbstract):
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!!!
+ 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()
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 e1ce9feec98bcc644b94397ceb64293f7488aef8..89f1454c08f61b605fe9d178264966e1e9d7f7a0 100644
--- a/python/smc/robots/interfaces/force_torque_sensor_interface.py
+++ b/python/smc/robots/interfaces/force_torque_sensor_interface.py
@@ -7,12 +7,15 @@ 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)
+ @property
+ def _ft_sensor_frame_id(self):
+ # whatever you set to be the frame
+ ...
+
def getWrench(self):
"""
getWrench
diff --git a/python/smc/robots/robotmanager_abstract.py b/python/smc/robots/robotmanager_abstract.py
index 9d21eb372cc33498cb1a10e7e568ebec1a33dd39..32aee3ba47d33fe0c701333e502c2c4610c2d334 100644
--- a/python/smc/robots/robotmanager_abstract.py
+++ b/python/smc/robots/robotmanager_abstract.py
@@ -273,6 +273,7 @@ class RobotManagerRealAbstract(RobotManagerAbstract, abc.ABC):
"""
pass
+ @abc.abstractmethod
def _step(self):
self._getQ()
self._getV()
@@ -280,15 +281,7 @@ class RobotManagerRealAbstract(RobotManagerAbstract, abc.ABC):
# 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):
@@ -319,6 +312,7 @@ class RobotManagerRealAbstract(RobotManagerAbstract, abc.ABC):
"""
pass
+ @abc.abstractmethod
def unSetFreedrive(self):
"""
setFreedrive