At least for 64bit: FindMatlab.cmake could not find mex, mx and eng libraries (even if they are present).

I was reading https://github.com/robotology/codyco/blob/master/src/simulink/README.md for the first time and it was a bit unclear what the CODYCO_ROOT and CODYCO_DIR enviroment variables are. I guess they should be point to the codyco repository root directory, but this details are currently missing from documentation. @jeljaik

Testing the wbi (with the model loaded from the URDF file) the velocity terms (C*dq) seems incorrect.
It can be either a problem of the URDF parser or of the wbi.
@traversaro already found some issues in the parser. Maybe they are related.

Tested with yarpwbiTest, model taken from here, configuration file taken here

Output is

C term is 1.761797 but should be -1.897020
C Term is 1.825830 but should be -1.965969

Just opened this issue to track every problem that we find in the design of iWholeBodyActuactors.

Filtering noise in sensor outputs and computing numerical derivatives are fundamental steps in the implementation of most controllers. Currently I'm using 1st order IIR low pass filters for noise filtering (see FirstOrderLowPassFilter) and adaptive windows polynomial fitting for computing numerical derivatives (see adaptWinPolyEstimator). Both are implemented in the C++ library ctrlLib, which is part of the iCub repository.

Current Issues

The reasons why I am not happy with the current solution are:

• computation time is too large (mainly due to dynamic memory allocation)
• better noise attenuation could be achieved with higher order and more advanced filters (e.g. Chebyshev, Butterworth, elliptic)
• polynomial fitting provides only moderate high frequency attenuation (see this paper for details) and I suspect that the adaptive window size does not improve this aspect

Proposed solution

I would like to replace these classes with more advanced filter implementations, with particular focus on computation time (since these filters should be used inside real-time control loops). Rather than implementing a digital signal processing library from scratch I'd like to use an existing library. Although clearly thought for audio filtering, the C++ library DSPFilters seems particularly interesting, because of the following features:

• Butterworth, Chebyshev, Elliptic, Bessel and Legendre filters
• any type of frequency filtering (Low Pass, High Pass, Band Pass, Band Stop)
• smooth interpolation of filter settings (e.g. cut frequency) to achieve seamless parameter changes
• no calls to malloc or new, great for embedded systems
• no external dependencies, just the standard C++ library
• GUI with charts to show magnitude, phase response and pole/zero placement of any filter

My intention is to test this library and, if the results are satisfactory, start using it in the iCubWholeBodyState class.

Note on numerical derivatives

The library provides no direct method for numerical derivative computation, but once the input signal has been filtered to attenuate noise, numerical derivatives can be safely computed through exact polynomial fitting (e.g. fitting a 3rd order polynomial to a 4-point moving window).

Other references

Just for the records, I report here other interesting libraries I found in my research.

• mkfilter C++ library with web-based front-end, Butterworth, Bessel or Chebyshev IIR filters, with lowpass, highpass, bandpass or bandstop characteristcis, designed by bilinear transform or matched z-transform method; IIR bandpass, bandstop (notch) or allpass resonators, designed directly in the z-plane; FIR raised-cosine filters, optionally with a square-root response; FIR Hilbert transformers.
• Butterworth filter design: C++ classes for high order Butterworth IIR and EQ filters using the bilinear transform; generated filter coefficients are split out into cascaded biquad sections, for easy use in any biquad or second-order section (SOS) implementation.
• spuc: C++ library for DSP Simulation/Modeling

In current wholeBodyInterface documentation a clear indication of the units of measurement of the physical quantities is missing. From the implementation I guess that we use SI units for every interface (so radians for angles). As this is very common source of errors and bugs, we should make a definite decision and write it down in documentation.

@andreadelprete

Probably this will require some changes in wbi .

wholeBodyDynamicsTree

This issue is an informal blueprint for the architecture of the new wholeBodyDynamics module, which will substitute the iDyn-based wholeBodyDynamics module used in the iCub. This module is in charge of estimating the external contact forces and the joint torques given the measurements of the internal 6-axis force/torque sensors. More generally we can see it as a whole-body estimator.

