Recent changes to VirtualRobot

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Fri, 04 Jul 2014 07:57:28 -0000

--- v51
+++ v52
@@ -77,7 +77,11 @@
    cout << "Using robot file " << filename << endl;
    //...
 }
-~~~~~~   
+~~~~~~
+
+## VirtualRobot::SceneObject
+
+All objects in VirtualRobot that may have a visualization (e.g. Robots, RobotNodes, Obstacles) are derived from the class SceneObject, which allows a hierarchical scene representation. SceneObjects may have children which are automatically updated/positioned when the parent is moved.

 # Accessing a Robot

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 10:48:58 -0000

--- v50
+++ v51
@@ -522,8 +522,10 @@

    * The `ReachabilityMap` tool can be used to examine the behavior of this approach.

-[[img src=Reachability_iCub_HipArm.jpg width=50% alt="The reachability distribution of iCub's kinematic chain covering 3 hip and 7 arm joints."]][[img src=Reachability_HipRightArm_ArmarIII.png width=50% alt="The `reachability demo` tool allows to create and visualize reachability data."]]
-[[img src=Reachmap.png width=50% alt="The `reachability map` tool is used visualize the reachability map for a specific grasp."]][[img src=ReachMapAll.png width=50% alt="The ''reachability map'' tool is used visualize the reachability map for a set of grasp."]]
+[[img src=Reachability_iCub_HipArm.jpg width=50% alt="The reachability distribution of iCub's kinematic chain covering 3 hip and 7 arm joints."]]
+[[img src=Reachability_HipRightArm_ArmarIII.png width=50% alt="The `reachability demo` tool allows to create and visualize reachability data."]]
+[[img src=Reachmap.png width=50% alt="The `reachability map` tool is used visualize the reachability map for a specific grasp."]]
+[[img src=ReachMapAll.png width=50% alt="The `reachability map` tool is used visualize the reachability map for a set of grasp."]]

 # Jacobians and IK solving

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 10:48:07 -0000

--- v49
+++ v50
@@ -178,9 +178,8 @@
 std::vector contacts = eef->closeActors(obstacle);
 ~~~~~~

-[[img src=Tutorial9.png width=300px alt="A model of iCub's left hand."]] 
-[[img src=Tutorial9c.png width=300px alt="The model after executing the ''close'' command. The fingers are closed until self contact is determined."]] 
-[[img src=ContactPointsBox.png width=260px alt="The object is considered for collisions and all contacts with the object are visualized."]] 
+[[img src=Tutorial9.png width=50% alt="A model of iCub's left hand."]][[img src=Tutorial9c.png width=50% alt="The model after executing the ''close'' command. The fingers are closed until self contact is determined."]] 
+[[img src=ContactPointsBox.png width=50% alt="The object is considered for collisions and all contacts with the object are visualized."]] 

 # Obstacles and Grasps

@@ -248,8 +247,7 @@
 std::vector objectSets = scene->getSceneObjectSets();
 ~~~~~~    

-[[img src=Wok_grasps_EEF.png width=300px alt="A wok with several grasps defined for ARMAR-III's right hand."]]
-[[img src=Tutorial10.png width=300px alt="A scene with two robots and objects."]]
+[[img src=Wok_grasps_EEF.png width=50% alt="A wok with several grasps defined for ARMAR-III's right hand."]][[img src=Tutorial10.png width=50% alt="A scene with two robots and objects."]]

 # Visualization

@@ -524,10 +522,8 @@

    * The `ReachabilityMap` tool can be used to examine the behavior of this approach.

-[[img src=Reachability_iCub_HipArm.jpg width=300px alt="The reachability distribution of iCub's kinematic chain covering 3 hip and 7 arm joints."]]
-[[img src=Reachability_HipRightArm_ArmarIII.png width=300px alt="The ''reachability demo'' tool allows to create and visualize reachability data."]]
-[[img src=Reachmap.png width=300px alt="The ''reachability map''' tool is used visualize the reachability map for a specific grasp."]]
-[[img src=ReachMapAll.png width=300px alt="The ''reachability map'' tool is used visualize the reachability map for a set of grasp."]]
+[[img src=Reachability_iCub_HipArm.jpg width=50% alt="The reachability distribution of iCub's kinematic chain covering 3 hip and 7 arm joints."]][[img src=Reachability_HipRightArm_ArmarIII.png width=50% alt="The `reachability demo` tool allows to create and visualize reachability data."]]
+[[img src=Reachmap.png width=50% alt="The `reachability map` tool is used visualize the reachability map for a specific grasp."]][[img src=ReachMapAll.png width=50% alt="The ''reachability map'' tool is used visualize the reachability map for a set of grasp."]]

 # Jacobians and IK solving

@@ -585,7 +581,7 @@
 Eigen::MatrixXf jacI = j->getPseudoInverseJacobianMatrix(tcp, IKSolver::All);
 ~~~~~~

-[[img src=IKDemo.png width=300px alt="The `GenericIKDemo`."]]
+[[img src=IKDemo.png width=50% alt="The `GenericIKDemo`"]]

 # Hierarchical IK solving

@@ -629,7 +625,7 @@
 rnsJointsAll->setJointValues(jv);
 ~~~~~~

-[[img src=HierarchicalIK.png width=300px alt="The `IK soving with multiple objectives`."]]
+[[img src=HierarchicalIK.png width=50% alt="The `IK soving with multiple objectives`"]]

 # Stability computations

@@ -671,4 +667,4 @@
 comIK.solveIK(0.3f,0,20);
 ~~~~~~

-[[img src=StabilityDemo.png width=300px alt="`StabilityDemo`."]]
+[[img src=StabilityDemo.png width=50% alt="`StabilityDemo`"]]

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 10:24:11 -0000

--- v48
+++ v49
@@ -8,7 +8,7 @@

 ## Shared Pointers

