nullspace control

notebook/controller.py

@@ -91,6 +91,7 @@ def __init__(self, pose=TransformStamped(header=Header(frame_id='panda_link8'),
             [(j.min+j.max)/2 + 0.1*(j.max-j.min)*random.uniform(0, 1) for j in self.robot.active_joints])
         self.target_link = pose.child_frame_id
         self.T, self.J = self.robot.fk(self.target_link, dict(zip(self.joint_msg.name, self.joint_msg.position)))
+        self.preferred_joints = self.joint_msg.position.copy()
 
         self.im_server = MyInteractiveMarkerServer("controller", self.T)
 
@@ -114,10 +115,14 @@ def invert_smooth_clip(s):
 
         U, S, Vt = numpy.linalg.svd(J)
         rank = min(U.shape[0], Vt.shape[1])
+        accepted_singular_values = (S > 1e-3).sum()
+        VN = Vt[accepted_singular_values:].T
         for i in range(rank):
             S[i] = invert_smooth_clip(S[i])
 
-        return numpy.dot(Vt.T[:, 0:rank], S * U.T.dot(numpy.array(error)))
+        qdot1 = numpy.dot(Vt.T[:, 0:rank], S * U.T.dot(numpy.array(error)))
+        qdot2 = VN.dot(0.1 * VN.T.dot(self.preferred_joints - self.joint_msg.position))
+        return qdot1 + qdot2
 
     @staticmethod
     def position_error(T_tgt, T_cur):

notebook/ns_demo.ipynb

@@ -0,0 +1,138 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Launch the ROS demo"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "%%bash --bg\n",
+    "trap 'kill $(jobs -p)' EXIT\n",
+    "xterm -e /bin/bash -l -c \"roslaunch demo.launch\""
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Create the controller and run it periodically in a thread"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import rospy\n",
+    "from threading import Thread\n",
+    "from controller import Controller\n",
+    "\n",
+    "rospy.init_node('ns_demo')\n",
+    "c = Controller()\n",
+    "\n",
+    "def worker():\n",
+    "    rate = rospy.Rate(50)\n",
+    "    while not rospy.is_shutdown():\n",
+    "        c.pose_control(c.im_server.target)\n",
+    "        rate.sleep()\n",
+    "    \n",
+    "t = Thread(target=worker)\n",
+    "t.start()"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Create a list of slider widgets, one for each joint, to chose the default pose"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import ipywidgets\n",
+    "from ipywidgets import FloatSlider, Layout, Button, Box\n",
+    "joint_widgets = [FloatSlider(min = j.min, max = j.max, step = (j.max-j.min) / 100, description = j.name) \\\n",
+    "                 for j in c.robot.active_joints]"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "React to slider (value) changes by adapting the default joint pose"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def on_sent(event):\n",
+    "    widget = event.owner\n",
+    "    c.preferred_joints[c.joint_msg.name.index(widget.description)] = widget.value\n",
+    "\n",
+    "for widget in joint_widgets:\n",
+    "    widget.observe(on_sent, 'value')"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Collect all widgets (sliders and buttons) in a form and display them"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {
+    "scrolled": false
+   },
+   "outputs": [],
+   "source": [
+    "form = Box(joint_widgets, layout=Layout(\n",
+    "    display='flex',\n",
+    "    flex_flow='column',\n",
+    "    border='solid 2px',\n",
+    "    align_items='stretch',\n",
+    "    width='100%'\n",
+    "))\n",
+    "display(form)"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 2",
+   "language": "python",
+   "name": "python2"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 2
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython2",
+   "version": "2.7.17"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}