[Jderobot-admin] jderobot-r1049 - in trunk/src/stable/components/gazeboserver/pluginsGazebo: . nao nao/build

bmenendez en jderobot.org bmenendez en jderobot.org
Lun Oct 14 19:47:38 CEST 2013


Author: bmenendez
Date: 2013-10-14 19:47:38 +0200 (Mon, 14 Oct 2013)
New Revision: 1049

Added:
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg
   trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h
Log:
#69 Uploaded simulated Nao. Neck, shoulders, elbows and cameras works fine.


Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/CMakeLists.txt	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,55 @@
+if (GAZEBO_INCLUDE_DIRS)
+
+include_directories(
+	${GAZEBO_INCLUDE_DIRS}
+    ${INTERFACES_CPP_DIR}
+    ${LIBS_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR})
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+
+add_library(camera_nao SHARED camera_dump.cc)
+target_link_libraries(camera_nao ${GAZEBO_libraries} 
+	CameraPlugin 
+	JderobotInterfaces
+	colorspacesmm	
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+)
+    
+add_library(poseneck SHARED poseneck.cc)
+target_link_libraries(poseneck ${GAZEBO_libraries} 	 	
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+)
+
+add_library(poserightshoulder SHARED poserightshoulder.cc)
+target_link_libraries(poserightshoulder ${GAZEBO_libraries} 
+    JderobotInterfaces	 	
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+)
+
+add_library(poseleftshoulder SHARED poseleftshoulder.cc)
+target_link_libraries(poseleftshoulder ${GAZEBO_libraries} 	 	
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+add_library(poserightelbow SHARED poserightelbow.cc)
+target_link_libraries(poserightelbow ${GAZEBO_libraries} 	 	
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+add_library(poseleftelbow SHARED poseleftelbow.cc)
+target_link_libraries(poseleftelbow ${GAZEBO_libraries} 	 	
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+ENDIF()

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,266 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "ballencoder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(BallGreen)
+
+    BallGreen::BallGreen () {
+        pthread_mutex_init(&mutex_ballgreenencoders, NULL);
+        pthread_mutex_init(&mutex_ballgreenmotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_ballgreen = std::string("--Ice.Config=ballgreen.cfg");
+
+        std::cout << "Constructor BallGreen" << std::endl;
+    }
+
+    void BallGreen::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        this->ballgreen.joint_pan = _model->GetJoint("jball_vertical");
+        this->ballgreen.joint_tilt = _model->GetJoint("wmp2");
+        
+        this->stiffness = 40.0;
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                        boost::bind(&BallGreen::OnUpdate, this));
+    }
+
+    void BallGreen::Init () {}
+
+    void BallGreen::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+        
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_BallGreenICE, (void*) this);
+            
+            this->ballgreen.motorsdata.pan = 0.0;
+            this->ballgreen.motorsdata.tilt = 0.0;
+        }
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the ball (PAN&TILT)
+        this->ballgreen.encoders.pan = - this->ballgreen.joint_pan->GetAngle(0).Radian();
+        this->ballgreen.encoders.tilt = - this->ballgreen.joint_tilt->GetAngle(0).Radian();
+
+        //          ----------MOTORS----------
+        if (this->ballgreen.motorsdata.pan > 0) {
+            if (this->ballgreen.encoders.pan < this->ballgreen.motorsdata.pan) {
+                this->ballgreen.joint_pan->SetVelocity(0, -0.3);
+                this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+            } else if (this->ballgreen.encoders.pan > this->ballgreen.motorsdata.pan) {
+                this->ballgreen.joint_pan->SetVelocity(0, 0.3);
+                this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        } else if (this->ballgreen.motorsdata.pan < 0) {
+            if (this->ballgreen.encoders.pan > this->ballgreen.motorsdata.pan) {
+                this->ballgreen.joint_pan->SetVelocity(0, 0.3);
+                this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+            } else if (this->ballgreen.encoders.pan < this->ballgreen.motorsdata.pan) {
+                this->ballgreen.joint_pan->SetVelocity(0, -0.3);
+                this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+
+        if (this->ballgreen.motorsdata.tilt > 0) {
+            if (this->ballgreen.encoders.tilt < this->ballgreen.motorsdata.tilt) {
+                this->ballgreen.joint_tilt->SetVelocity(0, -0.3);
+                this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else if (this->ballgreen.encoders.tilt > this->ballgreen.motorsdata.tilt) {
+                this->ballgreen.joint_tilt->SetVelocity(0, 0.3);
+                this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else if (this->ballgreen.motorsdata.tilt < 0) {
+            if (this->ballgreen.encoders.tilt > this->ballgreen.motorsdata.tilt) {
+                this->ballgreen.joint_tilt->SetVelocity(0, 0.3);
+                this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else if (this->ballgreen.encoders.tilt < this->ballgreen.motorsdata.tilt) {
+                this->ballgreen.joint_tilt->SetVelocity(0, -0.3);
+                this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseGreenBall* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_greenballencoders);
+            
+            pose3DEncodersData->x = pose->ballgreen.encoders.x;
+            pose3DEncodersData->y = pose->ballgreen.encoders.y;
+            pose3DEncodersData->z = pose->ballgreen.encoders.z;
+            pose3DEncodersData->pan = pose->ballgreen.encoders.pan;
+            pose3DEncodersData->tilt = pose->ballgreen.encoders.tilt;
+            pose3DEncodersData->roll = pose->ballgreen.encoders.roll;
+            pose3DEncodersData->clock = pose->ballgreen.encoders.clock;
+            pose3DEncodersData->maxPan = pose->ballgreen.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->ballgreen.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->ballgreen.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->ballgreen.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_greenballencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseNeck* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseGreenBall* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_greenballmotors);
+            
+            pose3DMotorsData->x = pose->ballgreen.motorsdata.x;
+            pose3DMotorsData->y = pose->ballgreen.motorsdata.y;
+            pose3DMotorsData->z = pose->ballgreen.motorsdata.z;
+            pose3DMotorsData->pan = pose->ballgreen.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->ballgreen.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->ballgreen.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->ballgreen.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->ballgreen.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_greenballmotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_greenballmotors);
+            
+            pose3DMotorsParams->maxPan = pose->ballgreen.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->ballgreen.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->ballgreen.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->ballgreen.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->ballgreen.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->ballgreen.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_greenballmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_greenballmotors);
+            
+            pose->ballgreen.motorsdata.x = data->x;
+            pose->ballgreen.motorsdata.y = data->y;
+            pose->ballgreen.motorsdata.z = data->z;
+            pose->ballgreen.motorsdata.pan = data->pan;
+            pose->ballgreen.motorsdata.tilt = data->tilt;
+            pose->ballgreen.motorsdata.roll = data->roll;
+            pose->ballgreen.motorsdata.panSpeed = data->panSpeed;
+            pose->ballgreen.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_greenballmotors);
+        }
+
+        gazebo::PoseGreenBall* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_BallGreenICE ( void* v ) {
+
+        gazebo::BallGreen* ballgreen = (gazebo::BallGreen*)v;
+        char* name = (char*) ballgreen->cfgfile_ballgreen.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("BallGreenEncoders.Endpoints");
+            std::cout << "BallGreenEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("BallGreenMotors.Endpoints");
+            std::cout << "BallGreenMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterBallGreenEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterBallGreenMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(ballgreen);
+            Ice::ObjectPtr motors = new Pose3DMotors(ballgreen);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("BallGreenEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("BallGreenMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (icB) {
+            try {
+                icB->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+BallGreenEncoders.Endpoints=default -h localhost -p 9294
+BallGreenMotors.Endpoints=default -h localhost -p 9295

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballgreen.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef BALLGREEN_H
+#define BALLGREEN_H
+
+namespace gazebo {
+
+    void* thread_BallGreenICE ( void* v );
+
+    class BallGreen : public ModelPlugin {
+    public:
+
+        BallGreen ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_ballgreen;
+        pthread_mutex_t mutex_ballgreenencoders;
+        pthread_mutex_t mutex_ballgreenmotors;
+
+        struct ballgreen_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        ballgreen_t ballgreen;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // BALLGREEN_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,266 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "ballencoder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(BallRed)
+
+    BallRed::BallRed () {
+        pthread_mutex_init(&mutex_ballredencoders, NULL);
+        pthread_mutex_init(&mutex_ballredmotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_ballred = std::string("--Ice.Config=ballred.cfg");
+
+        std::cout << "Constructor BallRed" << std::endl;
+    }
+
+    void BallRed::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        this->ballred.joint_pan = _model->GetJoint("jball_vertical");
+        this->ballred.joint_tilt = _model->GetJoint("wmp2");
+        
+        this->stiffness = 40.0;
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                        boost::bind(&BallRed::OnUpdate, this));
+    }
+
+    void BallRed::Init () {}
+
+    void BallRed::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+        
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_BallRedICE, (void*) this);
+            
+            this->ballred.motorsdata.pan = 0.0;
+            this->ballred.motorsdata.tilt = 0.0;
+        }
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the ball (PAN&TILT)
+        this->ballred.encoders.pan = - this->ballred.joint_pan->GetAngle(0).Radian();
+        this->ballred.encoders.tilt = - this->ballred.joint_tilt->GetAngle(0).Radian();
+
+        //          ----------MOTORS----------
+        if (this->ballred.motorsdata.pan > 0) {
+            if (this->ballred.encoders.pan < this->ballred.motorsdata.pan) {
+                this->ballred.joint_pan->SetVelocity(0, -0.3);
+                this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+            } else if (this->ballred.encoders.pan > this->ballred.motorsdata.pan) {
+                this->ballred.joint_pan->SetVelocity(0, 0.3);
+                this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        } else if (this->ballred.motorsdata.pan < 0) {
+            if (this->ballred.encoders.pan > this->ballred.motorsdata.pan) {
+                this->ballred.joint_pan->SetVelocity(0, 0.3);
+                this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+            } else if (this->ballred.encoders.pan < this->ballred.motorsdata.pan) {
+                this->ballred.joint_pan->SetVelocity(0, -0.3);
+                this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+
+        if (this->ballred.motorsdata.tilt > 0) {
+            if (this->ballred.encoders.tilt < this->ballred.motorsdata.tilt) {
+                this->ballred.joint_tilt->SetVelocity(0, -0.3);
+                this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else if (this->ballred.encoders.tilt > this->ballred.motorsdata.tilt) {
+                this->ballred.joint_tilt->SetVelocity(0, 0.3);
+                this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else if (this->ballred.motorsdata.tilt < 0) {
+            if (this->ballred.encoders.tilt > this->ballred.motorsdata.tilt) {
+                this->ballred.joint_tilt->SetVelocity(0, 0.3);
+                this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else if (this->ballred.encoders.tilt < this->ballred.motorsdata.tilt) {
+                this->ballred.joint_tilt->SetVelocity(0, -0.3);
+                this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseRedBall* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_redballencoders);
+            
+            pose3DEncodersData->x = pose->ballred.encoders.x;
+            pose3DEncodersData->y = pose->ballred.encoders.y;
+            pose3DEncodersData->z = pose->ballred.encoders.z;
+            pose3DEncodersData->pan = pose->ballred.encoders.pan;
+            pose3DEncodersData->tilt = pose->ballred.encoders.tilt;
+            pose3DEncodersData->roll = pose->ballred.encoders.roll;
+            pose3DEncodersData->clock = pose->ballred.encoders.clock;
+            pose3DEncodersData->maxPan = pose->ballred.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->ballred.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->ballred.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->ballred.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_redballencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseNeck* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseRedBall* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_redballmotors);
+            
+            pose3DMotorsData->x = pose->ballred.motorsdata.x;
+            pose3DMotorsData->y = pose->ballred.motorsdata.y;
+            pose3DMotorsData->z = pose->ballred.motorsdata.z;
+            pose3DMotorsData->pan = pose->ballred.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->ballred.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->ballred.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->ballred.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->ballred.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_redballmotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_redballmotors);
+            
+            pose3DMotorsParams->maxPan = pose->ballred.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->ballred.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->ballred.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->ballred.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->ballred.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->ballred.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_redballmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_redballmotors);
+            
+            pose->ballred.motorsdata.x = data->x;
+            pose->ballred.motorsdata.y = data->y;
+            pose->ballred.motorsdata.z = data->z;
+            pose->ballred.motorsdata.pan = data->pan;
+            pose->ballred.motorsdata.tilt = data->tilt;
+            pose->ballred.motorsdata.roll = data->roll;
+            pose->ballred.motorsdata.panSpeed = data->panSpeed;
+            pose->ballred.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_redballmotors);
+        }
+
+        gazebo::PoseRedBall* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_BallRedICE ( void* v ) {
+
+        gazebo::BallRed* ballred = (gazebo::BallRed*)v;
+        char* name = (char*) ballred->cfgfile_ballred.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("BallRedEncoders.Endpoints");
+            std::cout << "BallRedEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("BallRedMotors.Endpoints");
+            std::cout << "BallRedMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterBallRedEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterBallRedMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(ballred);
+            Ice::ObjectPtr motors = new Pose3DMotors(ballred);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("BallRedEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("BallRedMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (icB) {
+            try {
+                icB->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+BallRedEncoders.Endpoints=default -h localhost -p 9194
+BallRedMotors.Endpoints=default -h localhost -p 9195

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/ballred.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef BALLRED_H
+#define BALLRED_H
+
+namespace gazebo {
+
+    void* thread_BallRedICE ( void* v );
+
+    class BallRed : public ModelPlugin {
+    public:
+
+        BallRed ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_ballred;
+        pthread_mutex_t mutex_ballredencoders;
+        pthread_mutex_t mutex_ballredmotors;
+
+        struct ballred_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        ballred_t ballred;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // BALLRED_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/CMakeLists.txt	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+include (FindPkgConfig)
+if (PKG_CONFIG_FOUND)
+  pkg_check_modules(GAZEBO gazebo)
+  message(${GAZEBO_INCLUDE_DIRS})
+endif()
+
+SET( INTERFACES_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../interfaces/cpp) # Directorio con las interfaces ICE en C++
+SET( LIBS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../libs) # Directorio donde se encuentran las librerias propias de jderobot
+SET( SLICE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../interfaces/slice) # Directorio donde se encuentran las interfaces ICE
+SET( LIBS_NEEDED   jderobotice colorspaces jderobotutil) # Librerias de las que depende el componente
+SET( DEPS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../Deps) # Directorio donde se encuentran las dependencias
+
+include(FindPkgConfig)
+
+include(${DEPS_DIR}/gearbox/CMakeLists.txt)
+include(${DEPS_DIR}/ice/CMakeLists.txt)
+include(${DEPS_DIR}/opencv/CMakeLists.txt)
+
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/../../../.. ${CMAKE_CURRENT_SOURCE_DIR}/../../../..)
+add_subdirectory (${CMAKE_CURRENT_SOURCE_DIR}/.. ${CMAKE_CURRENT_SOURCE_DIR}/..)

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+make clean
+rm ../cmake_install.cmake
+rm -r CMakeFiles
+rm -r ../CMakeFiles
+mkdir ../temp
+mv CMakeLists.txt ../temp
+mv clean.sh ../temp
+mv cmake_uninstall.cmake.in ../temp
+rm -r *
+mv ../temp/CMakeLists.txt .
+mv ../temp/clean.sh .
+mv ../temp/cmake_uninstall.cmake.in .
+rm -r ../temp
+cd ../../../../
+./clean_repository 


Property changes on: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/build/clean.sh
___________________________________________________________________
Added: svn:executable
   + *

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_bottom.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h 0.0.0.0 -p 9991
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/cam_sensor_top.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,8 @@
+#without registry
+CameraGazebo.Endpoints=default -h 0.0.0.0 -p 9990
+
+#camera 1
+CameraGazebo.Camera.0.Name=cameraA
+CameraGazebo.Camera.0.ImageWidth=320
+CameraGazebo.Camera.0.ImageHeight=240
+CameraGazebo.Camera.0.Format=RGB8

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/camera_dump.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,280 @@
+#include "gazebo.hh"
+#include "plugins/CameraPlugin.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+#include <jderobot/camera.h>
+
+#include <visionlib/colorspaces/colorspacesmm.h>
+
+#include <iostream>
+
+#include <boost/algorithm/string.hpp>
+#include <boost/lexical_cast.hpp>
+
+void *myMain(void* v);
+
+namespace gazebo
+{   
+  class CameraDump : public CameraPlugin
+  { 
+    public: CameraDump() : CameraPlugin(),count(0) 
+	{
+		pthread_mutex_init (&mutex, NULL);
+		n = round(rand()*1000);
+	}
+
+    public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
+    {
+      // Don't forget to load the camera plugin
+      CameraPlugin::Load(_parent,_sdf);
+      std::cout << "Load: " <<n << " " <<  this->parentSensor->GetCamera()->GetName()<< std::endl;
+    } 
+
+    // Update the controller
+    public: void OnNewFrame(const unsigned char *_image, 
+        unsigned int _width, unsigned int _height, unsigned int _depth, 
+        const std::string &_format)
+    {
+      //std::cout << "OnNewFrame: " <<n << " " <<  this->parentSensor->GetCamera()->GetName()<< std::endl;
+		if(count==0){
+			std::vector<std::string> tokens;
+			nameCamera = this->parentSensor->GetCamera()->GetName();
+  			boost::split(tokens, nameCamera, boost::is_any_of("::"));
+  			boost::split(tokens, tokens[2], boost::is_any_of("("));
+			nameGlobal = tokens[0];
+			// El nombre del fichero de configuracion tiene que coincidir con el de la cámara en el .world y el .cfg 
+			nameCamera = std::string("--Ice.Config=" + tokens[0] + ".cfg"); 
+			
+			if (count == 0){
+				pthread_t thr_gui;
+				pthread_create(&thr_gui, NULL, &myMain, (void*)this);
+			}
+
+			image.create(_height, _width, CV_8UC3);
+			count++;
+		}
+		pthread_mutex_lock (&mutex);
+        memcpy((unsigned char *) image.data, &(_image[0]), _width*_height * 3);
+		
+		//cv::imshow(nameCamera, image);
+		//cv::waitKey(10);
+					
+		pthread_mutex_unlock (&mutex);
+		
+    }         
+
+    private: int count;
+    private: int n;
+    public: std::string nameCamera;
+    public: cv::Mat image;
+	public: pthread_mutex_t mutex;
+	public: std::string nameGlobal;
+  };
+
+  // Register this plugin with the simulator
+  GZ_REGISTER_SENSOR_PLUGIN(CameraDump)
+}
+
+class CameraI: virtual public jderobot::Camera {
+	public:
+		CameraI(std::string propertyPrefix, gazebo::CameraDump* camera)
+			   : prefix(propertyPrefix), cameraI(camera) {
+		
+			std::cout << "Constructor CameraI" << std::endl;
+
+			imageDescription = (new jderobot::ImageDescription());
+
+        	replyTask = new ReplyTask(this);
+		    replyTask->start(); // my own thread
+		  
+		}
+
+		std::string getName () {
+			return (cameraDescription->name);
+		}
+
+		std::string getRobotName () {
+			return "RobotName";
+		}
+
+		virtual ~CameraI() {
+
+
+		}
+
+		virtual jderobot::ImageDescriptionPtr getImageDescription(const Ice::Current& c){
+			return imageDescription;
+		}
+
+		virtual jderobot::CameraDescriptionPtr getCameraDescription(const Ice::Current& c){
+			return cameraDescription;
+		}
+
+		virtual Ice::Int setCameraDescription(const jderobot::CameraDescriptionPtr &description, const Ice::Current& c) {
+			return 0;
+		}
+
+		virtual void getImageData_async(const jderobot::AMD_ImageProvider_getImageDataPtr& cb,const Ice::Current& c){
+			replyTask->pushJob(cb);
+		}
+
+		virtual std::string startCameraStreaming(const Ice::Current&){
+
+		}
+
+		virtual void stopCameraStreaming(const Ice::Current&) {
+
+		}
+
+	private:
+		class ReplyTask: public IceUtil::Thread {
+			public:
+				ReplyTask(CameraI* camera){
+                    mycamera = camera;
+
+				   	std::cout << "safeThread" << std::endl;
+				}
+
+				void pushJob(const jderobot::AMD_ImageProvider_getImageDataPtr& cb){
+					IceUtil::Mutex::Lock sync(requestsMutex);
+					requests.push_back(cb);
+				}
+
+				virtual void run(){
+					jderobot::ImageDataPtr reply(new jderobot::ImageData);
+					struct timeval a, b;
+					int cycle = 48;
+					long totalb,totala;
+					long diff;
+					
+					int count =0 ;
+
+					while(1){
+						
+						if(!mycamera->cameraI->image.data){
+							usleep(100);
+							continue;
+						}
+						if(count==0){
+							pthread_mutex_lock (&mycamera->cameraI->mutex);
+							mycamera->imageDescription->width = mycamera->cameraI->image.cols;
+							mycamera->imageDescription->height = mycamera->cameraI->image.rows;
+							mycamera->imageDescription->size = mycamera->cameraI->image.cols*mycamera->cameraI->image.rows*3;
+							pthread_mutex_unlock (&mycamera->cameraI->mutex);
+
+							mycamera->imageDescription->format = "RGB8";
+
+							reply->description = mycamera->imageDescription;
+							count++;
+						}
+
+						//std::cout << nameGlobal<< std::endl;
+									
+						gettimeofday(&a,NULL);
+						totala=a.tv_sec*1000000+a.tv_usec;
+
+
+						IceUtil::Time t = IceUtil::Time::now();
+						reply->timeStamp.seconds = (long)t.toSeconds();
+						reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
+          				
+          				pthread_mutex_lock (&mycamera->cameraI->mutex);
+					    reply->pixelData.resize(mycamera->cameraI->image.rows*mycamera->cameraI->image.cols*3);
+					    
+					    memcpy( &(reply->pixelData[0]), (unsigned char *) mycamera->cameraI->image.data, mycamera->cameraI->image.rows*mycamera->cameraI->image.cols*3);
+						pthread_mutex_unlock (&mycamera->cameraI->mutex);
+
+					   { //critical region start
+						   IceUtil::Mutex::Lock sync(requestsMutex);
+						   while(!requests.empty()) {
+							   jderobot::AMD_ImageProvider_getImageDataPtr cb = requests.front();
+							   requests.pop_front();
+							   cb->ice_response(reply);
+						   }
+					   } //critical region end
+                  
+						gettimeofday(&b,NULL);
+						totalb=b.tv_sec*1000000+b.tv_usec;
+
+						diff = (totalb-totala)/1000;
+						diff = cycle-diff;
+
+						//std::cout << "Gazeboserver takes " << diff << " ms " << mycamera->fileName << std::endl;
+
+						if(diff < 33)
+							diff = 33;
+
+
+						/*Sleep Algorithm*/
+						usleep(diff*1000);
+					}
+				}
+
+				CameraI* mycamera;
+				IceUtil::Mutex requestsMutex;
+				std::list<jderobot::AMD_ImageProvider_getImageDataPtr> requests;
+		};
+
+		typedef IceUtil::Handle<ReplyTask> ReplyTaskPtr;
+		std::string prefix;
+
+		colorspaces::Image::FormatPtr imageFmt;
+		jderobot::ImageDescriptionPtr imageDescription;
+		jderobot::CameraDescriptionPtr cameraDescription;
+		ReplyTaskPtr replyTask;
+		gazebo::CameraDump* cameraI;	
+		
+}; // end class CameraI
+
+
+void *myMain(void* v)
+{
+
+	gazebo::CameraDump* camera = (gazebo::CameraDump*)v;
+
+	char* name = (char*)camera->nameCamera.c_str();
+
+    Ice::CommunicatorPtr ic;
+    int argc = 1;
+
+    Ice::PropertiesPtr prop;
+	char* argv[] = {name};
+
+    try {
+        
+        ic = Ice::initialize(argc, argv);
+        prop = ic->getProperties();
+        
+        std::string Endpoints = prop->getProperty("CameraGazebo.Endpoints");
+        std::cout << "CameraGazebo "<< camera->nameGlobal <<" Endpoints > "  << Endpoints << std::endl;
+        
+        Ice::ObjectAdapterPtr adapter =
+        ic->createObjectAdapterWithEndpoints("CameraGazebo", Endpoints);
+		
+        Ice::ObjectPtr object = new CameraI(std::string("CameraGazebo"),  camera);
+        adapter->add(object, ic->stringToIdentity(camera->nameGlobal));
+        adapter->activate();
+        ic->waitForShutdown();
+    } catch (const Ice::Exception& e) {
+        std::cerr << e << std::endl;
+    } catch (const char* msg) {
+        std::cerr << msg << std::endl;
+    }
+    if (ic) {
+        try {
+            ic->destroy();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        }
+    }
+    
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/common.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,58 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#ifndef NAOCOMMON_H
+#define NAOCOMMON_H
+
+struct encoders_t {
+    float x;
+    float y;
+    float z;
+    float pan;
+    float tilt;
+    float roll;
+    int clock;
+    float maxPan;
+    float maxTilt;
+    float minPan;
+    float minTilt;
+};
+
+struct motorsdata_t {
+    float x;
+    float y;
+    float z;
+    float pan;
+    float tilt;
+    float roll;
+    float panSpeed;
+    float tiltSpeed;
+};
+
+struct motorsparams_t {
+    float maxPan;
+    float minPan;
+    float maxTilt;
+    float minTilt;
+    float maxPanSpeed;
+    float maxTiltSpeed;
+};
+
+#endif // NAOCOMMON_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/naoworld.world	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,32 @@
+<?xml version="1.0" ?>
+<sdf version="1.3">
+  <world name="default">
+    <!-- Ground -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- My robots -->
+    <include>
+      <uri>model://nao_robot</uri>
+      <pose>0.08 0 0 0 0 3.14</pose>
+    </include>
+
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    
+    <!-- A ball -->
+<!--    <include>
+        <uri>model://pelota</uri>
+        <pose>0 -2 0 0 0 0</pose>
+    </include>
+
+    <include>
+	<uri>model://pelota_verde</uri>
+	<pose>-0.5 -2.3 0 0 0 0</pose>
+    </include>
+-->
+  </world>
+</sdf>

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseleftankle.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(Pose3DEncodersLeftAnkle)
+
+    PoseLeftAnkle::PoseLeftAnkle () {
+        pthread_mutex_init(&mutex_lefankleencoders, NULL);
+        pthread_mutex_init(&mutex_lefanklemotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_leftankle = std::string("--Ice.Config=poseleftankle.cfg");
+        
+        this->leftankle.motorsparams.maxPan = 1.57;
+        this->leftankle.motorsparams.minPan = -1.57;          
+        this->leftankle.motorsparams.maxTilt = 0.5;
+        this->leftankle.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseLeftAnkle" << std::endl;
+    }
+
+    void PoseLeftAnkle::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersleftankle_pitch"))
+            gzerr << "pose3dencodersleftankle plugin missing <joint_pose3dencodersleftankle_pitch> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersleftankle_roll"))
+            gzerr << "pose3dencodersleftankle plugin missing <joint_pose3dencodersleftankle_roll> element\n";
+
+        this->leftankle.joint_roll = this->model->GetJoint("lankle_roll");
+        this->leftankle.joint_tilt = this->model->GetJoint("lankle_pitch");
+
+        if (!this->leftankle.joint_roll)
+            gzerr << "Unable to find joint_pose3dencodersleftankle_roll["
+                << _sdf->GetElement("joint_pose3dencodersleftankle_roll")->GetValueString() << "]\n";
+        if (!this->leftankle.joint_tilt)
+            gzerr << "Unable to find joint_pose3dencodersleftankle_pitch["
+                << _sdf->GetElement("joint_pose3dencodersleftankle_pitch")->GetValueString() << "]\n"; 
+                
+        this->leftankle.link_roll = this->model->GetLink("left_foot");
+        this->leftankle.link_tilt = this->model->GetLink("leftankle_pitch");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseLeftAnkle::OnUpdate, this));
+    }
+
+    void PoseLeftAnkle::Init () {}
+
+    void PoseLeftAnkle::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (count == 0) {
+            count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_LeftAnkleICE, (void*) this);
+            
+            this->leftankle.encoders.roll = 0;    
+            this->leftankle.encoders.tilt = 0;
+        } else {
+            //          ----------ENCODERS----------
+            //GET pose3dencoders data from the left ankle (PAN&TILT)
+            this->leftankle.encoders.roll = this->leftankle.link_roll->GetRelativePose().rot.GetAsEuler().y;    
+            this->leftankle.encoders.tilt = this->leftankle.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+        }
+
+        //          ----------MOTORS----------
+        if (this->leftankle.motorsdata.roll >= 0) {
+            if (this->leftankle.encoders.roll < this->leftankle.motorsdata.roll) {
+                this->leftankle.joint_roll->SetVelocity(0, -0.1);
+                this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->leftankle.joint_roll->SetVelocity(0, 0.1);
+                this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->leftankle.encoders.roll > this->leftankle.motorsdata.roll) {
+                this->leftankle.joint_roll->SetVelocity(0, 0.1);
+                this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->leftankle.joint_roll->SetVelocity(0, -0.1);
+                this->leftankle.joint_roll->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->leftankle.motorsdata.tilt >= 0) {
+            if (this->leftankle.encoders.tilt < this->leftankle.motorsdata.tilt) {
+                this->leftankle.joint_tilt->SetVelocity(0, -0.1);
+                this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->leftankle.joint_tilt->SetVelocity(0, 0.1);
+                this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->leftankle.encoders.tilt > this->leftankle.motorsdata.tilt) {
+                this->leftankle.joint_tilt->SetVelocity(0, 0.1);
+                this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->leftankle.joint_tilt->SetVelocity(0, -0.1);
+                this->leftankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseLeftAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftankleencoders);
+            
+            pose3DEncodersData->x = pose->leftankle.encoders.x;
+            pose3DEncodersData->y = pose->leftankle.encoders.y;
+            pose3DEncodersData->z = pose->leftankle.encoders.z;
+            pose3DEncodersData->pan = pose->leftankle.encoders.pan;
+            pose3DEncodersData->tilt = pose->leftankle.encoders.tilt;
+            pose3DEncodersData->roll = pose->leftankle.encoders.roll;
+            pose3DEncodersData->clock = pose->leftankle.encoders.clock;
+            pose3DEncodersData->maxPan = pose->leftankle.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->leftankle.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->leftankle.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->leftankle.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_leftankleencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseLeftAnkle* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseLeftAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftanklemotors);
+            
+            pose3DMotorsData->x = pose->leftankle.motorsdata.x;
+            pose3DMotorsData->y = pose->leftankle.motorsdata.y;
+            pose3DMotorsData->z = pose->leftankle.motorsdata.z;
+            pose3DMotorsData->pan = pose->leftankle.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->leftankle.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->leftankle.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->leftankle.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->leftankle.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftanklemotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftanklemotors);
+            
+            pose3DMotorsParams->maxPan = pose->leftankle.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->leftankle.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->leftankle.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->leftankle.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->leftankle.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->leftankle.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftanklemotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftanklemotors);
+            
+            pose->leftankle.motorsdata.x = data->x;
+            pose->leftankle.motorsdata.y = data->y;
+            pose->leftankle.motorsdata.z = data->z;
+            pose->leftankle.motorsdata.pan = data->pan;
+            pose->leftankle.motorsdata.tilt = data->tilt;
+            pose->leftankle.motorsdata.roll = data->roll;
+            pose->leftankle.motorsdata.panSpeed = data->panSpeed;
+            pose->leftankle.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftanklemotors);
+        }
+
+        gazebo::PoseLeftAnkle* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_LeftAnkleICE ( void* v ) {
+
+        gazebo::PoseLeftAnkle* leftankle = (gazebo::PoseLeftAnkle*)v;
+        char* name = (char*) leftankle->cfgfile_leftankle.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseLeftAnkleEncoders.Endpoints");
+            std::cout << "PoseLeftAnkleEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseLeftAnkleMotors.Endpoints");
+            std::cout << "PoseLeftAnkleMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftAnkleEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftAnkleMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(leftankle);
+            Ice::ObjectPtr motors = new Pose3DMotors(leftankle);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("LeftAnkleEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("LeftAnkleMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftAnkleEncoders.Endpoints=default -h localhost -p 11996
+PoseLeftAnkleMotors.Endpoints=default -h localhost -p 11997

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftankle.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSELEFTANKLE_H
+#define POSELEFTANKLE_H
+
+namespace gazebo {
+
+    void* thread_LeftAnkleICE ( void* v );
+
+    class PoseLeftAnkle : public ModelPlugin {
+    public:
+
+        PoseLeftAnkle ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_lefankle;
+        pthread_mutex_t mutex_lefankleencoders;
+        pthread_mutex_t mutex_lefanklemotors;
+
+        struct lefankle_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        lefankle_t lefankle;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSELEFTANKLE_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,296 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseleftelbow.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+
+    GZ_REGISTER_MODEL_PLUGIN(PoseLeftElbow)
+
+    PoseLeftElbow::PoseLeftElbow () {
+        pthread_mutex_init(&this->mutex_leftelbowencoders, NULL);
+        pthread_mutex_init(&this->mutex_leftelbowmotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_leftelbow = std::string("--Ice.Config=poseleftelbow.cfg");
+        
+        this->leftelbow.motorsparams.maxPan = 1.57;
+        this->leftelbow.motorsparams.minPan = -1.57;          
+        this->leftelbow.motorsparams.maxTilt = 0.5;
+        this->leftelbow.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseLeftElbow" << std::endl;
+    }
+
+    void PoseLeftElbow::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersleftelbow_yaw"))
+            gzerr << "pose3dencodersleftelbow plugin missing <joint_pose3dencodersleftelbow_yaw> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersleftelbow_roll"))
+            gzerr << "pose3dencodersleftelbow plugin missing <joint_pose3dencodersleftelbow_roll> element\n";
+
+        this->leftelbow.joint_pan = _model->GetJoint("lelbow_yaw");
+        this->leftelbow.joint_roll = _model->GetJoint("lelbow_roll");
+
+        if (!this->leftelbow.joint_pan)
+            gzerr << "Unable to find joint_pose3dencodersleftelbow_yaw["
+                << _sdf->GetElement("joint_pose3dencodersleftelbow_yaw")->GetValueString() << "]\n"; 
+        if (!this->leftelbow.joint_roll)
+            gzerr << "Unable to find joint_pose3dencodersleftelbow_roll["
+                << _sdf->GetElement("joint_pose3dencodersleftelbow_roll")->GetValueString() << "]\n";
+                
+        this->leftelbow.link_pan = _model->GetLink("leftelbow_yaw");
+        this->leftelbow.link_roll = _model->GetLink("left_lower_arm");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseLeftElbow::OnUpdate, this));
+    }
+
+    void PoseLeftElbow::Init () {}
+
+    void PoseLeftElbow::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_LeftElbowICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the left elbow (PAN&TILT)
+//        this->leftelbow.encoder.pan = this->leftelbow.leftelbow_link_pan->GetRelativePose().rot.GetAsEuler().z;    
+//        this->leftelbow.encoder.roll = this->leftelbow.leftelbow_link_roll->GetRelativePose().rot.GetAsEuler().y;
+        
+        this->leftelbow.encoders.pan = - this->leftelbow.joint_pan->GetAngle(0).Radian();
+        this->leftelbow.encoders.roll = - this->leftelbow.joint_roll->GetAngle(0).Radian();
+
+        //          ----------MOTORS----------
+        if (this->leftelbow.motorsdata.pan >= 0) {
+            if (this->leftelbow.encoders.pan < this->leftelbow.motorsdata.pan) {
+                this->leftelbow.joint_pan->SetVelocity(0, -0.1);
+                this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->leftelbow.joint_pan->SetVelocity(0, 0.1);
+                this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->leftelbow.encoders.pan > this->leftelbow.motorsdata.pan) {
+                this->leftelbow.joint_pan->SetVelocity(0, 0.1);
+                this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->leftelbow.joint_pan->SetVelocity(0, -0.1);
+                this->leftelbow.joint_pan->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->leftelbow.motorsdata.roll >= 0) {
+            if (this->leftelbow.encoders.roll < this->leftelbow.motorsdata.roll) {
+                this->leftelbow.joint_roll->SetVelocity(0, -0.1);
+                this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            } else {
+                this->leftelbow.joint_roll->SetVelocity(0, 0.1);
+                this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->leftelbow.encoders.roll > this->leftelbow.motorsdata.roll) {
+                this->leftelbow.joint_roll->SetVelocity(0, 0.1);
+                this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            } else {
+                this->leftelbow.joint_roll->SetVelocity(0, -0.1);
+                this->leftelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseLeftElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftelbowencoders);
+            
+            pose3DEncodersData->x = pose->leftelbow.encoders.x;
+            pose3DEncodersData->y = pose->leftelbow.encoders.y;
+            pose3DEncodersData->z = pose->leftelbow.encoders.z;
+            pose3DEncodersData->pan = pose->leftelbow.encoders.pan;
+            pose3DEncodersData->tilt = pose->leftelbow.encoders.tilt;
+            pose3DEncodersData->roll = pose->leftelbow.encoders.roll;
+            pose3DEncodersData->clock = pose->leftelbow.encoders.clock;
+            pose3DEncodersData->maxPan = pose->leftelbow.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->leftelbow.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->leftelbow.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->leftelbow.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_leftelbowencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseLeftElbow* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseLeftElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftelbowmotors);
+            
+            pose3DMotorsData->x = pose->leftelbow.motorsdata.x;
+            pose3DMotorsData->y = pose->leftelbow.motorsdata.y;
+            pose3DMotorsData->z = pose->leftelbow.motorsdata.z;
+            pose3DMotorsData->pan = pose->leftelbow.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->leftelbow.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->leftelbow.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->leftelbow.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->leftelbow.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftelbowmotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftelbowmotors);
+            
+            pose3DMotorsParams->maxPan = pose->leftelbow.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->leftelbow.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->leftelbow.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->leftelbow.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->leftelbow.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->leftelbow.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftelbowmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftelbowmotors);
+            
+            pose->leftelbow.motorsdata.x = data->x;
+            pose->leftelbow.motorsdata.y = data->y;
+            pose->leftelbow.motorsdata.z = data->z;
+            pose->leftelbow.motorsdata.pan = data->pan;
+            pose->leftelbow.motorsdata.tilt = data->tilt;
+            pose->leftelbow.motorsdata.roll = data->roll;
+            pose->leftelbow.motorsdata.panSpeed = data->panSpeed;
+            pose->leftelbow.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftelbowmotors);
+        }
+
+        gazebo::PoseLeftElbow* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_LeftElbowICE ( void* v ) {
+
+        gazebo::PoseLeftElbow* leftelbow = (gazebo::PoseLeftElbow*)v;
+        char* name = (char*) leftelbow->cfgfile_leftelbow.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseLeftElbowEncoders.Endpoints");
+            std::cout << "PoseNeckEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseLeftElbowMotors.Endpoints");
+            std::cout << "PoseNeckMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftElbowEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftElbowMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(leftelbow);
+            Ice::ObjectPtr motors = new Pose3DMotors(leftelbow);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("LeftElbowEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("LeftElbowMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftElbowEncoders.Endpoints=default -h localhost -p 5998
+PoseLeftElbowMotors.Endpoints=default -h localhost -p 5999

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftelbow.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSELEFTELBOW_H
+#define POSELEFTELBOW_H
+
+namespace gazebo {
+
+    void* thread_LeftElbowICE ( void* v );
+
+    class PoseLeftElbow : public ModelPlugin {
+    public:
+
+        PoseLeftElbow ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_leftelbow;
+        pthread_mutex_t mutex_leftelbowencoders;
+        pthread_mutex_t mutex_leftelbowmotors;
+
+        struct leftelbow_t {
+            physics::JointPtr joint_pan, joint_roll;
+            physics::LinkPtr link_pan, link_roll;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        leftelbow_t leftelbow;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSELEFTELBOW_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseleftshoulder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseLeftShoulder)
+
+    PoseLeftShoulder::PoseLeftShoulder () {
+        pthread_mutex_init(&this->mutex_leftshoulderencoders, NULL);
+        pthread_mutex_init(&this->mutex_leftshouldermotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_leftshoulder = std::string("--Ice.Config=poseleftshoulder.cfg");
+        
+        this->leftshoulder.motorsparams.maxPan = 1.57;
+        this->leftshoulder.motorsparams.minPan = -1.57;          
+        this->leftshoulder.motorsparams.maxTilt = 0.5;
+        this->leftshoulder.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseLeftShoulder" << std::endl;
+    }
+
+    void PoseLeftShoulder::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersleftshoulder_pitch"))
+            gzerr << "pose3dencodersleftshoulder plugin missing <joint_pose3dencodersleftshoulder_pitch> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersleftshoulder_roll"))
+            gzerr << "pose3dencodersleftshoulder plugin missing <joint_pose3dencodersleftshoulder_roll> element\n";
+
+        this->leftshoulder.joint_roll = _model->GetJoint("lshoulder_roll");
+        this->leftshoulder.joint_tilt = _model->GetJoint("lshoulder_pitch");
+
+        if (!this->leftshoulder.joint_roll)
+            gzerr << "Unable to find joint_pose3dencodersleftshoulder_roll["
+                << _sdf->GetElement("joint_pose3dencodersleftshoulder_roll")->GetValueString() << "]\n";
+        if (!this->leftshoulder.joint_tilt)
+            gzerr << "Unable to find joint_pose3dencodersleftshoulder_pitch["
+                << _sdf->GetElement("joint_pose3dencodersleftshoulder_pitch")->GetValueString() << "]\n"; 
+                
+        this->leftshoulder.link_roll = _model->GetLink("left_upper_arm");
+        this->leftshoulder.link_tilt = _model->GetLink("leftshoulder_pitch");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for left shoulder.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseLeftShoulder::OnUpdate, this));
+    }
+
+    void PoseLeftShoulder::Init () {}
+
+    void PoseLeftShoulder::OnUpdate() {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_gui;
+            pthread_create(&thr_gui, NULL, &thread_LeftShoulderICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the left shoulder (PAN&TILT)
+//        this->leftshoulder.encoder.roll = this->leftshoulder.leftshoulder_link_roll->GetRelativePose().rot.GetAsEuler().y;    
+//        this->leftshoulder.encoder.tilt = this->leftshoulder.leftshoulder_link_tilt->GetRelativePose().rot.GetAsEuler().x;
+        
+        this->leftshoulder.encoders.tilt = - this->leftshoulder.joint_tilt->GetAngle(0).Radian();
+        this->leftshoulder.encoders.roll = - this->leftshoulder.joint_roll->GetAngle(0).Radian();
+
+        //          ----------MOTORS----------
+        if (this->leftshoulder.motorsdata.roll >= 0) {
+            if (this->leftshoulder.encoders.roll < this->leftshoulder.motorsdata.roll) {
+                this->leftshoulder.joint_roll->SetVelocity(0, -0.1);
+                this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->leftshoulder.joint_roll->SetVelocity(0, 0.1);
+                this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->leftshoulder.encoders.roll > this->leftshoulder.motorsdata.roll) {
+                this->leftshoulder.joint_roll->SetVelocity(0, 0.1);
+                this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->leftshoulder.joint_roll->SetVelocity(0, -0.1);
+                this->leftshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->leftshoulder.motorsdata.tilt >= 0) {
+            if (this->leftshoulder.encoders.tilt < this->leftshoulder.motorsdata.tilt) {
+                this->leftshoulder.joint_tilt->SetVelocity(0, -0.1);
+                this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->leftshoulder.joint_tilt->SetVelocity(0, 0.1);
+                this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->leftshoulder.encoders.tilt > this->leftshoulder.motorsdata.tilt) {
+                this->leftshoulder.joint_tilt->SetVelocity(0, 0.1);
+                this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->leftshoulder.joint_tilt->SetVelocity(0, -0.1);
+                this->leftshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseLeftShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftshoulderencoders);
+            
+            pose3DEncodersData->x = pose->leftshoulder.encoders.x;
+            pose3DEncodersData->y = pose->leftshoulder.encoders.y;
+            pose3DEncodersData->z = pose->leftshoulder.encoders.z;
+            pose3DEncodersData->pan = pose->leftshoulder.encoders.pan;
+            pose3DEncodersData->tilt = pose->leftshoulder.encoders.tilt;
+            pose3DEncodersData->roll = pose->leftshoulder.encoders.roll;
+            pose3DEncodersData->clock = pose->leftshoulder.encoders.clock;
+            pose3DEncodersData->maxPan = pose->leftshoulder.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->leftshoulder.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->leftshoulder.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->leftshoulder.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_leftshoulderencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseLeftShoulder* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseLeftShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftshouldermotors);
+            
+            pose3DMotorsData->x = pose->leftshoulder.motorsdata.x;
+            pose3DMotorsData->y = pose->leftshoulder.motorsdata.y;
+            pose3DMotorsData->z = pose->leftshoulder.motorsdata.z;
+            pose3DMotorsData->pan = pose->leftshoulder.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->leftshoulder.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->leftshoulder.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->leftshoulder.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->leftshoulder.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftshouldermotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftshouldermotors);
+            
+            pose3DMotorsParams->maxPan = pose->leftshoulder.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->leftshoulder.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->leftshoulder.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->leftshoulder.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->leftshoulder.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->leftshoulder.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftshouldermotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftshouldermotors);
+            
+            pose->leftshoulder.motorsdata.x = data->x;
+            pose->leftshoulder.motorsdata.y = data->y;
+            pose->leftshoulder.motorsdata.z = data->z;
+            pose->leftshoulder.motorsdata.pan = data->pan;
+            pose->leftshoulder.motorsdata.tilt = data->tilt;
+            pose->leftshoulder.motorsdata.roll = data->roll;
+            pose->leftshoulder.motorsdata.panSpeed = data->panSpeed;
+            pose->leftshoulder.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftshouldermotors);
+        }
+
+        gazebo::PoseLeftShoulder* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_LeftShoulderICE ( void* v ) {
+
+        gazebo::PoseLeftShoulder* leftshoulder = (gazebo::PoseLeftShoulder*)v;
+        char* name = (char*) leftshoulder->cfgfile_leftshoulder.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseLeftShoulderEncoders.Endpoints");
+            std::cout << "PoseLeftShoulderEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseLeftShoulderMotors.Endpoints");
+            std::cout << "PoseLeftShoulderMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftShoulderEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftShoulderMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(leftshoulder);
+            Ice::ObjectPtr motors = new Pose3DMotors(leftshoulder);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("LeftShoulderEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("LeftShoulderMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftShoulderEncoders.Endpoints=default -h localhost -p 7996
+PoseLeftShoulderMotors.Endpoints=default -h localhost -p 7997

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseleftshoulder.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef LEFTSHOULDER_H
+#define LEFTSHOULDER_H
+
+namespace gazebo {
+
+    void* thread_LeftShoulderICE ( void* v );
+
+    class PoseLeftShoulder : public ModelPlugin {
+    public:
+
+        PoseLeftShoulder ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_leftshoulder;
+        pthread_mutex_t mutex_leftshoulderencoders;
+        pthread_mutex_t mutex_leftshouldermotors;
+
+        struct leftshoulder_t {
+            physics::JointPtr joint_tilt, joint_roll;
+            physics::LinkPtr link_tilt, link_roll;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        leftshoulder_t leftshoulder;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // LEFTSHOULDER_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseneck.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseNeck)
+
+    PoseNeck::PoseNeck () {
+        pthread_mutex_init(&this->mutex_neckencoders, NULL);
+        pthread_mutex_init(&this->mutex_neckmotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_neck = std::string("--Ice.Config=poseneck.cfg");
+        
+        this->neck.motorsparams.maxPan = 1.57;
+        this->neck.motorsparams.minPan = -1.57;          
+        this->neck.motorsparams.maxTilt = 0.5;
+        this->neck.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseNeck" << std::endl;
+    }
+
+    void PoseNeck::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("bottom_joint_pose3dencodersneck_pan"))
+            gzerr << "pose3dencodersneck plugin missing <bottom_joint_pose3dencodersneck_pan> element\n";
+        if (!_sdf->HasElement("top_joint_pose3dencodersneck_tilt"))
+            gzerr << "pose3dencodersneck plugin missing <top_joint_pose3dencodersneck_tilt> element\n";
+
+        this->neck.joint_pan = _model->GetJoint("neck_pan");
+        this->neck.joint_tilt = _model->GetJoint("neck_tilt");
+
+        if (!this->neck.joint_pan)
+            gzerr << "Unable to find bottom_joint_pose3dencodersneck_pan["
+                << _sdf->GetElement("bottom_joint_pose3dencodersneck_pan")->GetValueString() << "]\n"; 
+        if (!this->neck.joint_tilt)
+            gzerr << "Unable to find top_joint_pose3dencodersneck_tilt["
+                << _sdf->GetElement("top_joint_pose3dencodersneck_tilt")->GetValueString() << "]\n";
+                
+        this->neck.link_pan = _model->GetLink("head_pan");
+        this->neck.link_tilt = _model->GetLink("head_tilt");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+            boost::bind(&PoseNeck::OnUpdate, this));
+
+    }
+
+    void PoseNeck::Init () {}
+
+    void PoseNeck::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_NeckICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the neck (PAN&TILT)
+        this->neck.encoders.pan = this->neck.link_pan->GetRelativePose().rot.GetAsEuler().z;    
+        this->neck.encoders.tilt = this->neck.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+        
+        //          ----------MOTORS----------
+        if (this->neck.motorsdata.pan >= 0) {
+            if (this->neck.encoders.pan < this->neck.motorsdata.pan) {
+                this->neck.joint_pan->SetVelocity(0, -0.1);
+                this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->neck.joint_pan->SetVelocity(0, 0.1);
+                this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->neck.encoders.pan > this->neck.motorsdata.pan) {
+                this->neck.joint_pan->SetVelocity(0, 0.1);
+                this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->neck.joint_pan->SetVelocity(0, -0.1);
+                this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->neck.motorsdata.tilt >= 0) {
+            if (this->neck.encoders.tilt < this->neck.motorsdata.tilt) {
+                this->neck.joint_tilt->SetVelocity(0, -0.1);
+                this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->neck.joint_tilt->SetVelocity(0, 0.1);
+                this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->neck.encoders.tilt > this->neck.motorsdata.tilt) {
+                this->neck.joint_tilt->SetVelocity(0, 0.1);
+                this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->neck.joint_tilt->SetVelocity(0, -0.1);
+                this->neck.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckencoders);
+            
+            pose3DEncodersData->x = pose->neck.encoders.x;
+            pose3DEncodersData->y = pose->neck.encoders.y;
+            pose3DEncodersData->z = pose->neck.encoders.z;
+            pose3DEncodersData->pan = pose->neck.encoders.pan;
+            pose3DEncodersData->tilt = pose->neck.encoders.tilt;
+            pose3DEncodersData->roll = pose->neck.encoders.roll;
+            pose3DEncodersData->clock = pose->neck.encoders.clock;
+            pose3DEncodersData->maxPan = pose->neck.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->neck.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->neck.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->neck.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_neckencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseNeck* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckmotors);
+            
+            pose3DMotorsData->x = pose->neck.motorsdata.x;
+            pose3DMotorsData->y = pose->neck.motorsdata.y;
+            pose3DMotorsData->z = pose->neck.motorsdata.z;
+            pose3DMotorsData->pan = pose->neck.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->neck.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->neck.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->neck.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->neck.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_neckmotors);
+
+            return pose3DMotorsData;
+
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckmotors);
+            
+            pose3DMotorsParams->maxPan = pose->neck.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->neck.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->neck.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->neck.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->neck.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->neck.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_neckmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckmotors);
+            
+            pose->neck.motorsdata.x = data->x;
+            pose->neck.motorsdata.y = data->y;
+            pose->neck.motorsdata.z = data->z;
+            pose->neck.motorsdata.pan = data->pan;
+            pose->neck.motorsdata.tilt = data->tilt;
+            pose->neck.motorsdata.roll = data->roll;
+            pose->neck.motorsdata.panSpeed = data->panSpeed;
+            pose->neck.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_neckmotors);
+
+        }
+
+        gazebo::PoseNeck* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_NeckICE ( void* v ) {
+
+        gazebo::PoseNeck* neck = (gazebo::PoseNeck*)v;
+        char* name = (char*) neck->cfgfile_neck.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseNeckEncoders.Endpoints");
+            std::cout << "PoseNeckEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseNeckMotors.Endpoints");
+            std::cout << "PoseNeckMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterNeckEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterNeckMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(neck);
+            Ice::ObjectPtr motors = new Pose3DMotors(neck);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("NeckEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("NeckMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseNeckMotors.Endpoints=default -h localhost -p 9994
+PoseNeckEncoders.Endpoints=default -h localhost -p 9995

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneck.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSENECK_H
+#define POSENECK_H
+
+namespace gazebo {
+
+    void* thread_NeckICE ( void* v );
+
+    class PoseNeck : public ModelPlugin {
+    public:
+
+        PoseNeck ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_neck;
+        pthread_mutex_t mutex_neckencoders;
+        pthread_mutex_t mutex_neckmotors;
+
+        struct neck_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        neck_t neck;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSENECK_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,259 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poseneckspeed.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseNeckSpeed)
+
+    PoseNeckSpeed::PoseNeckSpeed () {
+        pthread_mutex_init(&this->mutex_neckspeedencoders, NULL);
+        pthread_mutex_init(&this->mutex_neckspeedmotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_neckspeed = std::string("--Ice.Config=poseneckspeed.cfg");
+        
+        this->neckspeed.motorsparams.maxPan = 1.57;
+        this->neckspeed.motorsparams.minPan = -1.57;          
+        this->neckspeed.motorsparams.maxTilt = 0.5;
+        this->neckspeed.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseNeckSpeed" << std::endl;
+    }
+
+    void PoseNeckSpeed::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("bottom_joint_pose3dencodersneckvel_pan"))
+            gzerr << "pose3dencodersneck plugin missing <bottom_joint_pose3dencodersneckvel_pan> element\n";
+        if (!_sdf->HasElement("top_joint_pose3dencodersneckvel_tilt"))
+            gzerr << "pose3dencodersneck plugin missing <top_joint_pose3dencodersneckvel_tilt> element\n";
+
+        this->neckspeed.joint_pan = _model->GetJoint("neck_pan");
+        this->neckspeed.joint_tilt = _model->GetJoint("neck_tilt");
+
+        if (!this->neckspeed.joint_pan)
+            gzerr << "Unable to find bottom_joint_pose3dencodersneckvel_pan["
+                << _sdf->GetElement("bottom_joint_pose3dencodersneckvel_pan")->GetValueString() << "]\n"; 
+        if (!this->neckspeed.joint_tilt)
+            gzerr << "Unable to find top_joint_pose3dencodersneckvel_tilt["
+                << _sdf->GetElement("top_joint_pose3dencodersneckvel_tilt")->GetValueString() << "]\n";
+                
+        this->neckspeed.link_pan = _model->GetLink("head_pan");
+        this->neckspeed.link_tilt = _model->GetLink("head_tilt");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the neck speed.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseNeckSpeed::OnUpdate, this));
+    }
+
+    void PoseNeckSpeed::Init () {}
+
+    void PoseNeckSpeed::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_NeckSpeedICE, (void*) this);
+        }
+  
+        count++;
+        if ((count % 2) == 0){      
+            this->neckspeed.joint_pan->SetVelocity(0, this->neckspeed.motorsdata.panSpeed);
+            this->neckspeed.joint_pan->SetMaxForce(0, this->stiffness);
+        } else {
+            this->neckspeed.joint_tilt->SetVelocity(0, this->neckspeed.motorsdata.panSpeed);
+            this->neckspeed.joint_tilt->SetMaxForce(0, this->stiffness);
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckspeedencoders);
+            
+            pose3DEncodersData->x = pose->neckspeed.encoders.x;
+            pose3DEncodersData->y = pose->neckspeed.encoders.y;
+            pose3DEncodersData->z = pose->neckspeed.encoders.z;
+            pose3DEncodersData->pan = pose->neckspeed.encoders.pan;
+            pose3DEncodersData->tilt = pose->neckspeed.encoders.tilt;
+            pose3DEncodersData->roll = pose->neckspeed.encoders.roll;
+            pose3DEncodersData->clock = pose->neckspeed.encoders.clock;
+            pose3DEncodersData->maxPan = pose->neckspeed.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->neckspeed.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->neckspeed.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->neckspeed.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_neckspeedencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseNeck* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckspeedmotors);
+            
+            pose3DMotorsData->x = pose->neckspeed.motorsdata.x;
+            pose3DMotorsData->y = pose->neckspeed.motorsdata.y;
+            pose3DMotorsData->z = pose->neckspeed.motorsdata.z;
+            pose3DMotorsData->pan = pose->neckspeed.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->neckspeed.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->neckspeed.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->neckspeed.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->neckspeed.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
+
+            return pose3DMotorsData;
+
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckspeedmotors);
+            
+            pose3DMotorsParams->maxPan = pose->neckspeed.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->neckspeed.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->neckspeed.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->neckspeed.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->neckspeed.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->neckspeed.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_neckspeedmotors);
+            
+            pose->neckspeed.motorsdata.x = data->x;
+            pose->neckspeed.motorsdata.y = data->y;
+            pose->neckspeed.motorsdata.z = data->z;
+            pose->neckspeed.motorsdata.pan = data->pan;
+            pose->neckspeed.motorsdata.tilt = data->tilt;
+            pose->neckspeed.motorsdata.roll = data->roll;
+            pose->neckspeed.motorsdata.panSpeed = data->panSpeed;
+            pose->neckspeed.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
+
+        }
+
+        gazebo::PoseNeck* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_NeckSpeedICE ( void* v ) {
+
+        gazebo::PoseNeckSpeed* neckspeed = (gazebo::PoseNeckSpeed*)v;
+        char* name = (char*) neckspeed->cfgfile_neckspeed.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseNeckSpeedEncoders.Endpoints");
+            std::cout << "PoseNeckSpeedEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseNeckSpeedMotors.Endpoints");
+            std::cout << "PoseNeckSpeedMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterNeckSpeedEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterNeckSpeedMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(neckspeed);
+            Ice::ObjectPtr motors = new Pose3DMotors(neckspeed);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("NeckSpeedEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("NeckSpeedMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseNeckSpeedMotors.Endpoints=default -h localhost -p 12994
+PoseNeckSpeedEncoders.Endpoints=default -h localhost -p 12995

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poseneckspeed.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef NECKSPEED_H
+#define NECKSPEED_H
+
+namespace gazebo {
+
+    void* thread_NeckSpeedICE ( void* v );
+
+    class PoseNeckSpeed : public ModelPlugin {
+    public:
+
+        PoseNeckSpeed ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_neckspeed;
+        pthread_mutex_t mutex_neckspeedencoders;
+        pthread_mutex_t mutex_neckspeedmotors;
+
+        struct neckspeed_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        neckspeed_t neckspeed;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // NECKSPEED_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightankle.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseRightAnkle)
+
+    PoseRightAnkle::PoseRightAnkle () {
+        pthread_mutex_init(&this->mutex_rightankleencoders, NULL);
+        pthread_mutex_init(&this->mutex_rightanklemotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_rightshoulder = std::string("--Ice.Config=poserightankle.cfg");
+        
+        this->rightankle.motorsparams.maxPan = 1.57;
+        this->rightankle.motorsparams.minPan = -1.57;          
+        this->rightankle.motorsparams.maxTilt = 0.5;
+        this->rightankle.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseRightAnkle" << std::endl;
+    }
+
+    void PoseRightAnkle::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersrightankle_pitch"))
+            gzerr << "pose3dencodersrightankle plugin missing <joint_pose3dencodersrightankle_pitch> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersrightankle_roll"))
+            gzerr << "pose3dencodersrightankle plugin missing <joint_pose3dencodersrightankle_roll> element\n";
+
+        this->rightankle.joint_roll = _model->GetJoint("rankle_roll");
+        this->rightankle.joint_tilt = _model->GetJoint("rankle_pitch");
+
+        if (!this->rightankle.joint_pose3dencoders_roll)
+            gzerr << "Unable to find joint_pose3dencodersrightankle_roll["
+                << _sdf->GetElement("joint_pose3dencodersrightankle_roll")->GetValueString() << "]\n";
+        if (!this->rightankle.joint_pose3dencoders_tilt)
+            gzerr << "Unable to find joint_pose3dencodersrightankle_pitch["
+                << _sdf->GetElement("joint_pose3dencodersrightankle_pitch")->GetValueString() << "]\n"; 
+                
+        this->rightankle.link_roll = _model->GetLink("right_foot");
+        this->rightankle.link_tilt = _model->GetLink("rightankle_pitch");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseRightAnkle::OnUpdate, this));
+    }
+
+    void PoseRightAnkle::Init () {}
+
+    void PoseRightAnkle::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+        
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_RightAnkleICE, (void*) this);
+            
+            this->rightankle.encoders.roll = 0;    
+            this->rightankle.encoders.tilt = 0;
+        } else {
+            //          ----------ENCODERS----------
+            //GET pose3dencoders data from the right ankle (PAN&TILT)
+            this->rightankle.encoders.roll = this->rightankle.link_roll->GetRelativePose().rot.GetAsEuler().y;    
+            this->rightankle.encoders.tilt = this->rightankle.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+        }
+
+        //          ----------MOTORS----------
+        if (this->rightankle.motorsdata.roll >= 0) {
+            if (this->rightankle.encoders.roll < this->rightankle.motorsdata.roll) {
+                this->rightankle.joint_roll->SetVelocity(0, -0.1);
+                this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->rightankle.joint_roll->SetVelocity(0, 0.1);
+                this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightankle.encoders.roll > this->rightankle.motorsdata.roll) {
+                this->rightankle.joint_roll->SetVelocity(0, 0.1);
+                this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->rightankle.joint_roll->SetVelocity(0, -0.1);
+                this->rightankle.joint_roll->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->rightankle.motorsdata.tilt >= 0) {
+            if (this->rightankle.encoders.tilt < this->rightankle.motorsdata.tilt) {
+                this->rightankle.joint_tilt->SetVelocity(0, -0.1);
+                this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightankle.joint_tilt->SetVelocity(0, 0.1);
+                this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightankle.encoders.tilt > this->rightankle.motorsdata.tilt) {
+                this->rightankle.joint_tilt->SetVelocity(0, 0.1);
+                this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightankle.joint_tilt->SetVelocity(0, -0.1);
+                this->rightankle.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseRightAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightankleencoders);
+            
+            pose3DEncodersData->x = pose->rightankle.encoders.x;
+            pose3DEncodersData->y = pose->rightankle.encoders.y;
+            pose3DEncodersData->z = pose->rightankle.encoders.z;
+            pose3DEncodersData->pan = pose->rightankle.encoders.pan;
+            pose3DEncodersData->tilt = pose->rightankle.encoders.tilt;
+            pose3DEncodersData->roll = pose->rightankle.encoders.roll;
+            pose3DEncodersData->clock = pose->rightankle.encoders.clock;
+            pose3DEncodersData->maxPan = pose->rightankle.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->rightankle.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->rightankle.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->rightankle.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_rightankleencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseRightAnkle* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseRightAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightanklemotors);
+            
+            pose3DMotorsData->x = pose->rightankle.motorsdata.x;
+            pose3DMotorsData->y = pose->rightankle.motorsdata.y;
+            pose3DMotorsData->z = pose->rightankle.motorsdata.z;
+            pose3DMotorsData->pan = pose->rightankle.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->rightankle.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->rightankle.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->rightankle.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->rightankle.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightanklemotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightanklemotors);
+            
+            pose3DMotorsParams->maxPan = pose->rightankle.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->rightankle.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->rightankle.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->rightankle.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->rightankle.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->rightankle.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightanklemotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightanklemotors);
+            
+            pose->rightankle.motorsdata.x = data->x;
+            pose->rightankle.motorsdata.y = data->y;
+            pose->rightankle.motorsdata.z = data->z;
+            pose->rightankle.motorsdata.pan = data->pan;
+            pose->rightankle.motorsdata.tilt = data->tilt;
+            pose->rightankle.motorsdata.roll = data->roll;
+            pose->rightankle.motorsdata.panSpeed = data->panSpeed;
+            pose->rightankle.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightanklemotors);
+        }
+
+        gazebo::PoseRightAnkle* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_RightAnkleICE ( void* v ) {
+
+        gazebo::PoseRightAnkle* rightankle = (gazebo::PoseRightAnkle*)v;
+        char* name = (char*) rightankle->cfgfile_rightankle.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseRightAnkleEncoders.Endpoints");
+            std::cout << "PoseRightAnkleEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseRightAnkleMotors.Endpoints");
+            std::cout << "PoseRightAnkleMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightAnkleEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightAnkleMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(rightankle);
+            Ice::ObjectPtr motors = new Pose3DMotors(rightankle);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("RightAnkleEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("RightAnkleMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightAnkleEncoders.Endpoints=default -h localhost -p 10996
+PoseRightAnkleMotors.Endpoints=default -h localhost -p 10997

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightankle.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef RIGHTANKLE_H
+#define RIGHTANKLE_H
+
+namespace gazebo {
+
+    void* thread_RightAnkleICE ( void* v );
+
+    class PoseRightAnkle : public ModelPlugin {
+    public:
+
+        PoseRightAnkle ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_rightankle;
+        pthread_mutex_t mutex_rightankleencoders;
+        pthread_mutex_t mutex_rightanklemotors;
+
+        struct rightankle_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        rightankle_t rightankle;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // RIGHTANKLE_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,295 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightelbow.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseRightElbow)
+
+    PoseRightElbow::PoseRightElbow () {
+        pthread_mutex_init(&this->mutex_rightelbowencoders, NULL);
+        pthread_mutex_init(&this->mutex_rightelbowmotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_rightelbow = std::string("--Ice.Config=poserightelbow.cfg");
+        
+        this->rightelbow.motorsparams.maxPan = 1.57;
+        this->rightelbow.motorsparams.minPan = -1.57;          
+        this->rightelbow.motorsparams.maxTilt = 0.5;
+        this->rightelbow.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseRightElbow" << std::endl;
+    }
+
+    void PoseRightElbow::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersrightelbow_yaw"))
+            gzerr << "pose3dencodersrightelbow plugin missing <joint_pose3dencodersrightelbow_yaw> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersrightelbow_roll"))
+            gzerr << "pose3dencodersrightelbow plugin missing <joint_pose3dencodersrightelbow_roll> element\n";
+
+        this->rightelbow.joint_pan = _model->GetJoint("relbow_yaw");
+        this->rightelbow.joint_roll = _model->GetJoint("relbow_roll");
+
+        if (!this->rightelbow.joint_pan)
+            gzerr << "Unable to find joint_pose3dencodersrightelbow_yaw["
+                << _sdf->GetElement("joint_pose3dencodersrightelbow_yaw")->GetValueString() << "]\n"; 
+        if (!this->rightelbow.joint_roll)
+            gzerr << "Unable to find joint_pose3dencodersrightelbow_roll["
+                << _sdf->GetElement("joint_pose3dencodersrightelbow_roll")->GetValueString() << "]\n";
+                
+        this->rightelbow.link_pan = _model->GetLink("rightelbow_yaw");
+        this->rightelbow.link_roll = _model->GetLink("right_lower_arm");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the rigth elbow.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseRightElbow::OnUpdate, this));
+    }
+
+    void PoseRightElbow::Init () {}
+
+    void PoseRightElbow::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_RightElbowICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the right elbow (PAN&TILT)
+//        this->rightelbow.encoder.pan = this->rightelbow.rightelbow_link_pan->GetRelativePose().rot.GetAsEuler().z;    
+//        this->rightelbow.encoder.roll = this->rightelbow.rightelbow_link_roll->GetRelativePose().rot.GetAsEuler().y;
+        
+        this->rightelbow.encoders.pan = - this->rightelbow.joint_pan->GetAngle(0).Radian();
+        this->rightelbow.encoders.roll = - this->rightelbow.joint_roll->GetAngle(0).Radian();
+
+        //          ----------MOTORS----------
+        if (this->rightelbow.motorsdata.pan >= 0) {
+            if (this->rightelbow.encoders.pan < this->rightelbow.motorsdata.pan) {
+                this->rightelbow.joint_pan->SetVelocity(0, -0.1);
+                this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->rightelbow.joint_pan->SetVelocity(0, 0.1);
+                this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightelbow.encoders.pan > this->rightelbow.motorsdata.pan) {
+                this->rightelbow.joint_pan->SetVelocity(0, 0.1);
+                this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->rightelbow.joint_pan->SetVelocity(0, -0.1);
+                this->rightelbow.joint_pan->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->rightelbow.motorsdata.roll >= 0) {
+            if (this->rightelbow.encoders.roll < this->rightelbow.motorsdata.roll) {
+                this->rightelbow.joint_roll->SetVelocity(0, -0.1);
+                this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightelbow.joint_roll->SetVelocity(0, 0.1);
+                this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightelbow.encoders.roll > this->rightelbow.motorsdata.roll) {
+                this->rightelbow.joint_roll->SetVelocity(0, 0.1);
+                this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightelbow.joint_roll->SetVelocity(0, -0.1);
+                this->rightelbow.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseRightElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightelbowencoders);
+            
+            pose3DEncodersData->x = pose->rightelbow.encoders.x;
+            pose3DEncodersData->y = pose->rightelbow.encoders.y;
+            pose3DEncodersData->z = pose->rightelbow.encoders.z;
+            pose3DEncodersData->pan = pose->rightelbow.encoders.pan;
+            pose3DEncodersData->tilt = pose->rightelbow.encoders.tilt;
+            pose3DEncodersData->roll = pose->rightelbow.encoders.roll;
+            pose3DEncodersData->clock = pose->rightelbow.encoders.clock;
+            pose3DEncodersData->maxPan = pose->rightelbow.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->rightelbow.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->rightelbow.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->rightelbow.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_rightelbowencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseRightElbow* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseRightElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightelbowmotors);
+            
+            pose3DMotorsData->x = pose->rightelbow.motorsdata.x;
+            pose3DMotorsData->y = pose->rightelbow.motorsdata.y;
+            pose3DMotorsData->z = pose->rightelbow.motorsdata.z;
+            pose3DMotorsData->pan = pose->rightelbow.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->rightelbow.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->rightelbow.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->rightelbow.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->rightelbow.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightelbowmotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightelbowmotors);
+            
+            pose3DMotorsParams->maxPan = pose->rightelbow.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->rightelbow.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->rightelbow.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->rightelbow.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->rightelbow.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->rightelbow.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightelbowmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightelbowmotors);
+            
+            pose->rightelbow.motorsdata.x = data->x;
+            pose->rightelbow.motorsdata.y = data->y;
+            pose->rightelbow.motorsdata.z = data->z;
+            pose->rightelbow.motorsdata.pan = data->pan;
+            pose->rightelbow.motorsdata.tilt = data->tilt;
+            pose->rightelbow.motorsdata.roll = data->roll;
+            pose->rightelbow.motorsdata.panSpeed = data->panSpeed;
+            pose->rightelbow.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightelbowmotors);
+        }
+
+        gazebo::PoseRightElbow* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_RightElbowICE ( void* v ) {
+
+        gazebo::PoseRightElbow* rightelbow = (gazebo::PoseRightElbow*)v;
+        char* name = (char*) rightelbow->cfgfile_rightelbow.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseRightElbowEncoders.Endpoints");
+            std::cout << "PoseRightElbowEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseRightElbowMotors.Endpoints");
+            std::cout << "PoseRightElbowMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightElbowEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightElbowMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(rightelbow);
+            Ice::ObjectPtr motors = new Pose3DMotors(rightelbow);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("RightElbowEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("RightElbowMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightElbowEncoders.Endpoints=default -h localhost -p 9998
+PoseRightElbowMotors.Endpoints=default -h localhost -p 9999

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightelbow.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSERIGHTELBOW_H
+#define POSERIGHTELBOW_H
+
+namespace gazebo {
+
+    void* thread_RightElbowICE ( void* v );
+
+    class PoseRightElbow : public ModelPlugin {
+    public:
+
+        PoseRightElbow ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_rightelbow;
+        pthread_mutex_t mutex_rightelbowencoders;
+        pthread_mutex_t mutex_rightelbowmotors;
+
+        struct rightelbow_t {
+            physics::JointPtr joint_pan, joint_roll;
+            physics::LinkPtr link_pan, link_roll;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        rightelbow_t rightelbow;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSERIGHTELBOW_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,315 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserighthip.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseRightHip)
+
+    PoseRightHip::PoseRightHip () {
+        pthread_mutex_init(&this->mutex_righthipencoders, NULL);
+        pthread_mutex_init(&this->mutex_righthipmotors, NULL);
+        this->countH = 0;
+        this->cycle = 50;
+        this->cfgfile_righthip = std::string("--Ice.Config=poserighthip.cfg");
+                 
+        this->righthip.motorsparams.maxPan = 1.57;
+        this->righthip.motorsparams.minPan = -1.57;
+        this->righthip.motorsparams.maxTilt = 0.5;
+        this->righthip.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseRightHip" << std::endl;
+    }
+
+    void PoseRightHip::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersrhip_yawpitch"))
+            gzerr << "pose3dencodership plugin missing <joint_pose3dencodersrhip_yawpitch> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersrhip_roll"))
+            gzerr << "pose3dencodership plugin missing <joint_pose3dencodersrhip_roll> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersrhip_pitch"))
+            gzerr << "pose3dencodership plugin missing <joint_pose3dencodersrhip_pitch> element\n";
+
+        this->righthip.joint_pan = _model->GetJoint("rhip_yawpitch");
+        this->righthip.joint_tilt = _model->GetJoint("rhip_pitch");
+        this->righthip.joint_roll = _model->GetJoint("rhip_roll");
+
+        if (!this->righthip.joint_pan)
+            gzerr << "Unable to find joint_pose3dencodership_pan["
+                << _sdf->GetElement("joint_pose3dencodership_pan")->GetValueString() << "]\n";
+        if (!this->righthip.joint_tilt)
+            gzerr << "Unable to find joint_pose3dencodership_tilt["
+                << _sdf->GetElement("joint_pose3dencodership_tilt")->GetValueString() << "]\n";
+        if (!this->righthip.joint_roll)
+            gzerr << "Unable to find joint_pose3dencodership_roll["
+                << _sdf->GetElement("joint_pose3dencodership_roll")->GetValueString() << "]\n";
+                
+        this->righthip.link_pan = _model->GetLink("righthip_yawpitch");
+        this->righthip.link_tilt = _model->GetLink("right_thigh");
+        this->righthip.link_roll = _model->GetLink("righthip_roll");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the right hip.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseRightHip::OnUpdate, this));
+    }
+
+    void PoseRightHip::Init () {}
+
+    void PoseRightHip::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_RightHipICE, (void*) this);
+        }
+        
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the hip (PAN&TILT)
+        this->righthip.encoders.pan = this->righthip.link_pan->GetRelativePose().rot.GetAsEuler().z;
+        this->righthip.encoders.tilt = this->righthip.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+        this->righthip.encoders.roll = this->righthip.link_roll->GetRelativePose().rot.GetAsEuler().y;
+        
+        if (this->righthip.motorsdata.pan >= 0) {
+            if (this->righthip.encoders.pan < this->righthip.motorsdata.pan) {
+                this->righthip.joint_pan->SetVelocity(0, -0.1);
+                this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+            } else {
+                this->righthip.joint_pan->SetVelocity(0, 0.1);
+                this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->righthip.encoders.pan > this->righthip.motorsdata.pan) {
+                this->righthip.joint_pan->SetVelocity(0, 0.1);
+                this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+            } else {
+                this->righthip.joint_pan->SetVelocity(0, -0.1);
+                this->righthip.joint_pan->SetMaxForce(0, this->stiffness);
+            }
+        }
+        
+        if (this->righthip.motorsdata.tilt >= 0) {
+            if (this->righthip.encoders.tilt < this->righthip.motorsdata.tilt) {
+                this->righthip.joint_tilt->SetVelocity(0, -0.1);
+                this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->righthip.joint_tilt->SetVelocity(0, 0.1);
+                this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->righthip.encoders.tilt > this->righthip.motorsdata.tilt) {
+                this->righthip.joint_tilt->SetVelocity(0, 0.1);
+                this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->righthip.joint_tilt->SetVelocity(0, -0.1);
+                this->righthip.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+        
+        if (this->righthip.motorsdata.roll >= 0) {
+            if (this->righthip.encoders.roll < this->righthip.motorsdata.roll) {
+                this->righthip.joint_roll->SetVelocity(0, -0.1);
+                this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+            } else {
+                this->righthip.joint_roll->SetVelocity(0, 0.1);
+                this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->righthip.encoders.roll > this->righthip.motorsdata.roll) {
+                this->righthip.joint_roll->SetVelocity(0, 0.1);
+                this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+            } else {
+                this->righthip.joint_roll->SetVelocity(0, -0.1);
+                this->righthip.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseRightHip* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_righthipencoders);
+            
+            pose3DEncodersData->x = pose->righthip.encoders.x;
+            pose3DEncodersData->y = pose->righthip.encoders.y;
+            pose3DEncodersData->z = pose->righthip.encoders.z;
+            pose3DEncodersData->pan = pose->righthip.encoders.pan;
+            pose3DEncodersData->tilt = pose->righthip.encoders.tilt;
+            pose3DEncodersData->roll = pose->righthip.encoders.roll;
+            pose3DEncodersData->clock = pose->righthip.encoders.clock;
+            pose3DEncodersData->maxPan = pose->righthip.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->righthip.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->righthip.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->righthip.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_righthipencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseRightHip* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseRightHip* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_righthipmotors);
+            
+            pose3DMotorsData->x = pose->righthip.motorsdata.x;
+            pose3DMotorsData->y = pose->righthip.motorsdata.y;
+            pose3DMotorsData->z = pose->righthip.motorsdata.z;
+            pose3DMotorsData->pan = pose->righthip.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->righthip.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->righthip.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->righthip.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->righthip.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_righthipmotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_righthipmotors);
+            
+            pose3DMotorsParams->maxPan = pose->righthip.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->righthip.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->righthip.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->righthip.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->righthip.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->righthip.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_righthipmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_righthipmotors);
+            
+            pose->righthip.motorsdata.x = data->x;
+            pose->righthip.motorsdata.y = data->y;
+            pose->righthip.motorsdata.z = data->z;
+            pose->righthip.motorsdata.pan = data->pan;
+            pose->righthip.motorsdata.tilt = data->tilt;
+            pose->righthip.motorsdata.roll = data->roll;
+            pose->righthip.motorsdata.panSpeed = data->panSpeed;
+            pose->righthip.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_righthipmotors);
+        }
+
+        gazebo::PoseRightHip* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_RightHipICE ( void* v ) {
+
+        gazebo::PoseRightHip* righthip = (gazebo::PoseRightHip*)v;
+        char* name = (char*) righthip->cfgfile_righthip.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseRightHipEncoders.Endpoints");
+            std::cout << "PoseRightHipEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseRightHipkMotors.Endpoints");
+            std::cout << "PoseRightHipMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightHipEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightHipkMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(righthip);
+            Ice::ObjectPtr motors = new Pose3DMotors(righthip);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("RightHipEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("RightHipMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightHipEncoders.Endpoints=default -h localhost -p 8996
+PoseRightHipMotors.Endpoints=default -h localhost -p 8997

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserighthip.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSERIGHTHIP_H
+#define POSERIGHTHIP_H
+
+namespace gazebo {
+
+    void* thread_RightHipICE ( void* v );
+
+    class PoseRightHip : public ModelPlugin {
+    public:
+
+        PoseRightHip ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_righthip;
+        pthread_mutex_t mutex_righthipencoders;
+        pthread_mutex_t mutex_righthipmotors;
+
+        struct righthip_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        righthip_t righthip;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSERIGHTHIP_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,266 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightknee.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseRightKnee)
+
+    PoseRightKnee::PoseRightKnee () {
+        pthread_mutex_init(&this->mutex_rightkneeencoders, NULL);
+        pthread_mutex_init(&this->mutex_rightkneemotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_rightknee = std::string("--Ice.Config=poserightknee.cfg");
+        
+        this->rightknee.motorsparams.maxPan = 1.57;
+        this->rightknee.motorsparams.minPan = -1.57;          
+        this->rightknee.motorsparams.maxTilt = 0.5;
+        this->rightknee.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseRightKnee" << std::endl;
+    }
+
+    void PoseRightKnee::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersrightknee_pitch"))
+            gzerr << "pose3dencodersrightknee plugin missing <joint_pose3dencodersrightknee_pitch> element\n";
+
+        this->rightknee.joint_tilt = _model->GetJoint("right_knee");
+
+        if (!this->rightknee.joint_tilt)
+            gzerr << "Unable to find joint_pose3dencodersrightknee_pitch["
+                << _sdf->GetElement("joint_pose3dencodersrightknee_pitch")->GetValueString() << "]\n"; 
+                
+        this->rightknee.link_tilt = _model->GetLink("right_shin");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseRightKnee::OnUpdate, this));
+    }
+
+    void PoseRightKnee::Init () {}
+
+    void PoseRightKnee::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+        
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_RightKneeICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the right knee (PAN&TILT)  
+        this->rightknee.encoders.tilt = this->rightknee.link_tilt->GetRelativePose().rot.GetAsEuler().x;
+
+        //          ----------MOTORS----------
+        if (this->rightknee.motorsdata.tilt >= 0) {
+            if (this->rightknee.encoders.tilt < this->rightknee.motorsdata.tilt) {
+                this->rightknee.joint_tilt->SetVelocity(0, -0.1);
+                this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightknee.joint_tilt->SetVelocity(0, 0.1);
+                this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightknee.encoders.tilt > this->rightknee.motorsdata.tilt) {
+                this->rightknee.joint_tilt->SetVelocity(0, 0.1);
+                this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightknee.joint_tilt->SetVelocity(0, -0.1);
+                this->rightknee.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseRightKnee* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightkneeencoders);
+            
+            pose3DEncodersData->x = pose->rightknee.encoders.x;
+            pose3DEncodersData->y = pose->rightknee.encoders.y;
+            pose3DEncodersData->z = pose->rightknee.encoders.z;
+            pose3DEncodersData->pan = pose->rightknee.encoders.pan;
+            pose3DEncodersData->tilt = pose->rightknee.encoders.tilt;
+            pose3DEncodersData->roll = pose->rightknee.encoders.roll;
+            pose3DEncodersData->clock = pose->rightknee.encoders.clock;
+            pose3DEncodersData->maxPan = pose->rightknee.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->rightknee.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->rightknee.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->rightknee.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_rightkneeencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseRightKnee* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseRightKnee* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightkneemotors);
+            
+            pose3DMotorsData->x = pose->rightknee.motorsdata.x;
+            pose3DMotorsData->y = pose->rightknee.motorsdata.y;
+            pose3DMotorsData->z = pose->rightknee.motorsdata.z;
+            pose3DMotorsData->pan = pose->rightknee.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->rightknee.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->rightknee.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->rightknee.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->rightknee.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightkneemotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightkneemotors);
+            
+            pose3DMotorsParams->maxPan = pose->rightknee.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->rightknee.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->rightknee.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->rightknee.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->rightknee.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->rightknee.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightkneemotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightkneemotors);
+            
+            pose->rightknee.motorsdata.x = data->x;
+            pose->rightknee.motorsdata.y = data->y;
+            pose->rightknee.motorsdata.z = data->z;
+            pose->rightknee.motorsdata.pan = data->pan;
+            pose->rightknee.motorsdata.tilt = data->tilt;
+            pose->rightknee.motorsdata.roll = data->roll;
+            pose->rightknee.motorsdata.panSpeed = data->panSpeed;
+            pose->rightknee.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightkneemotors);
+        }
+
+        gazebo::PoseRightKnee* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_RightKneeICE ( void* v ) {
+
+        gazebo::PoseRightKnee* rightknee = (gazebo::PoseRightKnee*)v;
+        char* name = (char*) rightknee->cfgfile_rightknee.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseRightKneeEncoders.Endpoints");
+            std::cout << "PoseRightKneeEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseRightKneeMotors.Endpoints");
+            std::cout << "PoseRightKneeMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightKneeEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightKneeMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(rightknee);
+            Ice::ObjectPtr motors = new Pose3DMotors(rightknee);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("RightKneeEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("RightKneeMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+
+    }
+
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightKneeEncoders.Endpoints=default -h localhost -p 8994
+PoseRightKneeMotors.Endpoints=default -h localhost -p 8995

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightknee.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSERIGHTKNEE_H
+#define POSERIGHTKNEE_H
+
+namespace gazebo {
+
+    void* thread_RightKneeICE ( void* v );
+
+    class PoseRightKnee : public ModelPlugin {
+    public:
+
+        PoseRightKnee ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_rightknee;
+        pthread_mutex_t mutex_rightkneeencoders;
+        pthread_mutex_t mutex_rightkneemotors;
+
+        struct rightknee_t {
+            physics::JointPtr joint_tilt, joint_pan;
+            physics::LinkPtr link_pan, link_tilt;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        rightknee_t rightknee;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSERIGHTKNEE_H

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cc	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,298 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include "poserightshoulder.h"
+
+#define RADTODEG 57.29582790
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseRightShoulder)
+
+    PoseRightShoulder::PoseRightShoulder () {
+        pthread_mutex_init(&this->mutex_rightshoulderencoders, NULL);
+        pthread_mutex_init(&this->mutex_rightshouldermotors, NULL);
+        this->count = 0;
+        this->cycle = 50;
+        this->cfgfile_rightshoulder = std::string("--Ice.Config=poserightshoulder.cfg");
+        
+        this->rightshoulder.motorsparams.maxPan = 1.57;
+        this->rightshoulder.motorsparams.minPan = -1.57;          
+        this->rightshoulder.motorsparams.maxTilt = 0.5;
+        this->rightshoulder.motorsparams.minTilt = -0.5;
+
+        std::cout << "Constructor PoseRightShoulder" << std::endl;
+    }
+
+    void PoseRightShoulder::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        // LOAD CAMERA LEFT
+        if (!_sdf->HasElement("joint_pose3dencodersrightshoulder_pitch"))
+            gzerr << "pose3dencodersrightshoulder plugin missing <joint_pose3dencodersrightshoulder_pitch> element\n";
+        if (!_sdf->HasElement("joint_pose3dencodersrightshoulder_roll"))
+            gzerr << "pose3dencodersrightshoulder plugin missing <joint_pose3dencodersrightshoulder_roll> element\n";
+
+        this->rightshoulder.joint_roll = _model->GetJoint("rshoulder_roll");
+        this->rightshoulder.joint_tilt = _model->GetJoint("rshoulder_pitch");
+
+        if (!this->rightshoulder.joint_roll)
+            gzerr << "Unable to find joint_pose3dencodersrightshoulder_roll["
+                << _sdf->GetElement("joint_pose3dencodersrightshoulder_roll")->GetValueString() << "]\n";
+        if (!this->rightshoulder.joint_tilt)
+            gzerr << "Unable to find joint_pose3dencodersrightshoulder_pitch["
+                << _sdf->GetElement("joint_pose3dencodersrightshoulder_pitch")->GetValueString() << "]\n"; 
+                
+        this->rightshoulder.link_roll = _model->GetLink("right_upper_arm");
+        this->rightshoulder.link_tilt = _model->GetLink("rightshoulder_pitch");
+
+        //LOAD TORQUE        
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            this->stiffness = 5.0;
+        }
+
+        //LOAD POSE3DMOTORS
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+            boost::bind(&PoseRightShoulder::OnUpdate, this));
+
+    }
+
+    void PoseRightShoulder::Init () {}
+
+    void PoseRightShoulder::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+        
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        if (this->count == 0) {
+            this->count++;
+            pthread_t thr_ice;
+            pthread_create(&thr_ice, NULL, &thread_RightShoulderICE, (void*) this);
+        }
+
+        //          ----------ENCODERS----------
+        //GET pose3dencoders data from the right shoulder (PAN&TILT)
+//        this->rightshoulder.encoder.roll = this->rightshoulder.rightshoulder_link_roll->GetRelativePose().rot.GetAsEuler().y;    
+//        this->rightshoulder.encoder.tilt = this->rightshoulder.rightshoulder_link_tilt->GetRelativePose().rot.GetAsEuler().x;
+        
+        this->rightshoulder.encoders.tilt = - this->rightshoulder.joint_tilt->GetAngle(0).Radian();
+        this->rightshoulder.encoders.roll = - this->rightshoulder.joint_roll->GetAngle(0).Radian();
+        
+        //          ----------MOTORS----------
+        if (this->rightshoulder.motorsdata.roll >= 0) {
+            if (this->rightshoulder.encoders.roll < this->rightshoulder.motorsdata.roll) {
+                this->rightshoulder.joint_roll->SetVelocity(0, -0.1);
+                this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->rightshoulder.joint_roll->SetVelocity(0, 0.1);
+                this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightshoulder.encoders.roll > this->rightshoulder.motorsdata.roll) {
+                this->rightshoulder.joint_roll->SetVelocity(0, 0.1);
+                this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+                //std::cout << "AQUI" << std::endl;
+            } else {
+                this->rightshoulder.joint_roll->SetVelocity(0, -0.1);
+                this->rightshoulder.joint_roll->SetMaxForce(0, this->stiffness);
+            }            
+        }
+        
+        if (this->rightshoulder.motorsdata.tilt >= 0) {
+            if (this->rightshoulder.encoders.tilt < this->rightshoulder.motorsdata.tilt) {
+                this->rightshoulder.joint_tilt->SetVelocity(0, -0.1);
+                this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightshoulder.joint_tilt->SetVelocity(0, 0.1);
+                this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        } else {
+            if (this->rightshoulder.encoders.tilt > this->rightshoulder.motorsdata.tilt) {
+                this->rightshoulder.joint_tilt->SetVelocity(0, 0.1);
+                this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            } else {
+                this->rightshoulder.joint_tilt->SetVelocity(0, -0.1);
+                this->rightshoulder.joint_tilt->SetMaxForce(0, this->stiffness);
+            }
+        }
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncoders ( gazebo::PoseRightShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncoders () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightshoulderencoders);
+            
+            pose3DEncodersData->x = pose->rightshoulder.encoders.x;
+            pose3DEncodersData->y = pose->rightshoulder.encoders.y;
+            pose3DEncodersData->z = pose->rightshoulder.encoders.z;
+            pose3DEncodersData->pan = pose->rightshoulder.encoders.pan;
+            pose3DEncodersData->tilt = pose->rightshoulder.encoders.tilt;
+            pose3DEncodersData->roll = pose->rightshoulder.encoders.roll;
+            pose3DEncodersData->clock = pose->rightshoulder.encoders.clock;
+            pose3DEncodersData->maxPan = pose->rightshoulder.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->rightshoulder.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->rightshoulder.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->rightshoulder.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_rightshoulderencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseRightShoulder* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotors (gazebo::PoseRightShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotors() {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightshouldermotors);
+            
+            pose3DMotorsData->x = pose->rightshoulder.motorsdata.x;
+            pose3DMotorsData->y = pose->rightshoulder.motorsdata.y;
+            pose3DMotorsData->z = pose->rightshoulder.motorsdata.z;
+            pose3DMotorsData->pan = pose->rightshoulder.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->rightshoulder.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->rightshoulder.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->rightshoulder.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->rightshoulder.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
+
+            return pose3DMotorsData;
+
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightshouldermotors);
+            
+            pose3DMotorsParams->maxPan = pose->rightshoulder.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->rightshoulder.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->rightshoulder.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->rightshoulder.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->rightshoulder.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->rightshoulder.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_rightshouldermotors);
+            
+            pose->rightshoulder.motorsdata.x = data->x;
+            pose->rightshoulder.motorsdata.y = data->y;
+            pose->rightshoulder.motorsdata.z = data->z;
+            pose->rightshoulder.motorsdata.pan = data->pan;
+            pose->rightshoulder.motorsdata.tilt = data->tilt;
+            pose->rightshoulder.motorsdata.roll = data->roll;
+            pose->rightshoulder.motorsdata.panSpeed = data->panSpeed;
+            pose->rightshoulder.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
+
+        }
+
+        gazebo::PoseRightShoulder* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_RightShoulderICE ( void* v ) {
+
+        gazebo::PoseRightShoulder* rightshoulder = (gazebo::PoseRightShoulder*)v;
+        char* name = (char*) rightshoulder->cfgfile_rightshoulder.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseRightShoulderEncoders.Endpoints");
+            std::cout << "PoseRightShoulderEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseRightShoulderMotors.Endpoints");
+            std::cout << "PoseRightShoulderMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightShoulderEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterRightShoulderMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncoders(rightshoulder);
+            Ice::ObjectPtr motors = new Pose3DMotors(rightshoulder);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("RightShoulderEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("RightShoulderMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.cfg	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,3 @@
+#without registry
+PoseRightShoulderEncoders.Endpoints=default -h localhost -p 9996
+PoseRightShoulderMotors.Endpoints=default -h localhost -p 9997

Added: trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/nao/poserightshoulder.h	2013-10-14 17:47:38 UTC (rev 1049)
@@ -0,0 +1,75 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Authors : Borja Menéndez <borjamonserrano en gmail.com>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSERIGHTSHOULDER_H
+#define POSERIGHTSHOULDER_H
+
+namespace gazebo {
+
+    void* thread_RightShoulderICE ( void* v );
+
+    class PoseRightShoulder : public ModelPlugin {
+    public:
+
+        PoseRightShoulder ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_rightshoulder;
+        pthread_mutex_t mutex_rightshoulderencoders;
+        pthread_mutex_t mutex_rightshouldermotors;
+
+        struct rightshoulder_t {
+            physics::JointPtr joint_tilt, joint_roll;
+            physics::LinkPtr link_tilt, link_roll;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        rightshoulder_t rightshoulder;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int count;
+        int cycle;
+    };
+}
+
+#endif // POSERIGHTSHOULDER_H



More information about the Jderobot-admin mailing list