click works in pinocchio

python/README.md

 # TODO:
 ---------
 - go to a previous version, save basic version of click where everything is in one file
+- write test for every algorithm - just run it on sample data on simulation and pinocchio.
+  using things on the real robot needs to be tested manually, but simulation in URsimulator should
+  take care of that because the only difference is the IP address in that case.
+  point is test what you can.
 
 # installation
 ------------

python/setup.py

 from setuptools import setup, find_packages
 setup(name='ur_simple_control',
-        version='0.0.1',
-        # TODO add other ones and test, check .toml file for more
-        install_requires=['numpy'],
-        packages=find_packages(where='src'),
-
-        package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
-        )
+        version='0.1',
+        description='Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.',
+        author='Marko Guberina',
+        url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
+        packages=['ur_simple_control'],
+        #        package_dir={"": "ur_simple_control"},
+        package_data={
+            'ur_simple_control.robot_descriptions': ['*'],
+            },
+        zip_safe=False)
+# NOTE: if you want to switch to the toml thing,
+# here you go, knock yourself out, didn't really work for me,
+# and i don't care at this stage
+        # add other ones and test, check .toml file for more
+        # dependencies need to be taken care of separately
+        # because you don't want to install pinocchio via pip
+        #install_requires=['numpy'],
+        # broken, but if you'll want to switch later here you go
+        #packages=find_packages(where='src'),
+        #package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
+        #)
 

python/ur_simple_control.egg-info/PKG-INFO

+Metadata-Version: 2.1
+Name: ur-simple-control
+Version: 0.1
+Summary: Collection of control algorithms for the UR5e arm based on the ur_rtde interface for communication and pinocchio for calculations.
+Home-page: https://gitlab.control.lth.se/marko-g/ur_simple_control
+Author: Marko Guberina
+License-File: LICENSE.txt

python/ur_simple_control.egg-info/SOURCES.txt

+LICENSE.txt
+README.md
+setup.py
+ur_simple_control/__init__.py
+ur_simple_control.egg-info/PKG-INFO
+ur_simple_control.egg-info/SOURCES.txt
+ur_simple_control.egg-info/dependency_links.txt
+ur_simple_control.egg-info/not-zip-safe
+ur_simple_control.egg-info/top_level.txt
\ No newline at end of file

python/ur_simple_control.egg-info/dependency_links.txt

+

python/ur_simple_control.egg-info/not-zip-safe

+

python/ur_simple_control.egg-info/top_level.txt

+ur_simple_control

python/ur_simple_control/__init__.py

+from ur_simple_control import clik, dmp, robot_descriptions, util
+__all__ = [
+"clik", "dmp", "robot_descriptions", "util",
+]

python/ur_simple_control/clik/clik.py

@@ -75,7 +75,7 @@ TODO: write out other algorithms
 """
 def getController(args):
     if args.controller == "dampedPseudoinverse":
-        return partial(dampedPseudoinverse, args.tiknonov_damp)
+        return partial(dampedPseudoinverse, args.tikhonov_damp)
     if args.controller == "jacobianTranspose":
         return jacobianTranspose
     # TODO implement and add in the rest
@@ -107,11 +107,11 @@ def controlLoopClik(robot, controller, i):
     # first check whether we're at the goal
     SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
     err_vector = pin.log6(SEerror).vector 
-    if np.linalg.norm(err_vector) < eps:
+    if np.linalg.norm(err_vector) < robot.goal_error:
       print("Convergence achieved, reached destionation!")
       breakFlag = True
     # pin.computeJointJacobian is much different than the C++ version lel
-    J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+    J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     # compute the joint velocities.
     # just plug and play different ones
     qd = controller(J, err_vector)
@@ -119,7 +119,7 @@ def controlLoopClik(robot, controller, i):
     # we do the printing here because controlLoopManager should be general.
     # and these prints are click specific (although i'm not 100% happy with the arrangement)
     if not i % 1000:
-        print("pos", data.oMi[JOINT_ID])
+        print("pos", robot.data.oMi[robot.JOINT_ID])
         print("linear error = ", pin.log6(SEerror).linear)
         print("angular error = ", pin.log6(SEerror).angular)
         print(" error = ", err_vector.transpose())
@@ -133,5 +133,5 @@ if __name__ == "__main__":
     Mgoal = robot.defineGoalPoint()
     controller = getController(args)
     controlLoop = partial(controlLoopClik, robot, controller)
-    controlLoopManager(controlLoop, args)
-    controlLoopManager.run()
+    loop_manager = ControlLoopManager(controlLoop, args)
+    loop_manager.run()