Update to modification in the way the control graph is now implemented.

  Integration of the velocity is now extracted from the device and performed
  by an integrator class. The integration time is provided by the signal time
  (in microseconds).

Author: florent-lamiraux

src/controller.cc

@@ -64,7 +64,7 @@ void workThread(SoTRobotArmController *controller)
 }
 
 SoTRobotArmController::SoTRobotArmController():
-  device_(new SoTRobotArmDevice("RobotArm"))
+  device_(new SoTRobotArmDevice("RobotArm")), pythonInitialized_(false)
 {
   // Create thread and python interpreter
   init();
@@ -85,14 +85,14 @@ SoTRobotArmController::SoTRobotArmController():
 }
 
 SoTRobotArmController::SoTRobotArmController(std::string RobotName):
-  device_(new SoTRobotArmDevice (RobotName))
+  device_(new SoTRobotArmDevice (RobotName)), pythonInitialized_(false)
 {
   // Create thread and python interpreter
   init();
 }
 
 SoTRobotArmController::SoTRobotArmController(const char robotName[]):
-  device_(new SoTRobotArmDevice (robotName))
+  device_(new SoTRobotArmDevice (robotName)), pythonInitialized_(false)
 {
   // Create thread and python interpreter
   init();
@@ -110,10 +110,38 @@ SoTRobotArmController::~SoTRobotArmController()
 {
 }
 
+void SoTRobotArmController::setControlSize(const int& size)
+{
+  device_->setControlSize(size);
+}
+
+// Initialize the size of the signal plugged into device.control.
+// The size is not known when creating python interpreter. For this reason,
+// we need to run the following python lines later on.
+//
+// After calling this method,
+//   - the input velocity of the integrator is set to 0,
+//   - the output configuration of the integrator is computed for time 0,
+//     it contains the initial configuration of the robot as read from the joint
+//     encoders.
+void SoTRobotArmController::initializePython()
+{
+  std::ofstream aof(LOG_PYTHON.c_str(), std::ios::app);
+  runPython(aof,"robot.initializeEntities()", *interpreter_);
+  aof.close();
+}
+
+
 void SoTRobotArmController::setupSetSensors
 (map<string,SensorValues> &SensorsIn)
 {
   device_->setupSetSensors(SensorsIn);
+  // Finish initialization of python after reading the sensors to know the
+  // configuration of the robot.
+  if (!pythonInitialized_) {
+    initializePython();
+    pythonInitialized_ = true;
+  }
 }
 
 

src/controller.hh

@@ -62,6 +62,12 @@ class SoTRobotArmController: public AbstractSotExternalInterface
   void setNoIntegration(void);
   void setSecondOrderIntegration(void);
 
+  // Set the number of joints that are controlled
+  virtual void setControlSize(const int& size);
+
+  // Run some python commands that cannot be run at construction
+  virtual void initializePython();
+
   /// Embedded python interpreter accessible via Corba/ros
   shared_ptr<dynamicgraph::Interpreter> interpreter_;
 
@@ -79,6 +85,8 @@ class SoTRobotArmController: public AbstractSotExternalInterface
   void init();
 
   SoTRobotArmDevice* device_;
+  // Whether python space has been fully initialized
+  bool pythonInitialized_;
 }; // class SoTRobotArmController
 
 #endif // SOT_UNIVERSAL_ROBOT_CONTROLLER_HH

src/device-python.cc

@@ -10,5 +10,6 @@ BOOST_PYTHON_MODULE(wrap)
   dynamicgraph::python::exposeEntity<SoTRobotArmDevice,
     bp::bases<dg::sot::Device> >();
   dynamicgraph::python::exposeEntity<DeviceToDynamic,
-    bp::bases<dg::Entity> >();
+    bp::bases<dg::Entity> >()
+    .def("setInputSize", &DeviceToDynamic::setInputSize);
 }

src/device.cc

@@ -84,17 +84,6 @@ SoTRobotArmDevice::SoTRobotArmDevice(std::string RobotName):
   dynamicgraph::Vector data (3); data.setZero ();
   using namespace dynamicgraph::command;
   std::string docstring;
