From 54cd977687e68f469f7836936e8d01a7e99808ef Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 26 Oct 2023 14:34:41 +0200
Subject: [PATCH] stuff mostly works, now it is time to document all of it

---
 README.md                                  |  2 +-
 clik/src/clik.cpp                          |  2 ++
 docs/collaborative_manipulators_general.md |  4 +++
 docs/pinocchio.md                          |  6 ++++
 gepetto_ex.py                              | 37 ++++++++--------------
 5 files changed, 27 insertions(+), 24 deletions(-)
 create mode 100644 docs/collaborative_manipulators_general.md
 create mode 100644 docs/pinocchio.md

diff --git a/README.md b/README.md
index de796db..819cacd 100644
--- a/README.md
+++ b/README.md
@@ -4,7 +4,7 @@
 # some specifics
 - using the ur_rtde implementation of the client-side real-time interface for ur robots
 - using pinocchio for dynamics intergration on the client side
-- basic closed-loop inverse kinematics control will be implemented
+- basic closed-loop inverse kinematics control is implemented, more algorithms should be added
 - some basic tests will be here as well, other projects built on this will be in separate repos
 
 # TODO
diff --git a/clik/src/clik.cpp b/clik/src/clik.cpp
index a8db936..7d1db87 100644
--- a/clik/src/clik.cpp
+++ b/clik/src/clik.cpp
@@ -129,11 +129,13 @@ int main(void) {
 //      v_as_std_vec.push_back(v[3]);
 //      v_as_std_vec.push_back(v[4]);
 //      v_as_std_vec.push_back(v[5]);
+	  // TODO: play around with these values
       rtde_control->speedJ(v_as_std_vec, 0.1, 1.0 / 500);
 //      v_as_std_vec.clear();
 	} else {
       q = pinocchio::integrate(model, q, v * DT);
 	}
+	// TODO wrap this in an appropriate if
 	viewer.display(q);
     if (!(i % 10))
       std::cout << i << ": error = " << err.transpose() << std::endl;
diff --git a/docs/collaborative_manipulators_general.md b/docs/collaborative_manipulators_general.md
new file mode 100644
index 0000000..4e02c00
--- /dev/null
+++ b/docs/collaborative_manipulators_general.md
@@ -0,0 +1,4 @@
+## what is a robotic manipulator?
+TODO explain basic theory here
+also add a video explanation
+
diff --git a/docs/pinocchio.md b/docs/pinocchio.md
new file mode 100644
index 0000000..fcc3e99
--- /dev/null
+++ b/docs/pinocchio.md
@@ -0,0 +1,6 @@
+# nice exercises
+https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_d-practical-exercises_intro.html
+
+# how to learn this?
+play around with it in ipython
+--> TODO explain how you do this
diff --git a/gepetto_ex.py b/gepetto_ex.py
index 011b24a..3e59787 100644
--- a/gepetto_ex.py
+++ b/gepetto_ex.py
@@ -1,13 +1,14 @@
-# NOTE: this example needs gepetto-gui to be installed
-# usage: launch gepetto-gui and then run this test
- 
 import pinocchio as pin
 import numpy as np
 import sys
 import os
 from os.path import dirname, join, abspath
+import time
  
 from pinocchio.visualize import GepettoVisualizer
+import gepetto.corbaserver
+gepetto.corbaserver.start_server()
+time.sleep(3)
  
 # Load the URDF model.
 # Conversion with str seems to be necessary when executing this file with ipython
@@ -27,30 +28,20 @@ model = pin.buildModelFromUrdf(urdf_model_path)
 visual_model = pin.buildGeomFromUrdf(model, urdf_model_path, pin.GeometryType.VISUAL)
 collision_model = pin.buildGeomFromUrdf(model, urdf_model_path, pin.GeometryType.COLLISION)
 viz = GepettoVisualizer(model, collision_model, visual_model)
- 
-# Initialize the viewer.
-try:
-    viz.initViewer()
-except ImportError as err:
-    print("Error while initializing the viewer. It seems you should install gepetto-viewer")
-    print(err)
-    sys.exit(0)
- 
-try:
-    viz.loadViewerModel("pinocchio")
-except AttributeError as err:
-    print("Error while loading the viewer model. It seems you should start gepetto-viewer")
-    print(err)
-    sys.exit(0)
- 
+viz.initViewer()
+viz.loadViewerModel()
+
+NQ, NV = model.nq, model.nv
+
 # Display a robot configuration.
 q0 = pin.neutral(model)
 viz.display(q0)
 data = pin.Data(model)
-for i in range(10000):
-    q0[0] += 0.001
-    pin.forwardKinematics(model, data, q0)
-    viz.display(q0)
+
+#for i in range(10000):
+#    q0[0] += 0.001
+#    pin.forwardKinematics(model, data, q0)
+#    viz.display(q0)
  
 # Display another robot.
 #viz2 = GepettoVisualizer(model, collision_model, visual_model)
-- 
GitLab