-`Simox` uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: [http://boost.org/libs/smart_ptr/smart_ptr.htm](http://boost.org/libs/smart_ptr/smart_ptr.htm). In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers: 
+`Simox` uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: . In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers: 

 ~~~~~~    
 typedef boost::shared_ptr classPtr;
@@ -18,7 +18,7 @@

 ## Matrices and Poses

-Internally poses (which consist of a location and orientation) are handled with homogeneous 4x4 matrices. Therefore, the `Eigen::Matrix4f` type is used. Further information can be found at [http://eigen.tuxfamily.org](http://eigen.tuxfamily.org). 
+Internally poses (which consist of a location and orientation) are handled with homogeneous 4x4 matrices. Therefore, the `Eigen::Matrix4f` type is used. Further information can be found at . 

 ## Namespaces

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 10:03:01 -0000

--- v47
+++ v48
@@ -113,7 +113,7 @@
 float v =  rn->getJointValue();
 ~~~~~~

-The pose of the \'\'RobotNode\'s\'\' coordiante system can be retrieved with
+The pose of the `RobotNode`'s coordiante system can be retrieved with

 ~~~~~~
 Eigen::Matrix4f p = rn->getGlobalPose();

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 10:00:50 -0000

--- v46
+++ v47
@@ -8,7 +8,7 @@

 ## Shared Pointers

-`Simox` uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: http://boost.org/libs/smart_ptr/smart_ptr.htm. In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers: 
+`Simox` uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: [http://boost.org/libs/smart_ptr/smart_ptr.htm](http://boost.org/libs/smart_ptr/smart_ptr.htm). In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers: 

 ~~~~~~    
 typedef boost::shared_ptr classPtr;
@@ -18,7 +18,7 @@

 ## Matrices and Poses

-Internally poses (which consist of a location and orientation) are handled with homogeneous 4x4 matrices. Therefore, the `Eigen::Matrix4f` type is used. Further information can be found at http://eigen.tuxfamily.org. 
+Internally poses (which consist of a location and orientation) are handled with homogeneous 4x4 matrices. Therefore, the `Eigen::Matrix4f` type is used. Further information can be found at [http://eigen.tuxfamily.org](http://eigen.tuxfamily.org). 

 ## Namespaces

@@ -257,10 +257,10 @@

 ## Open Inventor / Coin3D (recommended) ##

-To use Coin3D's Open Inventor API we recommend to perform a setup together with [Qt](http://qt.nokia.com) and SoQt (a Coin3D library that allows to easily render OpenInventor scenes to a Qt widget).  
+To use Coin3D's Open Inventor API we recommend to perform a setup together with [Qt](http://qt-project.org/) and SoQt (a Coin3D library that allows to easily render OpenInventor scenes to a Qt widget).  
 For this example, we assume that you have created an Qt-UI class (e.g. with QtDesigner) that holds an empty Qt::Frame named `frameViewer`.

-The following code examples are taken from the VirtualRobot example [RobotViewer](Examples#robotviewer). 
+The following code examples are taken from the VirtualRobot example [RobotViewer](Examples/#robotviewer). 

   * The following variables are needed to setup your environment. The Coin3D's `SoSeparator`s are used to hold the Open Inventor data and the `SoQtExaminerViewerSoQt` class from the SoQt library is used to embed a Qt widget for rendering.
@@ -307,7 +307,7 @@

 A robot and objects can be visualized in a Qt window using the VirtualRobot::osgViewerWidget class. This widget supports OSG renderings and it can be embedded in an Qt::Frame (you can setup your window with QtDesigner and add an empty Frame that is accessed from the code). 

-The following code examples are taken from the VirtualRobot example [RobotViewerOSG](Examples#RobotViewerOSG). 
+The following code examples are taken from the VirtualRobot example [RobotViewerOSG](Examples/#robotviewerosg). 

   * For setting up your environment use the following varaibles: 

@@ -452,7 +452,7 @@
 ~~~~~~  

     * Demo  
-      A reference implementation of reachability analysis can be found in VirtualRobot's example directory. The _ReachabilityDemo_ can be used to build and visualize reachability data for custom robots and manipulators. The tool can be used to show myRobot.xml with a custom reachability file myReachFile.xml and by specifying the main TCP direction for visualization purposes. 
+      A reference implementation of reachability analysis can be found in VirtualRobot's example directory. The `ReachabilityDemo` can be used to build and visualize reachability data for custom robots and manipulators. The tool can be used to show myRobot.xml with a custom reachability file myReachFile.xml and by specifying the main TCP direction for visualization purposes. 

 ~~~~~~
        ReachabilityDemo --robot myRobot.xml --reachability myReachFile.bin --visualizationTCPAxis (1,0,0)

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 09:53:46 -0000

--- v45
+++ v46
@@ -11,7 +11,7 @@
 `Simox` uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: http://boost.org/libs/smart_ptr/smart_ptr.htm. In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers:

 ~~~~~~    
-    typedef boost::shared_ptr classPtr;
+typedef boost::shared_ptr classPtr;
 ~~~~~~

 Hence the pointer to an instance of a `VirtualRobot::Robot` object can be written as `VirtualRobot::RobotPtr` (instead of the long version `boost::shared_ptr`). Please note, that you never need to delete a pointer, since the smart pointer's reference counting system does this for you. 
@@ -33,49 +33,50 @@
 Several paths to data files are defined during the compilation process (E.g. the main data directory `SimoxDir/VirtualRobot/data` containing robot models, obstacles and precomputed grasp sets). Further, it is possible to add custom paths to the list of data paths in order to avoid absolute path names during execution. A data path can be set with 

 ~~~~~~
-    VirtualRobot::RuntimeEnvironment::addDataPath("/path/to/add");
+VirtualRobot::RuntimeEnvironment::addDataPath("/path/to/add");
 ~~~~~~    

 And the list of all paths can be retrieved with 

 ~~~~~~
-    std::vector allDataPaths = VirtualRobot::RuntimeEnvironment::getDataPaths();
+std::vector allDataPaths = VirtualRobot::RuntimeEnvironment::getDataPaths();
 ~~~~~~    

 In order to specify search paths from the command line, the build in command line parsing of Simox can be used. When starting your application with 

 ~~~~~~
-    ./yourBinary --data-path "my/path/to/data"
+./yourBinary --data-path "my/path/to/data"
 ~~~~~~    

 the specified path will automatically added to the list of search paths when you process the command line as follows: 

 ~~~~~~
-    int main(int argc, char *argv[])
-    {
-        VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
-        // ...
-    }
+int main(int argc, char *argv[])
+{
+   VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
+   // ...
+}
 ~~~~~~    

 When you want to pass custom flags to your program you can use the RuntimeEnvironment methods as well. Have a look at the following example: 

 ~~~~~~
-    int main(int argc, char *argv[])
-    {
-        VirtualRobot::RuntimeEnvironment::considerKey("robot");
-        VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
-        VirtualRobot::RuntimeEnvironment::print();
-        std::string filename(VR_BASE_DIR "/data/robots/ArmarIII/ArmarIII.xml");
-        if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
-        {
-            std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot");
-            // check if the path is absolute or if can be created 
-            if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile))
-                filename = robFile;
-        }
-        cout << "Using robot file " << filename << endl;
-        //...
+int main(int argc, char *argv[])
+{
+   VirtualRobot::RuntimeEnvironment::considerKey("robot");
+   VirtualRobot::RuntimeEnvironment::processCommandLine(argc,argv);
+   VirtualRobot::RuntimeEnvironment::print();
+   std::string filename(VR_BASE_DIR "/data/robots/ArmarIII/ArmarIII.xml");
+   if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
+   {
+       std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot");
+       // check if the path is absolute or if can be created 
+       if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile))
+           filename = robFile;
+   }
+   cout << "Using robot file " << filename << endl;
+   //...
+}
 ~~~~~~   

 # Accessing a Robot
@@ -83,67 +84,78 @@
 A robot can be loaded as follows:

 ~~~~~~
-   VirtualRobot::RobotPtr robot;
-   try
-   {
-       robot = VirtualRobot::RobotIO::loadRobot(filename);
-   } catch (VirtualRobot::VirtualRobotException &e)
-   {
-       std::cout << "Error: " << e.what() << std::endl;
-   }
+VirtualRobot::RobotPtr robot;
+try
+{
+   robot = VirtualRobot::RobotIO::loadRobot(filename);
+} catch (VirtualRobot::VirtualRobotException &e)
+{
+   std::cout << "Error: " << e.what() << std::endl;
+}
 ~~~~~~

 ## RobotNodes: Accessing joints ##
 A `RobotNode` can be retrieved with

 ~~~~~~
-   VirtualRobot::RobotNodePtr rn = robot->getRobotNode("name of robot node");
+VirtualRobot::RobotNodePtr rn = robot->getRobotNode("name of robot node");
 ~~~~~~

 The joint value can be set with 

 ~~~~~~
-   rn->setJointValue(myJointValueRadian);
+rn->setJointValue(myJointValueRadian);
 ~~~~~~

 The joint value can be retrieved with

 ~~~~~~
-   float v =  rn->getJointValue();
+float v =  rn->getJointValue();
 ~~~~~~

 The pose of the \'\'RobotNode\'s\'\' coordiante system can be retrieved with

 ~~~~~~
-   Eigen::Matrix4f p = rn->getGlobalPose();
+Eigen::Matrix4f p = rn->getGlobalPose();
+~~~~~~
+
+## RobotConfig: Configurations of the robot
+A `RobotConfig` represents a name,value map of joint values.
+
+~~~~~~
+// Consider all RobotNodes
+RobotConfigPtr c = robot->getConfig();
+c->setConfig("joint 1", 0.5f);
+robot->setConfig(c);
 ~~~~~~

 ## RobotNodeSets
 A set of `RobotNodes` defined in the robot's XML file can be accessed with

 ~~~~~~
-   VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("name of robot node set");
-   std::vector jointValues = rns->getJointValues();
-   rns->setJointValues(jointValues);
+VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("name of robot node set");
+std::vector jointValues = rns->getJointValues();
+rns->setJointValues(jointValues);
 ~~~~~~

 When a kinematic chain is defined via a `RobotNodeSet`, its TCP node can be retrieved: 
-    
-    VirtualRobot::RobotNodePtr tcp = rns->getTCP();
-    
+
+~~~~~~    
+VirtualRobot::RobotNodePtr tcp = rns->getTCP();
+~~~~~~

 ## Coordinate Transformations

 Usually poses of objects and RobotNodes are given in the global coordinate system, but they can be transformed to local coordinate systems of other objects and vice versa. E.g. an object's position can be transformed to the hand coordinate system. 

 ~~~~~~
-    Eigen::Matrix4f localObjectPose robotNodeHand->toLocalCoordinateSystem(poseObjectGlobal);
+Eigen::Matrix4f localObjectPose robotNodeHand->toLocalCoordinateSystem(poseObjectGlobal);
 ~~~~~~    

 In the following example the visually localized pose of a hand, given in the camera coordinate system, located in the left eye, is transformed to the global coordinate system: 

 ~~~~~~
-    Eigen::Matrix4f globalHandPose robotNodeEyeLeft->toGlobalCoordinateSystem(localHandPose);
+Eigen::Matrix4f globalHandPose robotNodeEyeLeft->toGlobalCoordinateSystem(localHandPose);
 ~~~~~~    

 ## End Effectors
@@ -151,27 +163,24 @@
 An end effector, defined in the XML file of a robot, can be accessed as follows: 

 ~~~~~~
-    VirtualRobot::EndEffectorPtr eef = robot->getEndEffector("name of eef");
+VirtualRobot::EndEffectorPtr eef = robot->getEndEffector("name of eef");
 ~~~~~~    

 This eef can be closed, while self-collisons are considered: 

 ~~~~~~
-    std::vector contacts = eef->closeActors();
+std::vector contacts = eef->closeActors();
 ~~~~~~    

 While grasping one or multiple obstacles can be considered: 

 ~~~~~~
-    std::vector contacts = eef->closeActors(obstacle);
-~~~~~~    
-
-  
-
-
-[[img src=Tutorial9.png width=100%]] 
-[[img src=Tutorial9c.png width=100%]] 
-[[img src=ContactPointsBox.png width=100%]] 
+std::vector contacts = eef->closeActors(obstacle);
+~~~~~~    
+
+[[img src=Tutorial9.png width=300px alt="A model of iCub's left hand."]] 
+[[img src=Tutorial9c.png width=300px alt="The model after executing the ''close'' command. The fingers are closed until self contact is determined."]] 
+[[img src=ContactPointsBox.png width=260px alt="The object is considered for collisions and all contacts with the object are visualized."]] 

 # Obstacles and Grasps

@@ -179,68 +188,68 @@
 Simple obstacles can be created as follows: 

 ~~~~~~
-    VirtualRobot::ObstaclePtr box = VirtualRobot::Obstacle::createBox(30.0f,30.0f,30.0f); // in mm
+VirtualRobot::ObstaclePtr box = VirtualRobot::Obstacle::createBox(30.0f,30.0f,30.0f); // in mm
 ~~~~~~    

 They can be positioned via homogeneous matrices: 

 ~~~~~~
-    box->setGlobalPose(matrix4x4);
+box->setGlobalPose(matrix4x4);
 ~~~~~~    

 Obstacles that can be grasped are called `ManipulationObjects` and offer the (optional) possibility to define grasps for a specific robot/end effector. 

 ~~~~~~
-    VirtualRobot::ManipulationObjectPtr object = VirtualRobot::ObjectIO::loadManipulationObject("filename.xml");
+VirtualRobot::ManipulationObjectPtr object = VirtualRobot::ObjectIO::loadManipulationObject("filename.xml");
 ~~~~~~    

 The corresponding grasps can be retrieved with: 

 ~~~~~~
-    VirtualRobot::GraspSetPtr grasps = object->getGraspSet("myRobot", "myEEF");
+VirtualRobot::GraspSetPtr grasps = object->getGraspSet("myRobot", "myEEF");
 ~~~~~~    

 A grasp can be accessed as follows: 

 ~~~~~~
-    VirtualRobot::GraspPtr g = grasps->getGrasp(0);
-    VirtualRobot::GraspPtr g2 = grasps->getGrasp("name of grasp");
+VirtualRobot::GraspPtr g = grasps->getGrasp(0);
+VirtualRobot::GraspPtr g2 = grasps->getGrasp("name of grasp");
 ~~~~~~    

 A grasp basically defines a object related pose of the end effector, whereas the pose of the end effector is given in its TCP coordinate system. To get the resulting pose for the TCP, when considering the object at pose `mObject`, you can use the following method: 

 ~~~~~~
-    Eigen::Matrix4f mTCP = g->getTcpPoseGlobal(mObject);
+Eigen::Matrix4f mTCP = g->getTcpPoseGlobal(mObject);
 ~~~~~~    

 If you want to compute the pose that an object has to be set to, in order that the grasp `g` can be applied, you need to pass a robot, which is used to retrieve the current configuration: 

 ~~~~~~
-    Eigen::Matrix4f mObject = g->getTargetPoseGlobal(robot);
+Eigen::Matrix4f mObject = g->getTargetPoseGlobal(robot);
 ~~~~~~    

 Scenes are a collection of robots, obstacles, and `ManipulationObject`s. A scene can be loaded and accessed in the following way: 

 ~~~~~~
-    VirtualRobot::ScenePtr scene = VirtualRobot::SceneIO:loadScene("scenefile.xml");
-    
-    // access robots
-    std::vector robots = scene->getRobots();
-    VirtualRobot::RobotPtr robot = scene->getRobot("name of robot");
-    
-    // get all configurations for a specific robot
-     std::vector configs = scene->getRobotConfigs(robot);
- 
-   // get all Obstacles/ManipulationObjects
-   std::vector manipObjects = scene->getManipulationObjects();
-   std::vector obstacles = scene->getObstacles();
- 
-   // get sets of SceneObjects (== obstacles or manipulationobjects)
-   std::vector objectSets = scene->getSceneObjectSets();
-~~~~~~    
-
-[[img src=Wok_grasps_EEF.png width=300px]]
-[[img src=Tutorial10.png width=300px]]
+VirtualRobot::ScenePtr scene = VirtualRobot::SceneIO:loadScene("scenefile.xml");
+
+// access robots
+std::vector robots = scene->getRobots();
+VirtualRobot::RobotPtr robot = scene->getRobot("name of robot");
+
+// get all configurations for a specific robot
+ std::vector configs = scene->getRobotConfigs(robot);
+
+// get all Obstacles/ManipulationObjects
+std::vector manipObjects = scene->getManipulationObjects();
+std::vector obstacles = scene->getObstacles();
+
+// get sets of SceneObjects (== obstacles or manipulationobjects)
+std::vector objectSets = scene->getSceneObjectSets();
+~~~~~~    
+
+[[img src=Wok_grasps_EEF.png width=300px alt="A wok with several grasps defined for ARMAR-III's right hand."]]
+[[img src=Tutorial10.png width=300px alt="A scene with two robots and objects."]]

 # Visualization

@@ -248,7 +257,7 @@

 ## Open Inventor / Coin3D (recommended) ##

-To use Coin3D's Open Inventor API we recommend to perform a setup together with [Qt](qt.nokia.com) and SoQt (a Coin3D library that allows to easily render OpenInventor scenes to a Qt widget).  
+To use Coin3D's Open Inventor API we recommend to perform a setup together with [Qt](http://qt.nokia.com) and SoQt (a Coin3D library that allows to easily render OpenInventor scenes to a Qt widget).  
 For this example, we assume that you have created an Qt-UI class (e.g. with QtDesigner) that holds an empty Qt::Frame named `frameViewer`.

 The following code examples are taken from the VirtualRobot example [RobotViewer](Examples#robotviewer). 
@@ -266,32 +275,32 @@
   * Initialization 

 ~~~~~~
-    sceneSep = new SoSeparator;
-    sceneSep->ref();
-    robotSep = new SoSeparator;
-    sceneSep->addChild(robotSep);
-    viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);
-    // the following setting is needed if you want to visualize highlighted parts of the robot
-    viewer->setGLRenderAction(new SoLineHighlightRenderAction);
-    
-    
-    // some optional parameters
-    viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
-    viewer->setAccumulationBuffer(true);
-    viewer->setAntialiasing(true, 4);
-    viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
-    viewer->setFeedbackVisibility(true);
-    viewer->setSceneGraph(sceneSep);
-    viewer->viewAll();
+   sceneSep = new SoSeparator;
+   sceneSep->ref();
+   robotSep = new SoSeparator;
+   sceneSep->addChild(robotSep);
+   viewer = new SoQtExaminerViewer(UI.frameViewer,"",TRUE,SoQtExaminerViewer::BUILD_POPUP);
+   // the following setting is needed if you want to visualize highlighted parts of the robot
+   viewer->setGLRenderAction(new SoLineHighlightRenderAction);
+
+
+   // some optional parameters
+   viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
+   viewer->setAccumulationBuffer(true);
+   viewer->setAntialiasing(true, 4);
+   viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
+   viewer->setFeedbackVisibility(true);
+   viewer->setSceneGraph(sceneSep);
+   viewer->viewAll();
 ~~~~~~    

   * A loaded robot can be added to the visualization as follows: 

 ~~~~~~    
-    SceneObject::VisualizationType visuMode = SceneObject::Full;
-    visualization = robot->getVisualization(visuMode);
-    SoNode* visualisationNode = visualisationNode = visualization->getCoinVisualization();
-    robotSep->addChild(visualisationNode);
+   SceneObject::VisualizationType visuMode = SceneObject::Full;
+   visualization = robot->getVisualization(visuMode);
+   SoNode* visualisationNode = visualisationNode = visualization->getCoinVisualization();
+   robotSep->addChild(visualisationNode);
 ~~~~~~

 ## OpenSceneGraph
@@ -303,29 +312,29 @@
   * For setting up your environment use the following varaibles: 

 ~~~~~~
-    osg::Group* osgRoot;    
-    osg::Group* osgRobot;
-    VirtualRobot::osgViewerWidget* osgWidget;
-    boost::shared_ptr visualization;
+   osg::Group* osgRoot;     
+   osg::Group* osgRobot;
+   VirtualRobot::osgViewerWidget* osgWidget;
+   boost::shared_ptr visualization;
 ~~~~~~    

   * Initialization 

 ~~~~~~
-    osgRoot = new osg::Group();
-    osgRobot = new osg::Group();
-    osgRoot->addChild(osgRobot);
-    osgWidget = new VirtualRobot::osgViewerWidget(osgRoot,UI.frameViewer);
-    sgWidget->show();
+   osgRoot = new osg::Group();
+   osgRobot = new osg::Group();
+   osgRoot->addChild(osgRobot);
+   osgWidget = new VirtualRobot::osgViewerWidget(osgRoot,UI.frameViewer);
+   sgWidget->show();
 ~~~~~~    

   * A loaded robot can be added to the visualization as follows: 

 ~~~~~~
-    SceneObject::VisualizationType visuMode = SceneObject::Full;
-    visualization = robot->getVisualization(visuMode);
-    osg::Node* visualisationNode = visualisationNode = visualization->getOSGVisualization();
-    osgRobot->addChild(visualisationNode);
+   SceneObject::VisualizationType visuMode = SceneObject::Full;
+   visualization = robot->getVisualization(visuMode);
+   osg::Node* visualisationNode = visualisationNode = visualization->getOSGVisualization();
+   osgRobot->addChild(visualisationNode);
 ~~~~~~    

 # Collision Detection
@@ -343,17 +352,18 @@
 In Simox a global collision checker singleton exists that can be accessed as follows: 

 ~~~~~~
-    VirtualRobot::CollisionCheckerPtr collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
+VirtualRobot::CollisionCheckerPtr collisionChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
 ~~~~~~    

 If you do not plan to implement multi-threaded collision queries, this collision checker object is all you need. 

 ## Collision Models

-All models (Objects, Obstacles, Robots, RobotNodes, etc) in Simox have two 3D representations. One for visualization (high def) and one for collision detection purposes (low def). You can visualize the models as well as the internal triangulated data structure by passing teh corresponding VirtualRobot::SceneObject::VisualizationType flag (Full, Collison, CollisionData) to the visualization routines. The collision models are usually defined in the robot's or object's XML description file. You can access them with: 
-    
-~~~~~~
-    VirtualRobot::CollisionModelPtr colModel = object->getCollisionModel();
+All models (Objects, Obstacles, Robots, RobotNodes, etc) in Simox have two 3D representations. One for visualization (high def) and one for collision detection purposes (low def). You can visualize the models as well as the internal triangulated data structure by passing teh corresponding VirtualRobot::SceneObject::VisualizationType flag (Full, Collison, CollisionData) to the visualization routines. The collision models are usually defined in the robot's or object's XML description file. 
+You can access them with: 
+    
+~~~~~~
+VirtualRobot::CollisionModelPtr colModel = object->getCollisionModel();
 ~~~~~~    

 ## Collision and Distance Queries
@@ -361,31 +371,31 @@
 The collision checker can be asked if two models are in collision or not: 

 ~~~~~~
-    bool inCollision = collisionChecker->checkCollision(colModel1,colModel2);
+bool inCollision = collisionChecker->checkCollision(colModel1,colModel2);
 ~~~~~~    

 Additionally, the shortest distance between the two surfaces can be queried. The following code fragment shows how to get the shortest distance and the corresponding points on the object's surface: 

 ~~~~~~
-    Eigen::Vector3f surface1,surface2;
-    float dist = collisionChecker->calculateDistance(colModel1,colModel2,surface1,surface2);
+Eigen::Vector3f surface1,surface2;
+float dist = collisionChecker->calculateDistance(colModel1,colModel2,surface1,surface2);
 ~~~~~~    

 Instead of using the collision models, SceneObjects (and all derived objects like Obstacles and RobotNodes) can be directly passed to the collision checker: 

 ~~~~~~
-    VirtualRobot::ObstaclePtr obstacle = VirtualRobot::Obstacle::createBox(100.0f,100.0f,100.0f);
-    o->setGlobalPose(Eigen::matrix4f::Identity());
-    VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode("Joint1");
-    bool inCollision = collisionChecker->checkCollision(robotNode,obstacle);
+VirtualRobot::ObstaclePtr obstacle = VirtualRobot::Obstacle::createBox(100.0f,100.0f,100.0f);
+obstacle->setGlobalPose(Eigen::matrix4f::Identity());
+VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode("Joint1");
+bool inCollision = collisionChecker->checkCollision(robotNode,obstacle);
 ~~~~~~    

 When operating with sets of objects (like kinematic chains, or a bunch of obstacles) you can use SceneObjectSets and RobotNodeSets as well: 

 ~~~~~~
-    VirtualRobot::ObstacleSetPtr obstacles = scene->getSceneObjectSet("All Obstacles");
-    VirtualRobot::RobotNodeSetPtr kinChain = robot->getRobotNodeSet("Left Arm");
-    float minDist = collisionChecker->calculateDistance(kinChain,obstacles);
+VirtualRobot::ObstacleSetPtr obstacles = scene->getSceneObjectSet("All Obstacles");
+VirtualRobot::RobotNodeSetPtr kinChain = robot->getRobotNodeSet("Left Arm");
+float minDist = collisionChecker->calculateDistance(kinChain,obstacles);
 ~~~~~~    

 ## Complex Collision Setups
@@ -395,11 +405,11 @@
 In the following example an obstacle and a robot node are selected for collision detcetion. Additionally any slef collisions between the kinematic structures of the arm and the torso and head should be considered. 

 ~~~~~~
-    VirtualRobot::CDManagerPtr cdm(new VirtualRobot::CDManager());
-    cdm->addCollisionPair(obstacle,robotNode);
-    cdm->addCollisionPair(rnsArm,rnsTorso);
-    cdm->addCollisionPair(rnsArm,rnsHead);
-    bool inCollision = cdm->isInCollision();
+VirtualRobot::CDManagerPtr cdm(new VirtualRobot::CDManager());
+cdm->addCollisionPair(obstacle,robotNode);
+cdm->addCollisionPair(rnsArm,rnsTorso);
+cdm->addCollisionPair(rnsArm,rnsHead);
+bool inCollision = cdm->isInCollision();
 ~~~~~~    

 # Workspace Analysis: Reachability and Manipulability
@@ -411,118 +421,215 @@
   * **Reachability**: This implementation can be used to encode reachable 6D poses with a simple quality measure which is related to the volume in joint space that maps to a voxel. 
     * Creation  
       Assuming a `robot` and a RobotNodeSet `rns` exist, an empty instance of the reachability can be created as follows:  
-
-            ReachabilityPtr reachSpace(new Reachability(robot));
-            reachSpace->initialize(rns,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);
-            reachSpace->print();
+      
+~~~~~~
+       ReachabilityPtr reachSpace(new Reachability(robot));
+       reachSpace->initialize(rns,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);
+       reachSpace->print();
+~~~~~~
+
     * Filling with data and IO  
       The reachability representation is initialized with two discretization parameters: discrTrans, the voxel size in translational components and discrRot, teh discretization for the three rotational dimensions. Further, the bounding box and in case self-collisions should be considered, a setup for collision detection is passed.  
 The reachability data can be filled by adding randomly generated robot poses (e.g. 1 mio poses): 

-            reachSpace->addRandomTCPPoses(1000000);
-            reachSpace->print();
-            reachSpace->save("myReachFile.bin");
-            reachSpace.reset(new Reachability(robot));
-            reachSpace->load("myReachFile.bin");
-            reachSpace->print();
-  
+~~~~~~
+       reachSpace->addRandomTCPPoses(1000000);
+       reachSpace->print();
+       reachSpace->save("myReachFile.bin");
+       reachSpace.reset(new Reachability(robot));
+       reachSpace->load("myReachFile.bin");
+       reachSpace->print();
+~~~~~~
+
     * Querying  
       The reachability data can be queried extremely fast. Here is an example: 

-
-            // get all grasps that are reachable
-            ManipulationObjectPtr object; // an object
-            // load object ....
-            GraspSetPtr reachableGrasps = reachSpace->getReachableGrasps(allGrasps, object);   
+~~~~~~
+       // get all grasps that are reachable
+       ManipulationObjectPtr object; // an object
+       // load object ....
+       GraspSetPtr reachableGrasps = reachSpace->getReachableGrasps(allGrasps, object); 
+~~~~~~  

     * Demo  
       A reference implementation of reachability analysis can be found in VirtualRobot's example directory. The _ReachabilityDemo_ can be used to build and visualize reachability data for custom robots and manipulators. The tool can be used to show myRobot.xml with a custom reachability file myReachFile.xml and by specifying the main TCP direction for visualization purposes. 

-            ReachabilityDemo --robot myRobot.xml --reachability myReachFile.bin --visualizationTCPAxis (1,0,0)
+~~~~~~
+       ReachabilityDemo --robot myRobot.xml --reachability myReachFile.bin --visualizationTCPAxis (1,0,0)
+~~~~~~

   * **Manipulability**: This class encapsulates an enhanced workspace representation that uses a basic manipulability measure for determining the quality of robot poses. Custom implementations of quality measures can be used as well. 
     * Here is a short example: 

-            ManipulabilityPtr manipulability(new Manipulability(robot));
-            PoseQualityManipulabilityPtr measure(new PoseQualityManipulability(rns)); // basic manipulability measure
-            measure->penalizeJointLimits(true,50000); // penalize poses near joint limits
-            manipulability->setManipulabilityMeasure(measure);
-            float minB[6];
-            float maxB[6];
-            float maxManip;
-            // automatically determine parameters
-            manipulability->checkForParameters(rns,2000,minB,maxB,maxManip,baseNode,tcpNode);
-            manipulability->initialize(rns,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);
-            manipulability->setMaxManipulability(maxManip);
-            manipulability->print();   
+~~~~~~
+       ManipulabilityPtr manipulability(new Manipulability(robot));
+       PoseQualityManipulabilityPtr measure(new PoseQualityManipulability(rns)); // basic manipulability measure
+       measure->penalizeJointLimits(true,50000); // penalize poses near joint limits
+       manipulability->setManipulabilityMeasure(measure);
+       float minB[6];
+       float maxB[6];
+       float maxManip;
+       // automatically determine parameters
+       manipulability->checkForParameters(rns,2000,minB,maxB,maxManip,baseNode,tcpNode);
+       manipulability->initialize(rns,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);
+       manipulability->setMaxManipulability(maxManip);
+       manipulability->print();  
+~~~~~~ 

     * Data and hole filling  
 Add some random configs and smooth the result: 

-            manipulability->addRandomTCPPoses(steps);
-            manipulability->smooth(2);
-            manipulability->print();  
+~~~~~~
+       manipulability->addRandomTCPPoses(steps);
+       manipulability->smooth(2);
+       manipulability->print();  
+~~~~~~

   * **Reachability Maps** are useful to determine promising robot base positions for grasping. Based on a reachability distribution of the manipulator, the reachability map can becomputed as a discretized 2D distribution serving potential robot positions for reaching a grasping pose. The reachability map data structure can be built as follows: 

-            // get boundong box extends of manipulator's reachability 
-            Eigen::Vector3f minBB,maxBB;
-            reachSpace->getWorkspaceExtends(minBB,maxBB);
-            // build reahcability grid in x/y plane with identical discretization
-            WorkspaceGridPtr reachGrid(new WorkspaceGrid(minBB(0),maxBB(0),minBB(1),maxBB(1),reachSpace->getDiscretizeParameterTranslation()));
-            
-            // the target object 
-            ManipulationObjectPtr graspObject = ObjectIO::loadManipulationObject("myObject.xml");
-            // and a specific grasp
-            GraspPtr g = graspObject->getGrasp("myGrasp1");
-            
-            // position the reachability grid
-            Eigen::Matrix4f gp = graspObject->getGlobalPose();
-            reachGrid->setGridPosition(gp(0,3),gp(1,3));
-            
-            // fill the grid according to one specific grasp
-            reachGrid->fillGridData(reachSpace, graspObject, g, robot->getRootNode() );
-            
-            
-            // or: fill the grid with all grasps
-            EndEffectorPtr eef = robot->getEndEffector("Right Hand");
-            GraspSetPtr grasps = graspObject->getGraspSet(eef);
-            for (int i=0;i jacobies;
-            
-            // first Right feet pose
-            DifferentialIKPtr ikLeft2Right(new DifferentialIK(rnsJointsAll));
-            Eigen::Matrix4f trafoLeft2Right = feetPosture->getTransformationLeftToRightFoot();
-            Eigen::Matrix4f goalRight = feetPosture->getLeftTCP()->getGlobalPose() * trafoLeft2Right;
-            ikLeft2Right->setGoal(goalRight,feetPosture->getRightTCP());
-            HierarchicalIK::JacobiDefinition jd;
-            jd.jacProvider = ikLeft2Right;
-            jacobies.push_back(jd);
-            
-            // second: COM
-            CoMIKPtr comIK(new CoMIK(rnsJointsAll,rnsModelsAll));
-            supportPolygon->updateSupportPolygon(10.0f);
-            Eigen::Vector2f c = MathTools::getConvexHullCenter(supportPolygon->getSupportPolygon2D());
-            comIK->setGoal(c); 
-            HierarchicalIK::JacobiDefinition jd2;
-            jd2.jacProvider = comIK;
-            jacobies.push_back(jd2);
-            
-            // third: EEF pose
-            Eigen::Matrix4f goal = tcpGoal;
-            DifferentialIKPtr ikTCP(new DifferentialIK(rnsJointsAll));
-            ikTCP->setGoal(goal,tcp);
-            HierarchicalIK::JacobiDefinition jd3;
-            jd3.jacProvider = ikTCP;
-            jacobies.push_back(jd3);
-            
-            //compute step
-            Eigen::VectorXf delta = hik->computeStep(jacobies,stepsize);
-            Eigen::VectorXf jv(delta.rows());
-            rnsJointsAll->getJointValues(jv);
-            jv += delta;
-            rnsJointsAll->setJointValues(jv);
-
-[[img src=HierarchicalIK.png  width=100%]] 
+~~~~~~
+   // get boundong box extends of manipulator's reachability 
+   Eigen::Vector3f minBB,maxBB;
+   reachSpace->getWorkspaceExtends(minBB,maxBB);
+   // build reachability grid in x/y plane with identical discretization
+   WorkspaceGridPtr reachGrid(new WorkspaceGrid(minBB(0),maxBB(0),minBB(1),maxBB(1),reachSpace->getDiscretizeParameterTranslation()));
+
+   // the target object 
+   ManipulationObjectPtr graspObject = ObjectIO::loadManipulationObject("myObject.xml");
+   // and a specific grasp
+   GraspPtr g = graspObject->getGrasp("myGrasp1");
+
+   // position the reachability grid
+   Eigen::Matrix4f gp = graspObject->getGlobalPose();
+   reachGrid->setGridPosition(gp(0,3),gp(1,3));
+
+   // fill the grid according to one specific grasp
+   reachGrid->fillGridData(reachSpace, graspObject, g, robot->getRootNode() );
+
+
+   // or: fill the grid with all grasps
+   EndEffectorPtr eef = robot->getEndEffector("Right Hand");
+   GraspSetPtr grasps = graspObject->getGraspSet(eef);
+   for (int i=0;i<(int)grasps->getSize();i++)
+   {
+       g = grasps->getGrasp(i);
+       reachGrid->fillGridData(reachSpace, graspObject, g, robot->getRootNode() );
+   }
+
+   // sample a base pose with minimum quality of 1
+   // the result is stroed in x,y together with the corresponding grasp g
+   float x,y;
+   bool ok = reachGrid->getRandomPos(1, x, y, g,);
+~~~~~~
+           
+   * The `ReachabilityMap` tool can be used to examine the behavior of this approach.
+
+[[img src=Reachability_iCub_HipArm.jpg width=300px alt="The reachability distribution of iCub's kinematic chain covering 3 hip and 7 arm joints."]]
+[[img src=Reachability_HipRightArm_ArmarIII.png width=300px alt="The ''reachability demo'' tool allows to create and visualize reachability data."]]
+[[img src=Reachmap.png width=300px alt="The ''reachability map''' tool is used visualize the reachability map for a specific grasp."]]
+[[img src=ReachMapAll.png width=300px alt="The ''reachability map'' tool is used visualize the reachability map for a set of grasp."]]
+
+# Jacobians and IK solving
+
+Simox provides a generic IK solver, which is based on the Jacobian's Pseudoinverse approach. The IK solver is instantiated with a kinematic chain and it can be queired for 3D or 6D poses. The `GenericIKDemo` and `JacobiDemo` provide exemplary implementations.
+Here is a short example:
+
+~~~~~~
+RobotNodeSetPtr kc = robot->getRobotNodeSet("Left Arm");
+// several Jacobi inversions algorithms are available
+// here, the SVD damped approach is used
+GenericIKSolverPtr ikSolver(new GenericIKSolver(kc, JacobiProvider::eSVDDamped));
+
+// solve a query (only position)
+Eigen::Matrix4f pose = kc->getTCP()->getGlobalPose();
+pose(0,3) += 10.0f; // move 10 mm
+bool ok = ikSolver->solve(pose,IKSolver::Position);
+
+// solve a query (position and orientation)
+// setup stepsize and max gradient decent loops
+// lower stepsize -> better convergence but slower
+// max loops: Just needed in special cases. Internally the iterative process is
+// canceled when no progress has been made in the last step
+ikSolver->setupJacobian(0.3f, 30); 
+ok = ikSolver->solve(pose,IKSolver::All);
+
+// Search solution to one of the grasps of the ManipulationObject
+// current pose of object is considered
+ok = ikSolver->solve(object, IKSolver::All);
+
+// Search solution for one specific grasp applied to the object
+GraspPtr g = object->getGraspSet("LeftHand")->getGrasp("Grasp0");
+ok = ikSolver->solve(object, g, IKSolver::All);
+
+// By default, the current configuration of the robot is used as start pose
+// If that failes (e.g. for large distances), the IKSolver can generate random seed poses
+// for the iterative gradient decent approach (50 in the following example).
+ok = ikSolver->solve(pose, IKSolver::Position, 50);
+~~~~~~
+
+Another example, showing how to access the Jacobians:
+
+~~~~~~
+// Setup
+RobotNodeSetPtr rns = robot->getRobotNodeSet("LeftArm");
+DifferentialIKPtr j(new DifferentialIK(rns));
+
+// setup a tcp with goal
+RobotNodePtr tcp = rns->getTcp();
+j->setGoal(targetPose,tcp,IKSolver::Position);
+
+// get the Jacobian matrix for the tcp setup
+Eigen::MatrixXf jac = j->getJacobianMatrix();
+
+// get the pseudoinverse
+Eigen::MatrixXf jacI = j->getPseudoInverseJacobianMatrix(tcp, IKSolver::All);
+~~~~~~
+
+[[img src=IKDemo.png width=300px alt="The `GenericIKDemo`."]]
+
+# Hierarchical IK solving
+
+Complex robot systems usually have to meet multiple objectives for IK solving. Such objectives could include stability, feet postures, eef positions, etc. Simox provides methods for hierarchical IK solving, where a stack of tasks is processed by solving each task in the Null Space of the preceding tasks. Therefore JoacobiProviders are implemented for several objectives like pose reaching and stability. The framework is extendable and allows to implement custom JacobiProvides in order to combine them for hierarchical IK solving.
+
+~~~~~~
+HierarchicalIKPtr hik(new HierarchicalIK(rnsJointsAll));
+std::vector jacobies;
+
+// first Right feet pose
+DifferentialIKPtr ikLeft2Right(new DifferentialIK(rnsJointsAll));
+Eigen::Matrix4f trafoLeft2Right = feetPosture->getTransformationLeftToRightFoot();
+Eigen::Matrix4f goalRight = feetPosture->getLeftTCP()->getGlobalPose() * trafoLeft2Right;
+ikLeft2Right->setGoal(goalRight,feetPosture->getRightTCP());
+HierarchicalIK::JacobiDefinition jd;
+jd.jacProvider = ikLeft2Right;
+jacobies.push_back(jd);
+
+// second: COM
+CoMIKPtr comIK(new CoMIK(rnsJointsAll,rnsModelsAll));
+supportPolygon->updateSupportPolygon(10.0f);
+Eigen::Vector2f c = MathTools::getConvexHullCenter(supportPolygon->getSupportPolygon2D());
+comIK->setGoal(c); 
+HierarchicalIK::JacobiDefinition jd2;
+jd2.jacProvider = comIK;
+jacobies.push_back(jd2);
+
+// third: EEF pose
+Eigen::Matrix4f goal = tcpGoal;
+DifferentialIKPtr ikTCP(new DifferentialIK(rnsJointsAll));
+ikTCP->setGoal(goal,tcp);
+HierarchicalIK::JacobiDefinition jd3;
+jd3.jacProvider = ikTCP;
+jacobies.push_back(jd3);
+
+//compute step
+Eigen::VectorXf delta = hik->computeStep(jacobies,stepsize);
+Eigen::VectorXf jv(delta.rows());
+rnsJointsAll->getJointValues(jv);
+jv += delta;
+rnsJointsAll->setJointValues(jv);
+~~~~~~
+
+[[img src=HierarchicalIK.png width=300px alt="The `IK soving with multiple objectives`."]]

 # Stability computations

@@ -531,14 +638,37 @@
 The center of mass of a robot (or a RobotNodeSet) can be queried as follows: 

 ~~~~~~
-    // get com of robot
-    Eigen::Vector3f com = robot->getCoMGlobal();
-~~~~~~    
-
-The support polygon is defined by the convex hull of all contact points with the floor plane. The robot is in a statically stable pose if the projection of its center of mass (COM) on the floor lies within the support polygon. It can be computed by detecting points of the feet which are near the floor, project them to the floor plane and compute their convex hull. 
-    
-~~~~~~
-    // Support polygon
-    MathTools::Plane floor =  MathTools::getFloorPlane();
-    std::vector
-~~~~~~    
+// get com of robot
+Eigen::Vector3f com = robot->getCoMGlobal();
+~~~~~~    
+
+The support polygon is defined by the convex hull of all contact points with the floor plane. The robot is in a statically stable pose if the projection of its center of mass (COM) on the floor lies within the support polygon. 
+It can be computed by detecting points of the feet which are near the floor, project them to the floor plane and compute their convex hull.
+
+~~~~~~
+// Support polygon
+MathTools::Plane floor =  MathTools::getFloorPlane();
+std::vector< CollisionModelPtr > colModels =  
+   robot->getCollisionModels();
+CollisionCheckerPtr colChecker = CollisionChecker::getGlobalCollisionChecker();
+std::vector< CollisionModelPtr >::iterator i = colModels.begin();
+std::vector< MathTools::ContactPoint > points;
+for (;i!=colModels.end();i++)
+   colChecker->getContacts(floor, *i, points, 5.0f);
+std::vector< Eigen::Vector2f > points2D;
+for (size_t u=0;u

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 08:44:14 -0000

--- v44
+++ v45
@@ -74,11 +74,60 @@
             if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile))
                 filename = robFile;
         }
-        cout  jointValues = rns->getJointValues();
-    rns->setJointValues(jointValues);
+        cout << "Using robot file " << filename << endl;
+        //...
 ~~~~~~

-When a kinematic chain is defined via a `RobotNodeSet`, its TCP node can be get: 
+# Accessing a Robot
+
+A robot can be loaded as follows:
+
+~~~~~~
+   VirtualRobot::RobotPtr robot;
+   try
+   {
+       robot = VirtualRobot::RobotIO::loadRobot(filename);
+   } catch (VirtualRobot::VirtualRobotException &e)
+   {
+       std::cout << "Error: " << e.what() << std::endl;
+   }
+~~~~~~
+
+## RobotNodes: Accessing joints ##
+A `RobotNode` can be retrieved with
+
+~~~~~~
+   VirtualRobot::RobotNodePtr rn = robot->getRobotNode("name of robot node");
+~~~~~~
+
+The joint value can be set with 
+
+~~~~~~
+   rn->setJointValue(myJointValueRadian);
+~~~~~~
+
+The joint value can be retrieved with
+
+~~~~~~
+   float v =  rn->getJointValue();
+~~~~~~
+
+The pose of the \'\'RobotNode\'s\'\' coordiante system can be retrieved with
+
+~~~~~~
+   Eigen::Matrix4f p = rn->getGlobalPose();
+~~~~~~
+
+## RobotNodeSets
+A set of `RobotNodes` defined in the robot's XML file can be accessed with
+
+~~~~~~
+   VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("name of robot node set");
+   std::vector jointValues = rns->getJointValues();
+   rns->setJointValues(jointValues);
+~~~~~~
+
+When a kinematic chain is defined via a `RobotNodeSet`, its TCP node can be retrieved: 

     VirtualRobot::RobotNodePtr tcp = rns->getTCP();

@@ -170,7 +219,7 @@
     Eigen::Matrix4f mObject = g->getTargetPoseGlobal(robot);
 ~~~~~~    

-Scenes are a collection of robots, obstacles, and ManipulationObjects. A scene can be loaded and accessed in the following way: 
+Scenes are a collection of robots, obstacles, and `ManipulationObject`s. A scene can be loaded and accessed in the following way: 

 ~~~~~~
     VirtualRobot::ScenePtr scene = VirtualRobot::SceneIO:loadScene("scenefile.xml");
@@ -180,9 +229,39 @@
     VirtualRobot::RobotPtr robot = scene->getRobot("name of robot");

     // get all configurations for a specific robot
-    std::vector visualization;
-    SoQtExaminerViewer *viewer;
-~~~~~~    
+     std::vector configs = scene->getRobotConfigs(robot);
+ 
+   // get all Obstacles/ManipulationObjects
+   std::vector manipObjects = scene->getManipulationObjects();
+   std::vector obstacles = scene->getObstacles();
+ 
+   // get sets of SceneObjects (== obstacles or manipulationobjects)
+   std::vector objectSets = scene->getSceneObjectSets();
+~~~~~~    
+
+[[img src=Wok_grasps_EEF.png width=300px]]
+[[img src=Tutorial10.png width=300px]]
+
+# Visualization
+
+Depending on the cmake setup, different visualization engines can be used. Currently [Coin3D](http://www.coin3d.org) is fully supported. Additionally, an exemplary implementation for [OpenSceneGraph](http://www.openscenegraph.org) is provided, but not all visualization features are supported right now.
+
+## Open Inventor / Coin3D (recommended) ##
+
+To use Coin3D's Open Inventor API we recommend to perform a setup together with [Qt](qt.nokia.com) and SoQt (a Coin3D library that allows to easily render OpenInventor scenes to a Qt widget).  
+For this example, we assume that you have created an Qt-UI class (e.g. with QtDesigner) that holds an empty Qt::Frame named `frameViewer`.
+
+The following code examples are taken from the VirtualRobot example [RobotViewer](Examples#robotviewer). 
+
+
+  * The following variables are needed to setup your environment. The Coin3D's `SoSeparator`s are used to hold the Open Inventor data and the `SoQtExaminerViewerSoQt` class from the SoQt library is used to embed a Qt widget for rendering.
+
+~~~~~~
+   SoSeparator *sceneSep;
+   SoSeparator *robotSep;
+   boost::shared_ptr visualization;
+   SoQtExaminerViewer *viewer;
+~~~~~~

   * Initialization 

@@ -207,6 +286,7 @@
 ~~~~~~    

   * A loaded robot can be added to the visualization as follows: 
+  
 ~~~~~~    
     SceneObject::VisualizationType visuMode = SceneObject::Full;
     visualization = robot->getVisualization(visuMode);

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Wed, 02 Jul 2014 07:47:58 -0000

--- v43
+++ v44
@@ -1,4 +1,4 @@
-Once a robot and/or scene information are created (see [ Build a robot model](RobotModels_)), one can load and access the data via the _VirtualRobot_ library. The C++ API offers convenient access, which is described in the following sections. Further information can be obtained by having a look on the source code of the examples that are provided with the library. 
+Once a robot and/or scene information are created (see [ Build a robot model](RobotModels)), one can load and access the data via the `VirtualRobot` library. The C++ API offers convenient access, which is described in the following sections. Further information can be obtained by having a look on the source code of the examples that are provided with the library.

 [TOC]

@@ -8,29 +8,29 @@

 ## Shared Pointers

-_Simox_ uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: http://boost.org/libs/smart_ptr/smart_ptr.htm. In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers: 
+`Simox` uses boost's shared pointers (also called smart pointers) in order to avoid memory leaks. If you are not familiar with shared pointer have a look at the official boost reference page: http://boost.org/libs/smart_ptr/smart_ptr.htm. In order to achieve a better readability of the source code, the following commonly used typedef is used for defining pointers: 

 ~~~~~~    
     typedef boost::shared_ptr classPtr;
 ~~~~~~

-Hence the pointer to an instance of a _VirtualRobot::Robot_ object can be written as _VirtualRobot::RobotPtr_ (instead of the long version `boost::shared_ptr`). Please note, that you never need to delete a pointer, since the smart pointer's reference counting system does this for you. 
+Hence the pointer to an instance of a `VirtualRobot::Robot` object can be written as `VirtualRobot::RobotPtr` (instead of the long version `boost::shared_ptr`). Please note, that you never need to delete a pointer, since the smart pointer's reference counting system does this for you. 

 ## Matrices and Poses

-Internally poses (which consist of a location and orientation) are handled with homogeneous 4x4 matrices. Therefore, the _Eigen::Matrix4f_ type is used. Further information can be found at http://eigen.tuxfamily.org. 
+Internally poses (which consist of a location and orientation) are handled with homogeneous 4x4 matrices. Therefore, the `Eigen::Matrix4f` type is used. Further information can be found at http://eigen.tuxfamily.org. 

 ## Namespaces

-In _Simox_ three namespaces are defined: 
-
-  * _VirtualRobot_
-  * _Saba_
-  * _GraspStudio_
+In `Simox` three namespaces are defined: 
+
+  * `VirtualRobot`
+  * `Saba`
+  * `GraspStudio`

 ## VirtualRobot::RuntimeEnvironment

-Several paths to data files are defined during the compilation process (E.g. the main data directory _SimoxDir/VirtualRobot/data_ containing robot models, obstacles and precomputed grasp sets). Further, it is possible to add custom paths to the list of data paths in order to avoid absolute path names during execution. A data path can be set with 
+Several paths to data files are defined during the compilation process (E.g. the main data directory `SimoxDir/VirtualRobot/data` containing robot models, obstacles and precomputed grasp sets). Further, it is possible to add custom paths to the list of data paths in order to avoid absolute path names during execution. A data path can be set with 

 ~~~~~~
     VirtualRobot::RuntimeEnvironment::addDataPath("/path/to/add");
@@ -78,7 +78,7 @@
     rns->setJointValues(jointValues);
 ~~~~~~   

-When a kinematic chain is defined via a _RobotNodeSet_, its TCP node can be get: 
+When a kinematic chain is defined via a `RobotNodeSet`, its TCP node can be get: 

     VirtualRobot::RobotNodePtr tcp = rns->getTCP();

@@ -139,7 +139,7 @@
     box->setGlobalPose(matrix4x4);
 ~~~~~~    

-Obstacles that can be grasped are called _ManipulationObjects_ and offer the (optional) possibility to define grasps for a specific robot/end effector. 
+Obstacles that can be grasped are called `ManipulationObjects` and offer the (optional) possibility to define grasps for a specific robot/end effector. 

 ~~~~~~
     VirtualRobot::ManipulationObjectPtr object = VirtualRobot::ObjectIO::loadManipulationObject("filename.xml");
@@ -158,13 +158,13 @@
     VirtualRobot::GraspPtr g2 = grasps->getGrasp("name of grasp");
 ~~~~~~    

-A grasp basically defines a object related pose of the end effector, whereas the pose of the end effector is given in its TCP coordinate system. To get the resulting pose for the TCP, when considering the object at pose _mObject_, you can use the following method: 
+A grasp basically defines a object related pose of the end effector, whereas the pose of the end effector is given in its TCP coordinate system. To get the resulting pose for the TCP, when considering the object at pose `mObject`, you can use the following method: 

 ~~~~~~
     Eigen::Matrix4f mTCP = g->getTcpPoseGlobal(mObject);
 ~~~~~~    

-If you want to compute the pose that an object has to be set to, in order that the grasp _g_ can be applied, you need to pass a robot, which is used to retrieve the current configuration: 
+If you want to compute the pose that an object has to be set to, in order that the grasp `g` can be applied, you need to pass a robot, which is used to retrieve the current configuration: 

 ~~~~~~
     Eigen::Matrix4f mObject = g->getTargetPoseGlobal(robot);
@@ -324,13 +324,13 @@

 # Workspace Analysis: Reachability and Manipulability

-A representation of the spatial reachability of a robot's manipulator can be precomputed in order to efficiently search for reachable grasps/poses during online processing. To this end a 6D voxel grid is filled, either with binary values (reachable/not reachable) or with quality information that indicates how _good_ a pose can be reached. E.g. the manipulability can be used as such a quality measure. 
-
-From the base class for performing workspace analysis _WorkspaceRepresentation_, currently two derivations exist: 
+A representation of the spatial reachability of a robot's manipulator can be precomputed in order to efficiently search for reachable grasps/poses during online processing. To this end a 6D voxel grid is filled, either with binary values (reachable/not reachable) or with quality information that indicates how _well_ a pose can be reached. E.g. the manipulability can be used as such a quality measure. 
+
+From the base class for performing workspace analysis `WorkspaceRepresentation`, currently two derivations exist: 

   * **Reachability**: This implementation can be used to encode reachable 6D poses with a simple quality measure which is related to the volume in joint space that maps to a voxel. 
     * Creation  
-      Assuming a _robot_ and a RobotNodeSet _rns_ exist, an empty instance of the reachability can be created as follows:  
+      Assuming a `robot` and a RobotNodeSet `rns` exist, an empty instance of the reachability can be created as follows:  

             ReachabilityPtr reachSpace(new Reachability(robot));
             reachSpace->initialize(rns,discrTr,discrRo,minB,maxB,staticModel,dynamicModel,baseNode,tcpNode);

VirtualRobot modified by Valerij Wittenbeck

Valerij Wittenbeck — Tue, 01 Jul 2014 11:20:45 -0000

--- v42
+++ v43
@@ -14,7 +14,7 @@
     typedef boost::shared_ptr classPtr;
 ~~~~~~

-Hence the pointer to an instance of a _VirtualRobot::Robot_ object can be written as _VirtualRobot::RobotPtr_ (instead of the long version _boost::shared_ptr_). Please note, that you never need to delete a pointer, since the smart pointer's reference counting system does this for you. 
+Hence the pointer to an instance of a _VirtualRobot::Robot_ object can be written as _VirtualRobot::RobotPtr_ (instead of the long version `boost::shared_ptr`). Please note, that you never need to delete a pointer, since the smart pointer's reference counting system does this for you. 

 ## Matrices and Poses