-  /* Command increment. */
-  docstring =
-      "\n"
-      "    Integrate dynamics for time step provided as input\n"
-      "\n"
-      "      take one floating point number as input\n"
-      "\n";
-  addCommand("increment",
-             makeCommandVoid1((Device&)*this,
-                              &Device::increment, docstring));
-
   docstring =
       "    Set the integration in closed loop\n"
       "\n"
@@ -208,25 +197,6 @@ void SoTRobotArmDevice::cleanupSetSensors(map<string, SensorValues>&
   setSensors (SensorsIn);
 }
 
-void SoTRobotArmDevice::getControl(map<string,ControlValues> &controlOut, const double& period)
-{
-  sotDEBUGIN(25) ;
-  std::vector<double> anglesOut;
-
-  // Integrate control
-  increment(period);
-  sotDEBUG (25) << "state = " << state_.transpose() << std::endl;
-  previousState_ = state_;
-
-  // Specify the joint values for the controller.
-  anglesOut.resize(state_.size());
-
-  for(unsigned int i=0; i < state_.size();++i)
-    anglesOut[i] = state_(i);
-  controlOut["control"].setValues(anglesOut);
-  sotDEBUGOUT(25) ;
-}
-
 void SoTRobotArmDevice::integrate(const double &dt)
 {
   using dynamicgraph::Vector;

src/device.hh

@@ -58,8 +58,6 @@ public:
 
   void cleanupSetSensors(std::map<std::string, SensorValues> &sensorsIn);
 
-  void getControl(std::map<std::string, ControlValues> &anglesOut, const double& period);
-
   /// \todo this should go into the parent class, in sot-core package
   void setTimeStep (double dt)
   {
@@ -112,6 +110,10 @@ public:
   {
     signalRegistration(sinSIN << soutSOUT);
   }
+  void setInputSize(const int& size)
+  {
+    inputSize_ = size;
+  }
 private:
   dynamicgraph::Vector& compute(dynamicgraph::Vector& res, int time)
   {
@@ -121,6 +123,7 @@ private:
   }
   dynamicgraph::SignalPtr<dynamicgraph::Vector, int> sinSIN;
   dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> soutSOUT;
+  int inputSize_;
 };
 
 #endif // SOT_UNIVERSAL_ROBOT_DEVICE_HH

src/dynamic_graph/sot/robot_arm/__init__.py

@@ -26,5 +26,3 @@
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 from .robot_arm import RobotArm
-from .universal_robot import UniversalRobot
-from .franka import Franka

src/dynamic_graph/sot/robot_arm/franka.py

@@ -24,36 +24,10 @@
 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
-from .robot_arm import RobotArm
-from .sot_robot_arm_device import DeviceToDynamic
+from dynamic_graph.sot.dynamic_pinocchio.robot import Robot
 
 class Franka(RobotArm):
     """
     This class defines a robot of type Franka Panda with gripper
     """
     defaultFilename = "package://sot_universal_robot/urdf/franka.urdf"
-
-    def __init__(self, name, device=None, tracer=None, loadFromFile=False):
-        RobotArm.__init__(self, name, device, tracer, loadFromFile)
-
-    # Resize the signal from dimension 7 to 8
-    #
-    # self.dynamic is initialized using the URDF model containing
-    # 9 joints: 7 for the arm, 2 for the fingers. The second finger
-    # joint mimics the first finger joint. The dimension is therefore 8.
-    #
-    #  self.device is initialized via ros params and contains only 7 joints.
-    def setClosedLoop(self, closedLoop):
-        if closedLoop:
-            raise RuntimeError('Closed loop not possible for Franka robot.')
-        else:
-            d = DeviceToDynamic('d2d')
-            plug(self.device.state, d.signal('sin'))
-            plug(d.signal('sout'), self.dynamic.position)
-            self.device.setClosedLoop(False)
-
-    ## Only the 7 arm joints are actuated
-    #
-    #  The fingers are not controlled via roscontrol.
-    def getActuatedJoints(self):
-        return range(0,7)

src/dynamic_graph/sot/robot_arm/prologue.py

@@ -27,8 +27,7 @@
 import sys
 
 from .sot_robot_arm_device import DeviceRobotArm
-from . import Franka
-from . import UniversalRobot
+from .robot_arm import RobotArm
 
 if not hasattr(sys, 'argv'):
     sys.argv = [
@@ -37,16 +36,13 @@
 
 print("Prologue RobotArm")
 
-RobotFactory = {'ur': UniversalRobot, 'franka': Franka}
-
 # Create the device.
 # This entity behaves exactly like robotsimu except:
 # 1. it does not provide the increment command
 # 2. it forwards the robot control to the sot-abstract
 #    controller.
 def makeRobot(robotType):
-    Robot = RobotFactory[robotType]
     # Create the robot using the device.
-    robot = Robot(name=robotType, device=DeviceRobotArm('RobotArm'))
+    robot = RobotArm(name=robotType, device=DeviceRobotArm('RobotArm'))
     return robot
 

src/dynamic_graph/sot/robot_arm/robot_arm.py

@@ -25,55 +25,30 @@
 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+import numpy
 import pinocchio as se3
 from dynamic_graph import plug
-from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector
 from dynamic_graph.sot.dynamic_pinocchio.dynamic import DynamicPinocchio
-from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractRobot
+from dynamic_graph.sot.dynamic_pinocchio.robot import AbstractRobot
 import pinocchio
 
 class RobotArm(AbstractRobot):
     """
     This class defines an industrial robot arm
     """
-
-    rosParamName = "/robot_description"
-    def __init__(self, name, device=None, tracer=None, loadFromFile=False):
-        self.OperationalPointsMap = None
-
-        if loadFromFile:
-            print("Loading from file " + self.defaultFilename)
-            self.loadModelFromUrdf(self.defaultFilename, rootJointType=None)
-        else:
-            print("Using ROS parameter \"/robot_description\"")
-            import rospy
-            if self.rosParamName not in rospy.get_param_names():
-                raise RuntimeError('"' + self.rosParamName +
-                                   '" is not a ROS parameter.')
-            s = rospy.get_param(self.rosParamName)
-            self.loadModelFromString(s, rootJointType=None)
-        AbstractRobot.__init__(self, name, tracer)
-
-        # Create rigid body dynamics model and data (pinocchio)
-        self.dynamic = DynamicPinocchio(self.name + "_dynamic")
-        self.dynamic.setModel(self.pinocchioModel)
-        self.dynamic.setData(self.pinocchioData)
-        self.dynamic.displayModel()
-        self.dimension = self.dynamic.getDimension()
-
-        self.device = device
-        self.initializeRobot()
-
-        # Create operational points based on operational points map
-        # (if provided)
-        if self.OperationalPointsMap is not None:
-            self.initializeOpPoints()
-
-    def defineHalfSitting(self, q):
-        pass
-
-    def _initialize(self):
-        AbstractRobot._initialize(self)
-        self.OperationalPoints.extend(['wrist', 'gaze'])
-
-__all__ = [RobotArm]
+    # Index of the first actuated joint in the configuration vector
+    firstJointIndex = 0
+    def getActuatedJoints(self):
+        return range(0, self.device.getControlSize())
+
+    def initializeEntities(self):
+        # initialize size of some signals. The size is not known at construction
+        controlSize = self.device.getControlSize()
+        self.selector.selec(self.firstJointIndex, self.firstJointIndex +
+                             controlSize)
+        q = numpy.zeros(self.dynamic.getDimension())
+        q[0:controlSize] = self.device.signal("robotState").value
+        self.integrator.setInitialConfig(q)
+        self.integrator.signal("velocity").value = \
+            numpy.zeros(self.pinocchioModel.nv)
+        self.integrator.signal('configuration').recompute(0)

src/dynamic_graph/sot/robot_arm/universal_robot.py

@@ -24,21 +24,10 @@
 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
-from .robot_arm import RobotArm
+from dynamic_graph.sot.dynamic_pinocchio.robot import Robot
 
-class UniversalRobot(RobotArm):
+class UniversalRobot(Robot):
     """
     This class defines a robot of type UniversalRobot: UR3, UR5, UR10
     """
     defaultFilename = "package://sot_universal_robot/urdf/ur10.urdf"
-
-    def __init__(self, name, device=None, tracer=None, loadFromFile=False):
-        RobotArm.__init__(self, name, device, tracer, loadFromFile)
-
-    def setClosedLoop(self, closedLoop):
-        if closedLoop:
-            plug(self.device.robotState, self.dynamic.position)
-            self.device.setClosedLoop(True)
-        else:
-            plug(self.device.state, self.dynamic.position)
-            self.device.setClosedLoop(False)