The new module (temporarily named wholeBodyDynamicsTree) will be based on the iDynTree library, and it will have several improvements over the old wholeBodyDynamics, among the others:

• loading of the kinematic and dynamic parameters from URDF,
• efficient dynamics computations,
• support for an arbitrary number of force/torque sensors,
• support for torque and external force estimation in arbitrary contact situation (fixed base, floating base).

Implementation

We plan to move all the code taking care of the estimation inside an implementation of the wholeBodyState interface (i.e. icubWholeBodyStates), so that wholeBodyDynamics will just have to:

• instantiate an object of this class
• periodically read the estimates using the getEstimates method
• send the estimates over the network through Yarp ports

This separation allows improving the estimation (e.g. implementing more complex data fusion techniques) without modifying the wholeBodyDynamicsTree module.

cc @andreadelprete @iron76 @lornat75

Add superbuild (using https://github.com/robotology/ycm) support for the codyco repository, using the walkman repository as reference.

When using iDynTree boost is not automatically included. Its inclusion should be automatically exported in cmake

@traversaro

Since iWholeBodyStates should define the interface for a generic estimator it should provide users with the possibility to set priors on the estimated quantities.

After discussing with @iron76 and @traversaro two ways to do this emerged. The user can specify either:

1. the system dynamics and the control inputs, OR
2. the priors on the estimated quantities

Option 1

The first option is "Kalman-style": the estimation process exploits the knowledge of the system dynamics to predict the future state given the current state and the current control inputs. Then this prediction is fused with the sensor measurements to estimate the state. The main problem I see with this pattern is how to allow the user to specify the system dynamics. Function pointer? Implementing an interface? Moreover I wonder if this interface may fit all possible use-cases.

Option 2

The second option requires the user to do more work because the estimator takes directly the predictions (i.e. which acts as priors) of the future state. The final result is the same, but in this case it is not the estimator to compute the system dynamics, but the user. At a first glance I prefer this solution because it seems less constrained and I have the impression that it could fit to any scenario. Another thing to notice is that the predictions of the future state are usually computed anyway by the controller (some of them are the controller inputs: desired forces, position, velocities), so it doesn't cost anything to the controller to give these values to the estimator, whereas it would be redundant if the estimator had to recompute them using the specified system dynamics.

Anyway the topic is still open for discussion, so please tell me what you think.

Problem

Currently the interface iWholeBodySensors works (more or less) like this. There are 4 methods for each sensor type:

• bool addSensor(LocalId)
• bool removeSensor(LocalId)
• LocalIdList getSensorList()
• bool readSensor(double* data, double* stamp, bool wait)

where "Sensor" can be either "Encoder", "IMU" or "FTsensor". This approach clearly doesn't scale when the number of sensor types increases (and I think it is bound to increase).

Proposed Solution

My idea is to switch to a new interface having only 4 methods, which take an additional input parameter representing the sensor type:

• bool addSensor(SensorType, LocalId)
• bool removeSensor(SensorType, LocalId)
• LocalIdList getSensorList(SensorType)
• bool readSensor(SensorType, double* data, double* stamp, bool wait)

SensorType could be either an int or an Enum. This way, whenever we need to add a sensor type, we don't need to add any methods to the interface, but at most to add an entry in the SensorType enum.

Joint-Sensor Connection

However, with this new design we lose the connection between joints and sensors (e.g. encoders, pwm, torque), which is useful when the user wants to "remove" a joint: by calling the method removeJoint the interface could automatically remove all the sensors associated to that joint. Considering that joints may have different sensors, a possible solution may be to add these methods:

• bool addJoint(LocalId, SensorType*)
• bool removeJoint(LocalId)
• LocalIdList getJointList(SensorType=ANY_SENSOR)

The first method adds a joint with the specified sensors associated to it. The second method removes all the sensors associated to the specified joint. The third method return the list of all the joints having the specified sensor type; if the sensor type is not specified, it returns the list of all joints (note that theoretically a joint may have no sensor). We don't need to add a method for reading the joint sensors, because we can use readSensor.

Apparently there are some non-trivial (at least for me) conflicts when trying to merge branch module/adaptiveControl in to master .
Could you look into it @francesco-romano ? Thanks.

The link concept is necessary in the wholeBodyInterface in a few user cases:

• We want to calculate a Floating Base Jacobian of a link.
• When getting the ESTIMATE_EXTERNAL_FORCE_TORQUE we want to get the estimate of the external force torque acting on a link.

In this two user cases there is an hidden assumption: that a link is associated to a unique frame.

Regardless of the orientation in which the jacobian and the external force torque is expressed (in wbi it is the world one) the origin of this frame is used as the reference point for the linear velocity of the jacobian and for the external torque.

This is ok as long as every link is associated to a unique frame.

Unfortunately the URDF format constraint the link frames to have their origin placed in the axis of the parent joint, which is quite inconvenient for control (for example if you want the foot frame origin to be placed on the sole of the foot, as actually recommended by REP-120 .

The usual workaround, used also in the iCub URDF, is to add a fake link connected to a real link with a fixed joint, and to place the frame of the fake link in the desired arbitrary location.

Clearly external forces can be applied only to a real link, but in the URDF there is no way to semantically discriminate between a real link and fake link. The workaround usually used to distinguish between real and fake links, for example in gazebo, is to consider any link with a fixed parent joint to be a fake link. Any fake link is then "merged" from the physics point of view with its closest "real" ancestor.

This workaround fails when two real links are actually connected by a fixed joint, if the two bodies are connected by a 6-axis Force Torque sensor.

Very long term proper solution would be to clearly distinguish between frame and link concepts in the file describing the robot structure.

A clean short term workaround to continue use URDF is to use an additional tag to clearly mark "fake/frame" links. In this case, the semantics of get(ESTIMATE_EXTERNAL_FORCE_TORQUE, fake_link) would be "get the force/torque applied on the real_link corresponding to the fake_link, but expressing the torque at the origin of the frame of the fake_link).

When closing wholeBodyDynamicsTree from rpc port (quit command) the following output is printed:

pure virtual method called
terminate called without an active exception
Aborted

I have opened this issue to clear the necessary features for Jacobian calculations, for fitting them in a clear API.

For example, discussing with @iron76 and @serena-ivaldi they expressed the need for a method that returned the jacobian between a link and a link such that:

where and are expressed in local coordinates, and is the vector of the real joint speeds.

While @andreadelprete expressed the need for a method that returned the jacobian of a link with respect to the world reference frame:

where is expressed in the world frame and is the vector of speeds considering also the base velocity.

P.s. : Methods currently implemented in iDynTree library are available here:

http://wiki.icub.org/codyco/dox/html/classiCub_1_1iDynTree_1_1DynTree.html

The documentation is not clear for some methods, in the next days I'll improve it.

We have a really good documentation on the coupled joints of the iCub thanks to @DanielePucci .
We should port this documentation to the iCub wiki.
I guess @randaz81 or @pattacini can suggest a good location for this information (perhaps integrated with the iCub manual http://wiki.icub.org/wiki/Manual ).

When running under valgrind (i.e. a lot slowed down), new wholeBodyDynamics fail some iDynTree assertion related to external forces estimation consistency. Using it in real time there are no issues, so I am not further investigating in this, but this could hide some structural bug.

Today @iron76 experienced issues in using multiple WBI Toolbox blocks that where reading and writing Yarp ports. He was using OS X 10.7 .
The nature of the problem is still unclear, but it appears that it was related to a conflict in the use of the same static variable among the different copies of the Yarp static library included in the different blocks.

The issue was solved by compiling Yarp as a shared library.

Until we fully understand the origin of this issue and its implications I strongly suggests that we list Yarp compiled as a shared library in the requirements of WBI-T. A even better choice would be to enforce the availability of Yarp as a shared library during the build, but I am not sure on how to implement this build behaviour.

Apparently is very difficult for an average user (for example someone that is not a programmer and is only used to simulink/matlab) to install all the software for running WBI-Toolbox with a simulator. This is a big problem for dissemination. We have to simplify it.

This must be tested for different configurations:

• two joints in the same part set in different controlMode
• set velocity reference on the whole part
• set position reference on the whole part

All input parameters of pointer type of wholeBodyModel should be declared as constant pointers to constant data.

Uninstall target is apparently using a not-existing icub-config-unistall.cmake file

Please, change the flag SIMULINK_LIBRARY into CODYCO_USES_SIMULINK. The current name is not clear and difficult to find.

Howto: carlosmn/libgit2@e11f23b
Probably worth adding various custom targets to ctest.

After a long chat, @traversaro and I decided to implement other three changes to the library WholeBodyInterface:

• switch from local id (i.e. IDs composed by two int, one for the "body part" and one for the "local ID") to global id (IDs composed only by one int, so that they are unique for the whole body of the robot)
• add a string to the ID (which at the moment is only numerical) to ease the interaction with urdf/collada formats
• allow any joint serialization (at the moment IDs can only be serialized so that body parts are in increasing order)

When setting a C(C++) compiler flag from CMake we should always append the flag, never overwrite it.

Currently this problem is in all CMakeLists.txt of simulink library and in the ISIR test.

BTW: I think we can remove the -fpermissive flag, at least from our own code.

This is just a todo item.
We should mark exported symbols in our libraries.
See this.

One of the weakest point in the current software architecture is the handling of the world to base/root transformation.
As a remainder for future implementation we should:

• have an only authoritative source of the world to base transformation, for example in the form of a yarp port publishing this transformation.
• add the getEstimate(ESTIMATE_BASE_POS , ..) implementation to wholeBodyState and use only this call in the control code to get the world to base position.

In this way if the assumption on the world location changes we have only to modify the configuration of the component that is publishing the world to base transformation, and all code should work.

it is not clear that the 'tau' from JointTorques (and Dummy JointTorques) and the ones from InverseDynamics are not the same, please specify the difference somewhere or change the blocks

A lot of aspects of the wholeBodyInterface need a redesign: this issue works as an entry point to all other issues related to wholeBodyInterface.

• Core issues:
  • New ID representation : #6
  • Document expected size everywhere : #26
  • Document units everywhere: #14
    • Always use international system
  • Difference between link and frame concepts : #39
  • Initialization should be done always in init() functions, not in constructors : #63
  • WBI implementations should be thread safe? #64
• iWholeBodyStates related:
  • Set priors in iWholeBodyStates : #10
  • Add/remove of estimates is broken : #18
    • Add/remove of estimates should be permitted just at configuration time, not at runtime
• iWholeBodyModel related:
  • General redesign of iWholeBodyModel : #27
• iWholeBodyActuactors related:
  • General iWholeBodyActuactors issues : #36
    • All interfaces are relative joints, while open loop is related to motors.
  • How iWholeBodyActuactors should deal with errors : #15
• iWholeBodySensors related:
  • How to specify available sensors in iWholeBodySensors : #17
  • Blocking sensor reading calls should have a optional timeout : #46

We have a first implementation of computeDJdq ( 956d852 ). We should also support computeDJdq with linkID == COM_LINK_ID (i.e. DJdq of the COM).

yread block fails reading port containing integers.

Manually patching WBI-T to hard code the loading of the appropriate URDF file is proving to be time-consuming and error-prone.
@jeljaik do you think we are able to add it as an option in WBI-T robot configuration?

Currently in the WBI (whole-body interface) orientations/rotations are represented as quaternions. However I noticed that this may be annoying, because the user may be using a different representation (e.g. rotation matrices, euler angles, angle-axis). To simplify user's life I'd like to introduce a simple class Rotation, which represents a rotation between two reference frames. I want to take inspiration from KDL::Rotation (see http://people.mech.kuleuven.be/~rsmits/kdl/api/html/classKDL_1_1Rotation.html). This class should provide methods for conversion between different rotation representations, making the use of the WBI easier.

Now that KDL (version 1.2.3) respects C++98 standard, we can compile again CoDyCo with libc++ support.
We have to modify CMake files to proper handle all cases (older version of KDL, user wanting to compile with libstdc++ support...)
Also simulink library cmake must be changed.

@traversaro

Method icubWholeBodyActuators::setControlMode called for all joints.
The controlMode (currentCtrlModes) map update can be skipped: see the following code:

switch(controlMode)
    {
        case CTRL_MODE_POS:
            FOR_ALL(itBp, itJ)
                if(currentCtrlModes[LocalId(itBp->first,*itJ)]!=controlMode)
                    ok = ok && icmd[itBp->first]->setPositionMode(itBp->first==TORSO ? 2-(*itJ) : *itJ);
            break;
.....
    }
    if(ok)
    {
        FOR_ALL(itBp, itJ)
            currentCtrlModes[LocalId(itBp->first, *itJ)] = controlMode;
        if(ref!=0)
            ok = ok && setControlReference(ref);
    }

If for the i-th joint the set*Mode returns false, then all subsequent updates are skipped, and the variable ok is false. But the first i joints are still in another control mode.
I think the map update should be done contextually to the set function call.

Apparently inertial values for the V2 head are not available in iDyn ( https://github.com/robotology/icub-main/blob/b41c73517c4ad2f7a1c346a4b391aca06585b6a1/src/libraries/iDyn/src/iDyn.cpp#L2626 ). The root_link is missing because the iDyn uses a fixed base model.

The current iDyn model uses a 1.33 Kg mass for the head and a 1.2 Kg mass for the root_link .

To get some reasonable a-priori estimate we weighted iCubGenova01 and iCubGenova03:

While the iDyn model was:

As a workaround we adopt a mass of 3 Kg for head and 4.72 kg for root_link.

We have to discuss how to implement the ZMP related calculation in iDynTree as several people ( @MirkoFerrati and the Walkman team, I guess also @jeljaik ) have an interest on this.

To have a consistent reference for terminology I propose to use the one used in Chapter 16 of the Springer Handbook of Robotics ( http://link.springer.com/referenceworkentry/10.1007%2F978-3-540-30301-5_17 ).

The ZMP is properly defined with respect to a contact plane, then the issue from the library interface point of view is how to specify this contact plane.

Possible options include:

• define it with respect to the World
• define it with respect to a given Link
• leave both the option to the user

Solving this issue, we can then have one method like getZMP(PlaneInformation), or a couple of methods like setZMPPlane(ZMPPlaneInformation) && getZMP() . The returned information could be a 2D vector with the ZMP position in the specified plane.

cc @francesco-romano @DanielePucci @andreadelprete @iron76 @serena-ivaldi

Technical P.S. :
From my point of view, the "Computed ZMP" is obtained as an intersection between the screw axis of the sum of inertial and gravitational wrenches of all the links of the robot ( that can be calculated from each link position/velocity/acceleration) and the contact plane (ZMP concept apply only if the contacts are occurring only on on one plane).

I think it can be useful to add a CoDyCo library project to the repository containing common stuff.
For example a Common.h file can contain useful macros, a Math.h file can contain useful function (e.g. generic Eigen pseudoinverse function)

by using the YARP time functionalities, we can sync automatically with simulation time instead of system time

For simplifying the codyco demo scenario (in relation to #39) I was looking into adding /icub/part/endEffectorWrench:o and /icub/part/cartesianEndEffectorWrench:o to stream directly the wrench acting at the end effector, expressed at root (base) and end effector reference frame.

Apparently this ports are used by some users out in the world, as reported on rc-hackers.

In general, we have to decide which ports to support in the new wholeBodyDynamics for backward compatibility (I guess we all agree that we do not want to support ALL the ports that are currently exposed by wholeBodyDynamics).

The file wbInterface.cpp currently uses a constructor for the object iCubTree_version_tag which is available only when CODYCO_USES_URDFDOM is enabled. Currently, a compile error is obtained when enabling SIMULINK_LIBRARY without enabling CODYCO_USES_URDFDOM.

I'm installing from scratch a new machine that has Ubuntu 12.04. In this version the maximum version of Eigen3 you can download from apt-get is 3.0.5.

So I installed eigen3 from the sources (https://bitbucket.org/eigen/eigen/) and I set the following environment variable in my bashrc:
export EIGEN3_INCLUDE_DIR=$ICUB_INSTALL_PREFIX/include/eigen3

But when I compile codyco, I get the following error:

CMake Error at
/home/ivaldi/software/src/yarp/conf/cmake-3.0.0/Modules/FindPackageHandleStand
ardArgs.cmake:136 (message):
Could NOT find Eigen3 (missing: EIGEN3_VERSION_OK) (Required is at least
version "3.2")
Call Stack (most recent call first):

/home/ivaldi/software/src/yarp/conf/cmake-3.0.0/Modules/FindPackageHandleStand
ardArgs.cmake:343 (_FPHSA_FAILURE_MESSAGE)
cmake/modules/FindEigen3.cmake:76 (find_package_handle_standard_args)
cmake/modules/CoDyCoFindDependencies.cmake:2 (find_package)
CMakeLists.txt:63 (include)

I tried to run cmake like this:

cmake .. -DCMAKE_MODULE_PATH=/home/ivaldi/software/include/eigen3 -DCMAKE_INSTALL_PREFIX=/home/ivaldi/software

But it is still finding eigen 3.0.5 in /usr/include/eigen3

A timeout option should be added to iWholeBodySensors::readSensor(s), otherwise a sensor never reporting data would leave the component using the wbi hanging.

There are some issues in the design of iWholeBodyModel.

First of all, the concept of "all" joints of the robot is necessary, to properly take into account blocked joints. The current approach of adding and removing joints or passing a vector to the iWholeBodyModel implementation constructor is neither viable nor intuitive (furthermore it does not match well with the stateless nature of the iWholeBodyModel interface, that is also something that could be re-discussed).

Secondly, it is not clear to the user the relation between the iWholeBodyState and the other interfaces (i.e. if the iWholeBodyModel implicitly uses the iWholeBodySensor or iWholeBodyStates to get some information). We should clearly state this in documentation.

cc @DanielePucci @francesco-romano

Inside icubWholeBodyInterface, especially icubWholeBodySensors, some behaviour (reading or not F/T sensors, torque sensors, etc, etc) changes using the hardcoded isRobotSimulator function.
This is an issue because the correct behaviour should change from a simulator to another (for example, PWM sensors are not available neither in iCubSim nor in iCub@Gazebo, while F/T are available in Gazebo, and in general also on the real robot the PWM measures could be available or not).

My idea was to start to fade out the use of this function, expecting instead that the user of the interface appropriately initializes the available sensors, using the addSensors methods. This would require to add a way to specify the available sensors in the icubWholeBodyInterface, instead of hardcoding them in the implementation. A possible idea is to use a specific structure as an argument to the icubWholeBodyInterface.

Observation and criticism on this topic will be greatly appreciated. @andreadelprete @francesco-romano

In current version of icubWholeBodyInterface, the removeEstimate method of icubWholeBodyState class simply remove the sensor on which the Estimate depends.

For example, the ESTIMATE_JOINT_ACC for a given joint depends on SENSOR_ENCODER, but if we remove the SENSOR_ENCODER sensor reading, we break also other estimates that depends on it, such as ESTIMATE_JOINT_VEL.

The situation complicates when we have an estimate that depends on multiple sensors. Perhaps we need some concept of "sensor decencies" for an estimate, for proper dealing with this issues. Furthermore, this is another example that requiring to support the adding/removal of estimates at runtime can really complicate the implementation, and we should probably move on leaving this freedom of adding/removing stuff only at configuration time.