[Jderobot-admin] jderobot-r1148 - in trunk/src/stable/components: gazeboserver/models/nao gazeboserver/plugins/nao gazeboserver/plugins/nao/worlds naoviewer

bmenendez en jderobot.org bmenendez en jderobot.org
Dom Ene 26 11:32:52 CET 2014


Author: bmenendez
Date: 2014-01-26 11:32:52 +0100 (Sun, 26 Jan 2014)
New Revision: 1148

Modified:
   trunk/src/stable/components/gazeboserver/models/nao/model.sdf
   trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.h
   trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/ballred.h
   trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_bottom.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_top.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/common.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.h
   trunk/src/stable/components/gazeboserver/plugins/nao/worlds/naoworld.world
   trunk/src/stable/components/naoviewer/control.cpp
   trunk/src/stable/components/naoviewer/naooperator.cfg
   trunk/src/stable/components/naoviewer/naooperator.cpp
Log:
#148 Updating naothings: plugin Nao for Gazebo, its model and NaoViewer


Modified: trunk/src/stable/components/gazeboserver/models/nao/model.sdf
===================================================================
--- trunk/src/stable/components/gazeboserver/models/nao/model.sdf	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/models/nao/model.sdf	2014-01-26 10:32:52 UTC (rev 1148)
@@ -809,7 +809,7 @@
 		                <far>5</far>
                     </clip>
                 </camera>
-                <plugin name='camera_nao' filename='libcamera_nao.so'/>
+                <plugin name='camera_dump' filename='libcamera_nao.so'/>
             </sensor>
             
 	 	    <visual name="visual">
@@ -861,7 +861,7 @@
 		                <far>5</far>
                     </clip>
                 </camera>
-                <plugin name='camera_nao' filename='libcamera_nao.so'/>
+                <plugin name='camera_dump' filename='libcamera_nao.so'/>
             </sensor>
             
 	 	    <visual name="visual">
@@ -899,17 +899,17 @@
             <pose>0 0 -0.041 0 0 0</pose>
             <child>left_shin</child>
             <parent>leftankle_pitch</parent>
-            <axis>                      <!--
+            <axis>                      
                 <limit>
                     <lower>-1.189516</lower>
                     <upper>0.922747</upper>
                 </limit>
-                <xyz>1 0 0</xyz>        -->
+                <xyz>1 0 0</xyz>        <!--
                 <limit>
                     <lower>0</lower>
                     <upper>0</upper>
                 </limit>
-                <xyz>0 0 0</xyz>        
+                <xyz>0 0 0</xyz>        -->
             </axis>
         </joint>
         
@@ -917,17 +917,17 @@
             <pose>0 0 0 0 0 0</pose>
             <child>leftankle_pitch</child>
             <parent>left_foot</parent>
-            <axis>                      <!--
+            <axis>                      
                 <limit>
                     <lower>-0.397880</lower>
                     <upper>0.769001</upper>
                 </limit>
-                <xyz>0 1 0</xyz>        --> 
+                <xyz>0 1 0</xyz>        <!-- 
                 <limit>
                     <lower>0</lower>
                     <upper>0</upper>
                 </limit>
-                <xyz>0 0 0</xyz>        
+                <xyz>0 0 0</xyz>        -->
             </axis>
         </joint>
         
@@ -935,17 +935,17 @@
             <pose>0 0 -0.046 0 0 0</pose>
             <child>left_thigh</child>
             <parent>left_shin</parent>
-            <axis>                      <!--
+            <axis>                      
                 <limit>
                     <lower>-0.092346</lower>
                     <upper>2.112528</upper>
                 </limit>
-                <xyz>0 0 1</xyz>        -->
+                <xyz>0 0 1</xyz>        <!--
                 <limit>
                     <lower>0</lower>
                     <upper>0</upper>
                 </limit>
-                <xyz>0 0 0</xyz>
+                <xyz>0 0 0</xyz>        -->
             </axis>
         </joint>
         
@@ -954,17 +954,17 @@
             <pose>0 0 -0.041 0 0 0</pose>
             <child>right_shin</child>
             <parent>rightankle_pitch</parent>
-            <axis>                      <!--
+            <axis>                      
                 <limit>
                     <lower>-1.186448</lower>
                     <upper>0.932056</upper>
                 </limit>
-                <xyz>1 0 0</xyz>        -->
+                <xyz>1 0 0</xyz>        <!--
                 <limit>
                     <lower>0</lower>
                     <upper>0</upper>
                 </limit>
-                <xyz>0 0 0</xyz>       
+                <xyz>0 0 0</xyz>        -->
             </axis>
         </joint>
         
@@ -972,17 +972,17 @@
             <pose>0 0 0 0 0 0</pose>
             <child>rightankle_pitch</child>
             <parent>right_foot</parent>
-            <axis>                      <!--
+            <axis>                     
                 <limit>
                     <lower>-0.785875</lower>
                     <upper>0.388676</upper>
                 </limit>
-                <xyz>0 1 0</xyz>        -->
+                <xyz>0 1 0</xyz>        <!--
                 <limit>
                     <lower>0</lower>
                     <upper>0</upper>
                 </limit>
-                <xyz>0 0 0</xyz>    
+                <xyz>0 0 0</xyz>        -->
             </axis>
         </joint>
 
@@ -990,17 +990,17 @@
             <pose>0 0 -0.046 0 0 0</pose>
             <child>right_thigh</child>
             <parent>right_shin</parent>
-            <axis>                      <!--
+            <axis>                      
                 <limit>
                     <lower>-0.103083</lower>
                     <upper>2.120198</upper>
                 </limit>
-                <xyz>0 0 1</xyz>        -->
+                <xyz>0 0 1</xyz>        <!--
                 <limit>
                     <lower>0</lower>
                     <upper>0</upper>
                 </limit>
-                <xyz>0 0 0</xyz>        
+                <xyz>0 0 0</xyz>        -->
             </axis>
         </joint>
         
@@ -1322,67 +1322,78 @@
             </axis>
         </joint>
         
-        <!-- PLUGINS -->    
+        <!-- PLUGINS -->       
         <plugin filename="libposeneck.so" name="poseneck">
-            <bottom_joint_pose3dencodersneck_pan>neck_pan</bottom_joint_pose3dencodersneck_pan>
-            <top_joint_pose3dencodersneck_tilt>neck_tilt</top_joint_pose3dencodersneck_tilt>
+            <joint_poseneck_pan>neck_pan</joint_poseneck_pan>
+            <joint_poseneck_tilt>neck_tilt</joint_poseneck_tilt>
             <torque>5</torque>
         </plugin>
-       <!--
-        <plugin filename="libpose3dencodersneckvelocity.so" name="pose3dencodersneckvelocity">
-            <bottom_joint_pose3dencodersneckvel_pan>neck_pan</bottom_joint_pose3dencodersneckvel_pan>
-            <top_joint_pose3dencodersneckvel_tilt>neck_tilt</top_joint_pose3dencodersneckvel_tilt>
+        <!--
+        <plugin filename="libposeneckspeed.so" name="poseneckspeed">
+            <joint_poseneckspeed_pan>neck_pan</joint_poseneckspeed_pan>
+            <joint_poseneckspeed_tilt>neck_tilt</joint_poseneckspeed_tilt>
             <torque>5</torque>
         </plugin>
-       -->
+        -->     
         <plugin filename="libposerightshoulder.so" name="poserightshoulder">
-            <joint_pose3dencodersrightshoulder_pitch>rshoulder_pitch</joint_pose3dencodersrightshoulder_pitch>
-            <joint_pose3dencodersrightshoulder_roll>rshoulder_roll</joint_pose3dencodersrightshoulder_roll>
+            <joint_poserightshoulder_pitch>rshoulder_pitch</joint_poserightshoulder_pitch>
+            <joint_poserightshoulder_roll>rshoulder_roll</joint_poserightshoulder_roll>
             <torque>5</torque>
         </plugin>
         
         <plugin filename="libposeleftshoulder.so" name="poseleftshoulder">
-            <joint_pose3dencodersleftshoulder_pitch>lshoulder_pitch</joint_pose3dencodersleftshoulder_pitch>
-            <joint_pose3dencodersleftshoulder_roll>lshoulder_roll</joint_pose3dencodersleftshoulder_roll>
+            <joint_poseleftshoulder_tilt>lshoulder_pitch</joint_poseleftshoulder_tilt>
+            <joint_poseleftshoulder_roll>lshoulder_roll</joint_poseleftshoulder_roll>
             <torque>5</torque>
         </plugin>
         
-        <plugin filename="libposerightelbow.so" name="poserightelbow">
-            <joint_pose3dencodersrightelbow_yaw>relbow_yaw</joint_pose3dencodersrightelbow_yaw>
-            <joint_pose3dencodersrightelbow_roll>relbow_roll</joint_pose3dencodersrightelbow_roll>
+        <plugin filename="libposerightelbow.so" name="poseencodersrightelbow">
+            <joint_poserightelbow_yaw>relbow_yaw</joint_poserightelbow_yaw>
+            <joint_poserightelbow_roll>relbow_roll</joint_poserightelbow_roll>
             <torque>5</torque>
         </plugin>  
         
         <plugin filename="libposeleftelbow.so" name="poseleftelbow">
-            <joint_pose3dencodersleftelbow_yaw>lelbow_yaw</joint_pose3dencodersleftelbow_yaw>
-            <joint_pose3dencodersleftelbow_roll>lelbow_roll</joint_pose3dencodersleftelbow_roll>
+            <joint_poseleftelbow_pan>lelbow_yaw</joint_poseleftelbow_pan>
+            <joint_poseleftelbow_roll>lelbow_roll</joint_poseleftelbow_roll>
             <torque>5</torque>
         </plugin>
         <!--
-        <plugin filename="libpose3dencodership.so" name="pose3dencodership">
-            <joint_pose3dencodersrhip_yawpitch>rhip_yawpitch</joint_pose3dencodersrhip_yawpitch>
-            <joint_pose3dencodersrhip_roll>rhip_roll</joint_pose3dencodersrhip_roll>
-            <joint_pose3dencodersrhip_pitch>rhip_pitch</joint_pose3dencodersrhip_pitch>
+        <plugin filename="libposerighthip.so" name="poserighthip">
+            <joint_poserighthip_yaw>rhip_yawpitch</joint_poserighthip_yaw>
+            <joint_poserighthip_roll>rhip_roll</joint_poserighthip_roll>
+            <joint_poserighthip_pitch>rhip_pitch</joint_poserighthip_pitch>
             <torque>5</torque>
         </plugin> 
-        --> <!--
-        <plugin filename="libpose3dencodersankle.so" name="pose3dencodersankle">
-            <joint_pose3dencodersrightankle_pitch>rankle_pitch</joint_pose3dencodersrightankle_pitch>
-            <joint_pose3dencodersrightankle_roll>rankle_roll</joint_pose3dencodersrightankle_roll>
+        
+        <plugin filename="libposelefthip.so" name="poselefthip">
+            <joint_poselefthip_yaw>lhip_yawpitch</joint_poselefthip_yaw>
+            <joint_poselefthip_roll>lhip_roll</joint_poselefthip_roll>
+            <joint_poselefthip_pitch>lhip_pitch</joint_poselefthip_pitch>
             <torque>5</torque>
+        </plugin> 
+        -->
+        <plugin filename="libposerightankle.so" name="poserightankle">
+            <joint_poserightankle_pitch>rankle_pitch</joint_poserightankle_pitch>
+            <joint_poserightankle_roll>rankle_roll</joint_poserightankle_roll>
+            <torque>5</torque>
         </plugin>
         
-        <plugin filename="libpose3dencodersleftankle.so" name="pose3dencodersleftankle">
-            <joint_pose3dencodersleftankle_pitch>lankle_pitch</joint_pose3dencodersleftankle_pitch>
-            <joint_pose3dencodersleftankle_roll>lankle_roll</joint_pose3dencodersleftankle_roll>
+        <plugin filename="libposeleftankle.so" name="poseleftankle">
+            <joint_poseleftankle_pitch>lankle_pitch</joint_poseleftankle_pitch>
+            <joint_poseleftankle_roll>lankle_roll</joint_poseleftankle_roll>
             <torque>5</torque>
-        </plugin>       -->
+        </plugin>
         
-<!--        
-        <plugin filename="libpose3dencodersknee.so" name="pose3dencodersknee">
-            <joint_pose3dencodersrightknee_pitch>right_knee</joint_pose3dencodersrightknee_pitch>
+        <plugin filename="libposerightknee.so" name="poserightknee">
+            <joint_poserightknee_pitch>right_knee</joint_poserightknee_pitch>
             <torque>5</torque>
         </plugin>
--->
+        
+        <plugin filename="libposeleftknee.so" name="poseleftknee">
+            <joint_poseleftknee_pitch>left_knee</joint_poseleftknee_pitch>
+            <torque>5</torque>
+        </plugin>
+
     </model>
 </sdf>

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt	2014-01-26 10:32:52 UTC (rev 1148)
@@ -8,36 +8,68 @@
 
 add_library(camera_nao SHARED camera_dump.cc)
 target_link_libraries(camera_nao ${GAZEBO_libraries} 
+    ${OpenCV_LIBS}
 	CameraPlugin 
+	Ice
+	IceUtil
 	JderobotInterfaces
-	colorspacesmm	
+	colorspacesmm
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES} 
 )
-    
+
+add_library(ballencoder SHARED ballred.cc)
+target_link_libraries(ballencoder ${GAZEBO_libraries} 	 
+    Ice
+	IceUtil	
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+)
+
+# NECK
 add_library(poseneck SHARED poseneck.cc)
-target_link_libraries(poseneck ${GAZEBO_libraries} 	 	
+target_link_libraries(poseneck ${GAZEBO_libraries} 	 
+    Ice
+	IceUtil	
 	JderobotInterfaces
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES} 
 )
 
+add_library(poseneckspeed SHARED poseneckspeed.cc)
+target_link_libraries(poseneckspeed ${GAZEBO_libraries} 	 
+    Ice
+	IceUtil	
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES} 
+)
+
+# SHOULDERS
 add_library(poserightshoulder SHARED poserightshoulder.cc)
 target_link_libraries(poserightshoulder ${GAZEBO_libraries} 
-    JderobotInterfaces	 	
+    Ice
+	IceUtil
+    JderobotInterfaces	
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES} 
 )
 
 add_library(poseleftshoulder SHARED poseleftshoulder.cc)
 target_link_libraries(poseleftshoulder ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
 	JderobotInterfaces
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES}
 )
 
+# ELBOWS
 add_library(poserightelbow SHARED poserightelbow.cc)
 target_link_libraries(poserightelbow ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
 	JderobotInterfaces
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES}
@@ -45,7 +77,66 @@
 
 add_library(poseleftelbow SHARED poseleftelbow.cc)
 target_link_libraries(poseleftelbow ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
 	JderobotInterfaces
 	${GAZEBO_libraries} 
 	${ZeroCIce_LIBRARIES}
 )
+
+# HIPS
+add_library(poserighthip SHARED poserighthip.cc)
+target_link_libraries(poserighthip ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+add_library(poselefthip SHARED poselefthip.cc)
+target_link_libraries(poselefthip ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+# KNEES
+add_library(poserightknee SHARED poserightknee.cc)
+target_link_libraries(poserightknee ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+add_library(poseleftknee SHARED poseleftknee.cc)
+target_link_libraries(poseleftknee ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+# ANKLES
+add_library(poserightankle SHARED poserightankle.cc)
+target_link_libraries(poserightankle ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)
+
+add_library(poseleftankle SHARED poseleftankle.cc)
+target_link_libraries(poseleftankle ${GAZEBO_libraries} 	 	
+    Ice
+	IceUtil
+	JderobotInterfaces
+	${GAZEBO_libraries} 
+	${ZeroCIce_LIBRARIES}
+)

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -65,11 +66,32 @@
         }
         //          ----------ENCODERS----------
         //GET pose3dencoders data from the ball (PAN&TILT)
+        pthread_mutex_lock(&this->mutex_ballgreenencoders);
+        
         this->ballgreen.encoders.pan = - this->ballgreen.joint_pan->GetAngle(0).Radian();
         this->ballgreen.encoders.tilt = - this->ballgreen.joint_tilt->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_ballgreenencoders);
 
         //          ----------MOTORS----------
-        if (this->ballgreen.motorsdata.pan > 0) {
+        this->ballgreen.joint_pan->SetMaxForce(0, this->stiffness);
+        this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
+        
+        pthread_mutex_lock(&this->mutex_ballgreenmotors);
+        
+        float panSpeed = this->ballgreen.encoders.pan - this->ballgreen.motorsdata.pan;
+        if ((std::abs(panSpeed) < 0.1) && (std::abs(panSpeed) > 0.001))
+            panSpeed = 0.1;
+        
+        float tiltSpeed = this->ballgreen.encoders.tilt - this->ballgreen.motorsdata.tilt;
+        if ((std::abs(tiltSpeed) < 0.1) && (std::abs(tiltSpeed) > 0.001))
+            tiltSpeed = 0.1;
+        
+        this->ballgreen.joint_pan->SetVelocity(0, panSpeed);
+        this->ballgreen.joint_tilt->SetVelocity(0, tiltSpeed);
+
+        pthread_mutex_unlock(&this->mutex_ballgreenmotors);
+/*        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);
@@ -105,7 +127,7 @@
                 this->ballgreen.joint_tilt->SetMaxForce(0, this->stiffness);
             }
         }
-
+*/
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-BallGreenEncoders.Endpoints=default -h localhost -p 9294
-BallGreenMotors.Endpoints=default -h localhost -p 9295
+BallGreenEncoders.Endpoints=default -h localhost -p 8010
+BallGreenMotors.Endpoints=default -h localhost -p 8011

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/ballgreen.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -69,6 +70,8 @@
         double stiffness;
         int count;
         int cycle;
+        
+        std::string modelPan, modelTilt;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,11 +14,12 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
-#include "ballencoder.h"
+#include "ballred.h"
 
 #define RADTODEG 57.29582790
 
@@ -68,43 +69,32 @@
         this->ballred.encoders.pan = - this->ballred.joint_pan->GetAngle(0).Radian();
         this->ballred.encoders.tilt = - this->ballred.joint_tilt->GetAngle(0).Radian();
 
+        //          ----------ENCODERS----------
+        pthread_mutex_lock(&this->mutex_ballredencoders);
+        
+        this->ballred.encoders.pan = - this->ballred.joint_pan->GetAngle(0).Radian();
+        this->ballred.encoders.tilt = - this->ballred.joint_tilt->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_ballredencoders);
+
         //          ----------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);
-            }            
-        }
+        this->ballred.joint_pan->SetMaxForce(0, this->stiffness);
+        this->ballred.joint_tilt->SetMaxForce(0, this->stiffness);
         
+        pthread_mutex_lock(&this->mutex_ballredmotors);
+        
+        float panSpeed = this->ballred.encoders.pan - this->ballred.motorsdata.pan;
+        if ((std::abs(panSpeed) < 0.1) && (std::abs(panSpeed) > 0.001))
+            panSpeed = 0.1;
+        
+        float tiltSpeed = this->ballred.encoders.tilt - this->ballred.motorsdata.tilt;
+        if ((std::abs(tiltSpeed) < 0.1) && (std::abs(tiltSpeed) > 0.001))
+            tiltSpeed = 0.1;
+        
+        this->ballred.joint_pan->SetVelocity(0, panSpeed);
+        this->ballred.joint_tilt->SetVelocity(0, tiltSpeed);
 
-        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);
-            }
-        }
+        pthread_mutex_unlock(&this->mutex_ballredmotors);
 
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
@@ -122,14 +112,14 @@
     class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseRedBall* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncoders ( gazebo::BallRed* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
         virtual ~Pose3DEncoders () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
-            pthread_mutex_lock(&pose->mutex_redballencoders);
+            pthread_mutex_lock(&pose->mutex_ballredencoders);
             
             pose3DEncodersData->x = pose->ballred.encoders.x;
             pose3DEncodersData->y = pose->ballred.encoders.y;
@@ -143,12 +133,12 @@
             pose3DEncodersData->maxTilt = pose->ballred.encoders.maxTilt;
             pose3DEncodersData->minTilt = pose->ballred.encoders.minTilt;
             
-            pthread_mutex_unlock(&pose->mutex_redballencoders);
+            pthread_mutex_unlock(&pose->mutex_ballredencoders);
 
             return pose3DEncodersData;
         }
 
-        gazebo::PoseNeck* pose;
+        gazebo::BallRed* pose;
 
     private:
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
@@ -157,14 +147,14 @@
     class Pose3DMotors : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseRedBall* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotors (gazebo::BallRed* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
         virtual ~Pose3DMotors() {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
-            pthread_mutex_lock(&pose->mutex_redballmotors);
+            pthread_mutex_lock(&pose->mutex_ballredmotors);
             
             pose3DMotorsData->x = pose->ballred.motorsdata.x;
             pose3DMotorsData->y = pose->ballred.motorsdata.y;
@@ -175,13 +165,13 @@
             pose3DMotorsData->panSpeed = pose->ballred.motorsdata.panSpeed;
             pose3DMotorsData->tiltSpeed = pose->ballred.motorsdata.tiltSpeed;
             
-            pthread_mutex_unlock(&pose->mutex_redballmotors);
+            pthread_mutex_unlock(&pose->mutex_ballredmotors);
 
             return pose3DMotorsData;
         }
 
         virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
-            pthread_mutex_lock(&pose->mutex_redballmotors);
+            pthread_mutex_lock(&pose->mutex_ballredmotors);
             
             pose3DMotorsParams->maxPan = pose->ballred.motorsparams.maxPan;
             pose3DMotorsParams->minPan = pose->ballred.motorsparams.minPan;
@@ -190,13 +180,13 @@
             pose3DMotorsParams->maxPanSpeed = pose->ballred.motorsparams.maxPanSpeed;
             pose3DMotorsParams->maxTiltSpeed = pose->ballred.motorsparams.maxTiltSpeed;
             
-            pthread_mutex_unlock(&pose->mutex_redballmotors);
+            pthread_mutex_unlock(&pose->mutex_ballredmotors);
             
             return pose3DMotorsParams;
         }
 
         virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
-            pthread_mutex_lock(&pose->mutex_redballmotors);
+            pthread_mutex_lock(&pose->mutex_ballredmotors);
             
             pose->ballred.motorsdata.x = data->x;
             pose->ballred.motorsdata.y = data->y;
@@ -207,10 +197,10 @@
             pose->ballred.motorsdata.panSpeed = data->panSpeed;
             pose->ballred.motorsdata.tiltSpeed = data->tiltSpeed;
             
-            pthread_mutex_unlock(&pose->mutex_redballmotors);
+            pthread_mutex_unlock(&pose->mutex_ballredmotors);
         }
 
-        gazebo::PoseRedBall* pose;
+        gazebo::BallRed* pose;
 
     private:
         jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
@@ -255,9 +245,9 @@
         } catch (const char* msg) {
             std::cerr << msg << std::endl;
         }
-        if (icB) {
+        if (ic) {
             try {
-                icB->destroy();
+                ic->destroy();
             } catch (const Ice::Exception& e) {
                 std::cerr << e << std::endl;
             }

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/ballred.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-BallRedEncoders.Endpoints=default -h localhost -p 9194
-BallRedMotors.Endpoints=default -h localhost -p 9195
+BallRedEncoders.Endpoints=default -h localhost -p 8000
+BallRedMotors.Endpoints=default -h localhost -p 8001

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/ballred.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/ballred.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/ballred.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_bottom.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_bottom.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_bottom.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,5 +1,5 @@
 #without registry
-CameraGazebo.Endpoints=default -h 0.0.0.0 -p 9991
+CameraGazebo.Endpoints=default -h 0.0.0.0 -p 10001
 
 #camera 1
 CameraGazebo.Camera.0.Name=cameraA

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_top.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_top.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/cam_sensor_top.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,5 +1,5 @@
 #without registry
-CameraGazebo.Endpoints=default -h 0.0.0.0 -p 9990
+CameraGazebo.Endpoints=default -h 0.0.0.0 -p 10000
 
 #camera 1
 CameraGazebo.Camera.0.Name=cameraA

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/common.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/common.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/common.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,66 +14,72 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #include "poseleftankle.h"
 
-#define RADTODEG 57.29582790
-
 namespace gazebo {
-    GZ_REGISTER_MODEL_PLUGIN(Pose3DEncodersLeftAnkle)
+    GZ_REGISTER_MODEL_PLUGIN(PoseLeftAnkle)
 
     PoseLeftAnkle::PoseLeftAnkle () {
-        pthread_mutex_init(&mutex_lefankleencoders, NULL);
-        pthread_mutex_init(&mutex_lefanklemotors, NULL);
-        this->count = 0;
+        pthread_mutex_init(&mutex_leftankleencoders, NULL);
+        pthread_mutex_init(&mutex_leftanklemotors, NULL);
         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;
+        this->modelPitch = std::string("joint_poseleftankle_pitch");
+        this->modelRoll = std::string("joint_poseleftankle_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseLeftAnkle plugin missing <" << this->modelPitch << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseLeftAnkle plugin missing <" << this->modelRoll << "> element\n";
+            
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+        
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseLeftAnkle plugin missing <" << elemPitch << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseLeftAnkle plugin missing <" << elemRoll << "> element\n";
+            
+        this->leftankle.joint_pitch = _model->GetJoint(elemPitch);
+        this->leftankle.joint_roll = _model->GetJoint(elemRoll);
 
-        this->leftankle.joint_roll = this->model->GetJoint("lankle_roll");
-        this->leftankle.joint_tilt = this->model->GetJoint("lankle_pitch");
+        this->maxPitch = (float) this->leftankle.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->leftankle.joint_pitch->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->leftankle.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->leftankle.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            gzwarn << "No torque value set for the left ankle plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_LeftAnkleICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseLeftAnkle::OnUpdate, this));
     }
 
-    void PoseLeftAnkle::Init () {}
+    void PoseLeftAnkle::Init () {
+        this->leftankle.encoders.tilt = 0.0;
+        this->leftankle.encoders.roll = 0.0;
+        
+        this->leftankle.motorsdata.tilt = 0.0;
+        this->leftankle.motorsdata.roll = 0.0;
+    }
 
     void PoseLeftAnkle::OnUpdate () {
         long totalb, totala, diff;
@@ -82,59 +88,34 @@
         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;
-        }
+        //          ----------ENCODERS----------
+        // GET pose3dencoders data from the left ankle (TILT&ROLL)
+        pthread_mutex_lock(&this->mutex_leftankleencoders);
+        
+        this->leftankle.encoders.tilt = this->leftankle.joint_pitch->GetAngle(0).Radian();
+        this->leftankle.encoders.roll = this->leftankle.joint_roll->GetAngle(0).Radian();   
+        
+        pthread_mutex_unlock(&this->mutex_leftankleencoders);
 
         //          ----------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);
-            }            
-        }
+        this->leftankle.joint_pitch->SetMaxForce(0, this->stiffness);
+        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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_leftanklemotors);
+        
+        float tiltSpeed = - this->leftankle.motorsdata.tilt - this->leftankle.encoders.tilt;
+        if ((std::abs(tiltSpeed) < 0.1) && (std::abs(tiltSpeed) > 0.001))
+            tiltSpeed = 0.1;
+        
+        float rollSpeed = - this->leftankle.motorsdata.roll - this->leftankle.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->leftankle.joint_pitch->SetVelocity(0, tiltSpeed);
+        this->leftankle.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_leftanklemotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -148,14 +129,14 @@
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersLA : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseLeftAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersLA ( gazebo::PoseLeftAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersLA () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_leftankleencoders);
@@ -183,14 +164,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsLA : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseLeftAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsLA (gazebo::PoseLeftAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsLA () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_leftanklemotors);
@@ -269,8 +250,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterLeftAnkleMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(leftankle);
-            Ice::ObjectPtr motors = new Pose3DMotors(leftankle);
+            Ice::ObjectPtr encoders = new Pose3DEncodersLA(leftankle);
+            Ice::ObjectPtr motors = new Pose3DMotorsLA(leftankle);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("LeftAnkleEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("LeftAnkleMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseLeftAnkleEncoders.Endpoints=default -h localhost -p 11996
-PoseLeftAnkleMotors.Endpoints=default -h localhost -p 11997
+PoseLeftAnkleEncoders.Endpoints=default -h localhost -p 9000
+PoseLeftAnkleMotors.Endpoints=default -h localhost -p 9001

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftankle.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -48,27 +49,28 @@
         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;
+        std::string cfgfile_leftankle;
+        pthread_mutex_t mutex_leftankleencoders;
+        pthread_mutex_t mutex_leftanklemotors;
 
-        struct lefankle_t {
-            physics::JointPtr joint_tilt, joint_pan;
-            physics::LinkPtr link_pan, link_tilt;
+        struct leftankle_t {
+            physics::JointPtr joint_pitch, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
         };
 
-        lefankle_t lefankle;
+        leftankle_t leftankle;
 
     private:
         void OnUpdate ();
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxPitch, maxRoll, minPitch, minRoll;
+        std::string modelPitch, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,14 +14,13 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #include "poseleftelbow.h"
 
-#define RADTODEG 57.29582790
-
 namespace gazebo {
 
     GZ_REGISTER_MODEL_PLUGIN(PoseLeftElbow)
@@ -29,52 +28,59 @@
     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;
+        this->modelYaw = std::string("joint_poseleftelbow_pan");
+        this->modelRoll = std::string("joint_poseleftelbow_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelYaw))
+            gzerr << "PoseLeftElbow plugin missing <" << this->modelYaw << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseLeftElbow plugin missing <" << this->modelRoll << "> element\n";
+        
+        std::string elemYaw = std::string(_sdf->GetElement(this->modelYaw)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+            
+        if (!_sdf->HasElement(elemYaw))
+            gzerr << "PoseLeftElbow plugin missing <" << elemYaw << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseLeftElbow plugin missing <" << elemRoll << "> element\n";
+            
+        this->leftelbow.joint_yaw = _model->GetJoint(elemYaw);
+        this->leftelbow.joint_roll = _model->GetJoint(elemRoll);
 
-        this->leftelbow.joint_pan = _model->GetJoint("lelbow_yaw");
-        this->leftelbow.joint_roll = _model->GetJoint("lelbow_roll");
+        this->maxYaw = (float) this->leftelbow.joint_yaw->GetUpperLimit(0).Radian();
+        this->minYaw = (float) this->leftelbow.joint_yaw->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->leftelbow.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->leftelbow.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            gzwarn << "No torque value set for the left elbow plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_LeftElbowICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseLeftElbow::OnUpdate, this));
     }
 
-    void PoseLeftElbow::Init () {}
+    void PoseLeftElbow::Init () {
+        this->leftelbow.encoders.pan = 0.0;
+        this->leftelbow.encoders.roll = 0.0;
+        
+        this->leftelbow.motorsdata.pan = 0.0;
+        this->leftelbow.motorsdata.roll = 0.0;
+    }
 
     void PoseLeftElbow::OnUpdate () {
         long totalb, totala, diff;
@@ -83,59 +89,34 @@
         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;
+        //GET pose3dencoders data from the left elbow (PAN&ROLL)
+        pthread_mutex_lock(&this->mutex_leftelbowencoders);
         
-        this->leftelbow.encoders.pan = - this->leftelbow.joint_pan->GetAngle(0).Radian();
-        this->leftelbow.encoders.roll = - this->leftelbow.joint_roll->GetAngle(0).Radian();
+        this->leftelbow.encoders.pan = this->leftelbow.joint_yaw->GetAngle(0).Radian();
+        this->leftelbow.encoders.roll = this->leftelbow.joint_roll->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_leftelbowencoders);
 
         //          ----------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);
-            }            
-        }
+        this->leftelbow.joint_yaw->SetMaxForce(0, this->stiffness);
+        this->leftelbow.joint_roll->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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_leftelbowmotors);
+        
+        float yawSpeed = - this->leftelbow.motorsdata.pan - this->leftelbow.encoders.pan;
+        if ((std::abs(yawSpeed) < 0.1) && (std::abs(yawSpeed) > 0.001))
+            yawSpeed = 0.1;
+        
+        float rollSpeed = - this->leftelbow.motorsdata.roll - this->leftelbow.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->leftelbow.joint_yaw->SetVelocity(0, yawSpeed);
+        this->leftelbow.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_leftelbowmotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -145,18 +126,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersLE : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseLeftElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersLE ( gazebo::PoseLeftElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersLE () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_leftelbowencoders);
@@ -184,14 +164,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsLE : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseLeftElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsLE (gazebo::PoseLeftElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsLE () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_leftelbowmotors);
@@ -248,7 +228,6 @@
     };
 
     void* thread_LeftElbowICE ( void* v ) {
-
         gazebo::PoseLeftElbow* leftelbow = (gazebo::PoseLeftElbow*)v;
         char* name = (char*) leftelbow->cfgfile_leftelbow.c_str();
         Ice::CommunicatorPtr ic;
@@ -261,17 +240,17 @@
 
             prop = ic->getProperties();
             std::string EndpointsEncoders = prop->getProperty("PoseLeftElbowEncoders.Endpoints");
-            std::cout << "PoseNeckEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::cout << "PoseLeftElbowEncoders Endpoints > " << EndpointsEncoders << std::endl;
             std::string EndpointsMotors = prop->getProperty("PoseLeftElbowMotors.Endpoints");
-            std::cout << "PoseNeckMotors Endpoints > " << EndpointsMotors << std::endl;
+            std::cout << "PoseLeftElbowMotors 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);
+            Ice::ObjectPtr encoders = new Pose3DEncodersLE(leftelbow);
+            Ice::ObjectPtr motors = new Pose3DMotorsLE(leftelbow);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("LeftElbowEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("LeftElbowMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseLeftElbowEncoders.Endpoints=default -h localhost -p 5998
-PoseLeftElbowMotors.Endpoints=default -h localhost -p 5999
+PoseLeftElbowEncoders.Endpoints=default -h localhost -p 9060
+PoseLeftElbowMotors.Endpoints=default -h localhost -p 9061

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_leftelbowmotors;
 
         struct leftelbow_t {
-            physics::JointPtr joint_pan, joint_roll;
-            physics::LinkPtr link_pan, link_roll;
+            physics::JointPtr joint_yaw, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxYaw, maxRoll, minYaw, minRoll;
+        std::string modelYaw, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,66 +14,72 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->modelPitch = std::string("joint_poseleftshoulder_tilt");
+        this->modelRoll = std::string("joint_poseleftshoulder_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseLeftShoulder plugin missing <" << this->modelPitch << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseLeftShoulder plugin missing <" << this->modelRoll << "> element\n";
+         
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseLeftShoulder plugin missing <" << elemPitch << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseLeftShoulder plugin missing <" << elemRoll << "> element\n";
+            
+        this->leftshoulder.joint_pitch = _model->GetJoint(elemPitch);
+        this->leftshoulder.joint_roll = _model->GetJoint(elemRoll);
 
-        this->leftshoulder.joint_roll = _model->GetJoint("lshoulder_roll");
-        this->leftshoulder.joint_tilt = _model->GetJoint("lshoulder_pitch");
+        this->maxPitch = (float) this->leftshoulder.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->leftshoulder.joint_pitch->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->leftshoulder.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->leftshoulder.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // 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;
         }
+        
+        pthread_t thr_gui;
+        pthread_create(&thr_gui, NULL, &thread_LeftShoulderICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseLeftShoulder::OnUpdate, this));
     }
 
-    void PoseLeftShoulder::Init () {}
+    void PoseLeftShoulder::Init () {
+        this->leftshoulder.encoders.tilt = 0.0;
+        this->leftshoulder.encoders.roll = 0.0;
+        
+        this->leftshoulder.motorsdata.tilt = 0.0;
+        this->leftshoulder.motorsdata.roll = 0.0;
+    }
 
     void PoseLeftShoulder::OnUpdate() {
         long totalb, totala, diff;
@@ -82,59 +88,34 @@
         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;
+        // GET pose3dencoders data from the left shoulder (ROLL&TILT)
+        pthread_mutex_lock(&this->mutex_leftshoulderencoders);
         
-        this->leftshoulder.encoders.tilt = - this->leftshoulder.joint_tilt->GetAngle(0).Radian();
-        this->leftshoulder.encoders.roll = - this->leftshoulder.joint_roll->GetAngle(0).Radian();
+        this->leftshoulder.encoders.tilt = this->leftshoulder.joint_pitch->GetAngle(0).Radian();
+        this->leftshoulder.encoders.roll = this->leftshoulder.joint_roll->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_leftshoulderencoders);
 
         //          ----------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);
-            }            
-        }
+        this->leftshoulder.joint_pitch->SetMaxForce(0, this->stiffness);
+        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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_leftshouldermotors);
+        
+        float pitchSpeed = - this->leftshoulder.motorsdata.tilt - this->leftshoulder.encoders.tilt;
+        if ((std::abs(pitchSpeed) < 0.1) && (std::abs(pitchSpeed) > 0.001))
+            pitchSpeed = 0.1;
+        
+        float rollSpeed = - this->leftshoulder.motorsdata.roll - this->leftshoulder.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->leftshoulder.joint_pitch->SetVelocity(0, pitchSpeed);
+        this->leftshoulder.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_leftshouldermotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -144,18 +125,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersLS : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseLeftShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersLS ( gazebo::PoseLeftShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersLS () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_leftshoulderencoders);
@@ -183,14 +163,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsLS : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseLeftShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsLS (gazebo::PoseLeftShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsLS () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_leftshouldermotors);
@@ -247,7 +227,6 @@
     };
 
     void* thread_LeftShoulderICE ( void* v ) {
-
         gazebo::PoseLeftShoulder* leftshoulder = (gazebo::PoseLeftShoulder*)v;
         char* name = (char*) leftshoulder->cfgfile_leftshoulder.c_str();
         Ice::CommunicatorPtr ic;
@@ -269,8 +248,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterLeftShoulderMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(leftshoulder);
-            Ice::ObjectPtr motors = new Pose3DMotors(leftshoulder);
+            Ice::ObjectPtr encoders = new Pose3DEncodersLS(leftshoulder);
+            Ice::ObjectPtr motors = new Pose3DMotorsLS(leftshoulder);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("LeftShoulderEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("LeftShoulderMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseLeftShoulderEncoders.Endpoints=default -h localhost -p 7996
-PoseLeftShoulderMotors.Endpoints=default -h localhost -p 7997
+PoseLeftShoulderEncoders.Endpoints=default -h localhost -p 9080
+PoseLeftShoulderMotors.Endpoints=default -h localhost -p 9081

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_leftshouldermotors;
 
         struct leftshoulder_t {
-            physics::JointPtr joint_tilt, joint_roll;
-            physics::LinkPtr link_tilt, link_roll;
+            physics::JointPtr joint_pitch, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxPitch, maxRoll, minPitch, minRoll;
+        std::string modelPitch, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,68 +14,74 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->modelPan = std::string("joint_poseneck_pan");
+        this->modelTilt = std::string("joint_poseneck_tilt");
+        this->speed = 0.1;
 
         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";
+        if (!_sdf->HasElement(this->modelPan))
+            gzerr << "PoseNeck plugin missing <" << this->modelPan << "> element first\n";
+        if (!_sdf->HasElement(this->modelTilt))
+            gzerr << "PoseNeck plugin missing <" << this->modelTilt << "> element first\n";
+        
+        std::string elemPan = std::string(_sdf->GetElement(this->modelPan)->GetValueString());
+        std::string elemTilt = std::string(_sdf->GetElement(this->modelTilt)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPan))
+            gzerr << "PoseNeck plugin missing <" << elemPan << "> element\n";
+        if (!_sdf->HasElement(elemTilt))
+            gzerr << "PoseNeck plugin missing <" << elemTilt << "> element\n";
+            
+        this->neck.joint_pan = _model->GetJoint(elemPan);
+        this->neck.joint_tilt = _model->GetJoint(elemTilt);
 
-        this->neck.joint_pan = _model->GetJoint("neck_pan");
-        this->neck.joint_tilt = _model->GetJoint("neck_tilt");
+        this->neck.motorsparams.maxPan = (float) this->neck.joint_pan->GetUpperLimit(0).Radian();
+        this->neck.motorsparams.minPan = (float) this->neck.joint_pan->GetLowerLimit(0).Radian();
+        this->neck.motorsparams.maxTilt = (float) this->neck.joint_tilt->GetUpperLimit(0).Radian();
+        this->neck.motorsparams.minTilt = (float) this->neck.joint_tilt->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            gzwarn << "No torque value set for the neck plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_NeckICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
-            boost::bind(&PoseNeck::OnUpdate, this));
+                                    boost::bind(&PoseNeck::OnUpdate, this));
+    }
 
+    void PoseNeck::Init () {
+        this->neck.encoders.pan = 0.0;
+        this->neck.encoders.tilt = 0.0;
+        
+        this->neck.motorsdata.pan = 0.0;
+        this->neck.motorsdata.tilt = 0.0;
     }
 
-    void PoseNeck::Init () {}
-
     void PoseNeck::OnUpdate () {
         long totalb, totala, diff;
         struct timeval a, b;
@@ -83,56 +89,34 @@
         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;
+        // GET pose data from the neck (PAN&TILT)
+        pthread_mutex_lock(&this->mutex_neckencoders);
         
+        this->neck.encoders.pan = this->neck.joint_pan->GetAngle(0).Radian();
+        this->neck.encoders.tilt = this->neck.joint_tilt->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_neckencoders);
+        
         //          ----------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);
-            }            
-        }
+        this->neck.joint_pan->SetMaxForce(0, this->stiffness);
+        this->neck.joint_tilt->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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_neckmotors);
+        
+        float panSpeed = - this->neck.motorsdata.pan - this->neck.encoders.pan;
+        if ((std::abs(panSpeed) < 0.1) && (std::abs(panSpeed) > 0.001))
+            panSpeed = 0.1;
+        
+        float tiltSpeed = - this->neck.motorsdata.tilt - this->neck.encoders.tilt;
+        if ((std::abs(tiltSpeed) < 0.1) && (std::abs(tiltSpeed) > 0.001))
+            tiltSpeed = 0.1;
+        
+        this->neck.joint_pan->SetVelocity(0, panSpeed);
+        this->neck.joint_tilt->SetVelocity(0, tiltSpeed);
 
+        pthread_mutex_unlock(&this->mutex_neckmotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -142,18 +126,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersN : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersN ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersN () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_neckencoders);
@@ -181,14 +164,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsN : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsN (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsN () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_neckmotors);
@@ -205,7 +188,6 @@
             pthread_mutex_unlock(&pose->mutex_neckmotors);
 
             return pose3DMotorsData;
-
         }
 
         virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
@@ -236,7 +218,6 @@
             pose->neck.motorsdata.tiltSpeed = data->tiltSpeed;
             
             pthread_mutex_unlock(&pose->mutex_neckmotors);
-
         }
 
         gazebo::PoseNeck* pose;
@@ -247,7 +228,6 @@
     };
 
     void* thread_NeckICE ( void* v ) {
-
         gazebo::PoseNeck* neck = (gazebo::PoseNeck*)v;
         char* name = (char*) neck->cfgfile_neck.c_str();
         Ice::CommunicatorPtr ic;
@@ -269,8 +249,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterNeckMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(neck);
-            Ice::ObjectPtr motors = new Pose3DMotors(neck);
+            Ice::ObjectPtr encoders = new Pose3DEncodersN(neck);
+            Ice::ObjectPtr motors = new Pose3DMotorsN(neck);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("NeckEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("NeckMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseNeckMotors.Endpoints=default -h localhost -p 9994
-PoseNeckEncoders.Endpoints=default -h localhost -p 9995
+PoseNeckMotors.Endpoints=default -h localhost -p 9110
+PoseNeckEncoders.Endpoints=default -h localhost -p 9111

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_neckmotors;
 
         struct neck_t {
-            physics::JointPtr joint_tilt, joint_pan;
-            physics::LinkPtr link_pan, link_tilt;
+            physics::JointPtr joint_pan, joint_tilt;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -66,9 +66,10 @@
         void OnUpdate ();
         
         event::ConnectionPtr updateConnection;
-        double stiffness;
-        int count;
+        double stiffness, speed;
         int cycle;
+        
+        std::string modelPan, modelTilt;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,66 +14,74 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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->neckspeed.motorsdata.panSpeed = 0.0;
+        this->neckspeed.motorsdata.tiltSpeed = 0.0;
         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;
+        this->modelPan = std::string("joint_poseneckspeed_pan");
+        this->modelTilt = std::string("joint_poseneckspeed_tilt");
 
         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";
+        if (!_sdf->HasElement(this->modelPan))
+            gzerr << "PoseNeckSpeed plugin missing <" << this->modelPan << "> element\n";
+        if (!_sdf->HasElement(this->modelTilt))
+            gzerr << "PoseNeckSpeed plugin missing <" << this->modelTilt << "> element\n";
+            
+        std::string elemPan = std::string(_sdf->GetElement(this->modelPan)->GetValueString());
+        std::string elemTilt = std::string(_sdf->GetElement(this->modelTilt)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPan))
+            gzerr << "PoseNeck plugin missing <" << elemPan << "> element\n";
+        if (!_sdf->HasElement(elemTilt))
+            gzerr << "PoseNeck plugin missing <" << elemTilt << "> element\n";
+            
+        this->neckspeed.joint_pan = _model->GetJoint(elemPan);
+        this->neckspeed.joint_tilt = _model->GetJoint(elemTilt);
 
-        this->neckspeed.joint_pan = _model->GetJoint("neck_pan");
-        this->neckspeed.joint_tilt = _model->GetJoint("neck_tilt");
+        this->neckspeed.motorsparams.maxPan = (float) this->neckspeed.joint_pan->GetUpperLimit(0).Radian();
+        this->neckspeed.motorsparams.minPan = (float) this->neckspeed.joint_pan->GetLowerLimit(0).Radian();
+        this->neckspeed.motorsparams.maxTilt = (float) this->neckspeed.joint_tilt->GetUpperLimit(0).Radian();
+        this->neckspeed.motorsparams.minTilt = (float) this->neckspeed.joint_tilt->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque        
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the neck speed.\n";
+            gzwarn << "No torque value set for the neck speed plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_NeckSpeedICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseNeckSpeed::OnUpdate, this));
     }
 
-    void PoseNeckSpeed::Init () {}
+    void PoseNeckSpeed::Init () {
+        this->neckspeed.joint_pan->SetVelocity(0, this->neckspeed.motorsdata.panSpeed);
+        this->neckspeed.joint_pan->SetMaxForce(0, this->stiffness);
+        
+        this->neckspeed.joint_tilt->SetVelocity(0, this->neckspeed.motorsdata.tiltSpeed);
+        this->neckspeed.joint_tilt->SetMaxForce(0, this->stiffness);
+    }
 
     void PoseNeckSpeed::OnUpdate () {
         long totalb, totala, diff;
@@ -81,22 +89,27 @@
 
         gettimeofday(&a, NULL);
         totala = a.tv_sec * 1000000 + a.tv_usec;
+        
+        //          ----------ENCODERS----------
+        // GET pose data from the neck (PAN&TILT)
+        pthread_mutex_lock(&this->mutex_neckspeedencoders);
 
-        if (this->count == 0) {
-            this->count++;
-            pthread_t thr_ice;
-            pthread_create(&thr_ice, NULL, &thread_NeckSpeedICE, (void*) this);
-        }
+        this->neckspeed.encoders.pan = this->neckspeed.joint_pan->GetAngle(0).Radian();
+        this->neckspeed.encoders.tilt = this->neckspeed.joint_tilt->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_neckspeedencoders);
   
-        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.tiltSpeed);
-            this->neckspeed.joint_tilt->SetMaxForce(0, this->stiffness);
-        }
+        //          ----------MOTORS----------
+        this->neckspeed.joint_pan->SetMaxForce(0, this->stiffness);
+        this->neckspeed.joint_tilt->SetMaxForce(0, this->stiffness);
+        
+        pthread_mutex_lock(&this->mutex_neckspeedmotors);
+     
+        this->neckspeed.joint_pan->SetVelocity(0, -this->neckspeed.motorsdata.panSpeed);
+        this->neckspeed.joint_tilt->SetVelocity(0, this->neckspeed.motorsdata.tiltSpeed);
 
+        pthread_mutex_unlock(&this->mutex_neckspeedmotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -106,18 +119,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersNS : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseNeck* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersNS ( gazebo::PoseNeckSpeed* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersNS () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_neckspeedencoders);
@@ -139,20 +151,20 @@
             return pose3DEncodersData;
         }
 
-        gazebo::PoseNeck* pose;
+        gazebo::PoseNeckSpeed* pose;
 
     private:
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsNS : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseNeck* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsNS (gazebo::PoseNeckSpeed* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsNS () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_neckspeedmotors);
@@ -169,7 +181,6 @@
             pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
 
             return pose3DMotorsData;
-
         }
 
         virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
@@ -200,10 +211,9 @@
             pose->neckspeed.motorsdata.tiltSpeed = data->tiltSpeed;
             
             pthread_mutex_unlock(&pose->mutex_neckspeedmotors);
-
         }
 
-        gazebo::PoseNeck* pose;
+        gazebo::PoseNeckSpeed* pose;
 
     private:
         jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
@@ -211,7 +221,6 @@
     };
 
     void* thread_NeckSpeedICE ( void* v ) {
-
         gazebo::PoseNeckSpeed* neckspeed = (gazebo::PoseNeckSpeed*)v;
         char* name = (char*) neckspeed->cfgfile_neckspeed.c_str();
         Ice::CommunicatorPtr ic;
@@ -233,8 +242,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterNeckSpeedMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(neckspeed);
-            Ice::ObjectPtr motors = new Pose3DMotors(neckspeed);
+            Ice::ObjectPtr encoders = new Pose3DEncodersNS(neckspeed);
+            Ice::ObjectPtr motors = new Pose3DMotorsNS(neckspeed);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("NeckSpeedEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("NeckSpeedMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseNeckSpeedMotors.Endpoints=default -h localhost -p 12994
-PoseNeckSpeedEncoders.Endpoints=default -h localhost -p 12995
+PoseNeckSpeedMotors.Endpoints=default -h localhost -p 9100
+PoseNeckSpeedEncoders.Endpoints=default -h localhost -p 9101

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneckspeed.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_neckspeedmotors;
 
         struct neckspeed_t {
-            physics::JointPtr joint_tilt, joint_pan;
-            physics::LinkPtr link_pan, link_tilt;
+            physics::JointPtr joint_pan, joint_tilt;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,9 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        std::string modelPan, modelTilt;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,66 +14,72 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->cfgfile_rightankle = std::string("--Ice.Config=poserightankle.cfg");
+        this->modelPitch = std::string("joint_poserightankle_pitch");
+        this->modelRoll = std::string("joint_poserightankle_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseRightAnkle plugin missing <" << this->modelPitch << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseRightAnkle plugin missing <" << this->modelRoll << "> element\n";
+            
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+        
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseRightAnkle plugin missing <" << elemPitch << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseRightAnkle plugin missing <" << elemRoll << "> element\n";
+            
+        this->rightankle.joint_pitch = _model->GetJoint(elemPitch);
+        this->rightankle.joint_roll = _model->GetJoint(elemRoll);
 
-        this->rightankle.joint_roll = _model->GetJoint("rankle_roll");
-        this->rightankle.joint_tilt = _model->GetJoint("rankle_pitch");
+        this->maxPitch = (float) this->rightankle.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->rightankle.joint_pitch->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->rightankle.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->rightankle.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            gzwarn << "No torque value set for the left ankle plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_RightAnkleICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseRightAnkle::OnUpdate, this));
     }
 
-    void PoseRightAnkle::Init () {}
+    void PoseRightAnkle::Init () {
+        this->rightankle.encoders.tilt = 0.0;
+        this->rightankle.encoders.roll = 0.0;
+        
+        this->rightankle.motorsdata.tilt = 0.0;
+        this->rightankle.motorsdata.roll = 0.0;
+    }
 
     void PoseRightAnkle::OnUpdate () {
         long totalb, totala, diff;
@@ -82,59 +88,34 @@
         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;
-        }
+        //          ----------ENCODERS----------
+        // GET pose3dencoders data from the left ankle (TILT&ROLL)
+        pthread_mutex_lock(&this->mutex_rightankleencoders);
+        
+        this->rightankle.encoders.tilt = this->rightankle.joint_pitch->GetAngle(0).Radian();
+        this->rightankle.encoders.roll = this->rightankle.joint_roll->GetAngle(0).Radian();   
+        
+        pthread_mutex_unlock(&this->mutex_rightankleencoders);
 
         //          ----------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);
-            }            
-        }
+        this->rightankle.joint_pitch->SetMaxForce(0, this->stiffness);
+        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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_rightanklemotors);
+        
+        float pitchSpeed = - this->rightankle.motorsdata.tilt - this->rightankle.encoders.tilt;
+        if ((std::abs(pitchSpeed) < 0.1) && (std::abs(pitchSpeed) > 0.001))
+            pitchSpeed = 0.1;
+        
+        float rollSpeed = - this->rightankle.motorsdata.roll - this->rightankle.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->rightankle.joint_pitch->SetVelocity(0, pitchSpeed);
+        this->rightankle.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_rightanklemotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -148,14 +129,14 @@
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersRA : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseRightAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersRA ( gazebo::PoseRightAnkle* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersRA () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightankleencoders);
@@ -183,14 +164,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsRA : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseRightAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsRA (gazebo::PoseRightAnkle* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsRA () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightanklemotors);
@@ -269,8 +250,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterRightAnkleMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(rightankle);
-            Ice::ObjectPtr motors = new Pose3DMotors(rightankle);
+            Ice::ObjectPtr encoders = new Pose3DEncodersRA(rightankle);
+            Ice::ObjectPtr motors = new Pose3DMotorsRA(rightankle);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("RightAnkleEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("RightAnkleMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseRightAnkleEncoders.Endpoints=default -h localhost -p 10996
-PoseRightAnkleMotors.Endpoints=default -h localhost -p 10997
+PoseRightAnkleEncoders.Endpoints=default -h localhost -p 9010
+PoseRightAnkleMotors.Endpoints=default -h localhost -p 9011

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightankle.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_rightanklemotors;
 
         struct rightankle_t {
-            physics::JointPtr joint_tilt, joint_pan;
-            physics::LinkPtr link_pan, link_tilt;
+            physics::JointPtr joint_pitch, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxPitch, maxRoll, minPitch, minRoll;
+        std::string modelPitch, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,66 +14,75 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->modelYaw = std::string("joint_poserightelbow_yaw");
+        this->modelRoll = std::string("joint_poserightelbow_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelYaw))
+            gzerr << "PoseRightElbow plugin missing <" << this->modelYaw << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseRightElbow plugin missing <" << this->modelRoll << "> element\n";
+            
+        std::string elemPan = std::string(_sdf->GetElement(this->modelYaw)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPan))
+            gzerr << "PoseRightElbow plugin missing <" << elemPan << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseRightElbow plugin missing <" << elemRoll << "> element\n";
+            
+        this->rightelbow.joint_yaw = _model->GetJoint(elemPan);
+        this->rightelbow.joint_roll = _model->GetJoint(elemRoll);
+        
+//        this->rightelbow.link_pan = this->rightelbow.joint_yaw->GetParent();
+//        this->rightelbow.link_roll = this->rightelbow.joint_roll->GetParent();
 
-        this->rightelbow.joint_pan = _model->GetJoint("relbow_yaw");
-        this->rightelbow.joint_roll = _model->GetJoint("relbow_roll");
+        this->maxYaw = (float) this->rightelbow.joint_yaw->GetUpperLimit(0).Radian();
+        this->minYaw = (float) this->rightelbow.joint_yaw->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->rightelbow.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->rightelbow.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the rigth elbow.\n";
+            gzwarn << "No torque value set for the rigth elbow plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_RightElbowICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseRightElbow::OnUpdate, this));
     }
 
-    void PoseRightElbow::Init () {}
+    void PoseRightElbow::Init () {
+        this->rightelbow.encoders.pan = 0.0;
+        this->rightelbow.encoders.roll = 0.0;
+        
+        this->rightelbow.motorsdata.pan = 0.0;
+        this->rightelbow.motorsdata.roll = 0.0;
+    }
 
     void PoseRightElbow::OnUpdate () {
         long totalb, totala, diff;
@@ -82,59 +91,34 @@
         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;
+        // GET pose3dencoders data from the right elbow (PAN&ROLL)
+        pthread_mutex_lock(&this->mutex_rightelbowencoders);
         
-        this->rightelbow.encoders.pan = - this->rightelbow.joint_pan->GetAngle(0).Radian();
-        this->rightelbow.encoders.roll = - this->rightelbow.joint_roll->GetAngle(0).Radian();
+        this->rightelbow.encoders.pan = this->rightelbow.joint_yaw->GetAngle(0).Radian();
+        this->rightelbow.encoders.roll = this->rightelbow.joint_roll->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_rightelbowencoders);
 
         //          ----------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);
-            }            
-        }
+        this->rightelbow.joint_yaw->SetMaxForce(0, this->stiffness);
+        this->rightelbow.joint_roll->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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_rightelbowmotors);
+        
+        float yawSpeed = - this->rightelbow.motorsdata.pan - this->rightelbow.encoders.pan;
+        if ((std::abs(yawSpeed) < 0.1) && (std::abs(yawSpeed) > 0.001))
+            yawSpeed = 0.1;
+        
+        float rollSpeed = - this->rightelbow.motorsdata.roll - this->rightelbow.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->rightelbow.joint_yaw->SetVelocity(0, yawSpeed);
+        this->rightelbow.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_rightelbowmotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -144,18 +128,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersRE : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseRightElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersRE ( gazebo::PoseRightElbow* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersRE () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightelbowencoders);
@@ -183,14 +166,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsRE : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseRightElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsRE (gazebo::PoseRightElbow* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsRE () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightelbowmotors);
@@ -247,7 +230,6 @@
     };
 
     void* thread_RightElbowICE ( void* v ) {
-
         gazebo::PoseRightElbow* rightelbow = (gazebo::PoseRightElbow*)v;
         char* name = (char*) rightelbow->cfgfile_rightelbow.c_str();
         Ice::CommunicatorPtr ic;
@@ -269,8 +251,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterRightElbowMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(rightelbow);
-            Ice::ObjectPtr motors = new Pose3DMotors(rightelbow);
+            Ice::ObjectPtr encoders = new Pose3DEncodersRE(rightelbow);
+            Ice::ObjectPtr motors = new Pose3DMotorsRE(rightelbow);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("RightElbowEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("RightElbowMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseRightElbowEncoders.Endpoints=default -h localhost -p 9998
-PoseRightElbowMotors.Endpoints=default -h localhost -p 9999
+PoseRightElbowEncoders.Endpoints=default -h localhost -p 9070
+PoseRightElbowMotors.Endpoints=default -h localhost -p 9071

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_rightelbowmotors;
 
         struct rightelbow_t {
-            physics::JointPtr joint_pan, joint_roll;
-            physics::LinkPtr link_pan, link_roll;
+            physics::JointPtr joint_yaw, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxYaw, maxRoll, minYaw, minRoll;
+        std::string modelYaw, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,73 +14,83 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->modelYaw = std::string("joint_poserighthip_yaw");
+        this->modelPitch = std::string("joint_poserighthip_pitch");
+        this->modelRoll = std::string("joint_poserighthip_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelYaw))
+            gzerr << "PoseRightElbow plugin missing <" << this->modelYaw << "> element\n";
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseRightElbow plugin missing <" << this->modelPitch << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseRightElbow plugin missing <" << this->modelRoll << "> element\n";
+            
+        std::string elemYaw = std::string(_sdf->GetElement(this->modelYaw)->GetValueString());
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+            
+        if (!_sdf->HasElement(elemYaw))
+            gzerr << "PoseRightElbow plugin missing <" << elemYaw << "> element\n";
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseRightElbow plugin missing <" << elemPitch << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseRightElbow plugin missing <" << elemRoll << "> element\n";
+            
+        this->righthip.joint_yaw = _model->GetJoint(elemYaw);
+        this->righthip.joint_pitch = _model->GetJoint(elemPitch);
+        this->righthip.joint_roll = _model->GetJoint(elemRoll);
 
-        this->righthip.joint_pan = _model->GetJoint("rhip_yawpitch");
-        this->righthip.joint_tilt = _model->GetJoint("rhip_pitch");
-        this->righthip.joint_roll = _model->GetJoint("rhip_roll");
+        this->maxYaw = (float) this->righthip.joint_yaw->GetUpperLimit(0).Radian();
+        this->minYaw = (float) this->righthip.joint_yaw->GetLowerLimit(0).Radian();
+        this->maxPitch = (float) this->righthip.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->righthip.joint_pitch->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->righthip.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->righthip.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque       
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the right hip.\n";
+            gzwarn << "No torque value set for the right hip plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_RightHipICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseRightHip::OnUpdate, this));
     }
 
-    void PoseRightHip::Init () {}
+    void PoseRightHip::Init () {
+        this->righthip.encoders.pan = 0.0;
+        this->righthip.encoders.tilt = 0.0;
+        this->righthip.encoders.roll = 0.0;
+        
+        this->righthip.motorsdata.pan = 0.0;
+        this->righthip.motorsdata.tilt = 0.0;
+        this->righthip.motorsdata.roll = 0.0;
+    }
 
     void PoseRightHip::OnUpdate () {
         long totalb, totala, diff;
@@ -88,73 +98,42 @@
 
         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;
+        // GET pose3dencoders data from the right elbow (PAN&ROLL)
+        pthread_mutex_lock(&this->mutex_righthipencoders);
         
-        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);
-            }
-        }
+        this->righthip.encoders.pan = this->righthip.joint_yaw->GetAngle(0).Radian();
+        this->righthip.encoders.tilt = this->righthip.joint_pitch->GetAngle(0).Radian();
+        this->righthip.encoders.roll = this->righthip.joint_roll->GetAngle(0).Radian();
         
-        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);
-            }
-        }
+        pthread_mutex_unlock(&this->mutex_righthipencoders);
+
+        //          ----------MOTORS----------
+        this->righthip.joint_yaw->SetMaxForce(0, this->stiffness);
+        this->righthip.joint_pitch->SetMaxForce(0, this->stiffness);
+        this->righthip.joint_roll->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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_righthipmotors);
+        
+        float yawSpeed = - this->righthip.motorsdata.pan - this->righthip.encoders.pan;
+        if ((std::abs(yawSpeed) < 0.1) && (std::abs(yawSpeed) > 0.001))
+            yawSpeed = 0.1;
+        
+        float pitchSpeed = - this->righthip.motorsdata.tilt - this->righthip.encoders.tilt;
+        if ((std::abs(pitchSpeed) < 0.1) && (std::abs(pitchSpeed) > 0.001))
+            pitchSpeed = 0.1;
+            
+        float rollSpeed = - this->righthip.motorsdata.roll - this->righthip.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->righthip.joint_yaw->SetVelocity(0, yawSpeed);
+        this->righthip.joint_pitch->SetVelocity(0, pitchSpeed);
+        this->righthip.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_righthipmotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -168,14 +147,14 @@
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersRH : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseRightHip* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersRH ( gazebo::PoseRightHip* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersRH () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_righthipencoders);
@@ -203,14 +182,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsRH : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseRightHip* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsRH (gazebo::PoseRightHip* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsRH () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_righthipmotors);
@@ -281,16 +260,16 @@
             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::string EndpointsMotors = prop->getProperty("PoseRightHipMotors.Endpoints");
             std::cout << "PoseRightHipMotors Endpoints > " << EndpointsMotors << std::endl;
 
             Ice::ObjectAdapterPtr AdapterEncoders =
                     ic->createObjectAdapterWithEndpoints("AdapterRightHipEncoders", EndpointsEncoders);
             Ice::ObjectAdapterPtr AdapterMotors =
-                    ic->createObjectAdapterWithEndpoints("AdapterRightHipkMotors", EndpointsMotors);
+                    ic->createObjectAdapterWithEndpoints("AdapterRightHipMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(righthip);
-            Ice::ObjectPtr motors = new Pose3DMotors(righthip);
+            Ice::ObjectPtr encoders = new Pose3DEncodersRH(righthip);
+            Ice::ObjectPtr motors = new Pose3DMotorsRH(righthip);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("RightHipEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("RightHipMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseRightHipEncoders.Endpoints=default -h localhost -p 8996
-PoseRightHipMotors.Endpoints=default -h localhost -p 8997
+PoseRightHipEncoders.Endpoints=default -h localhost -p 9050
+PoseRightHipMotors.Endpoints=default -h localhost -p 9051

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_righthipmotors;
 
         struct righthip_t {
-            physics::JointPtr joint_tilt, joint_pan;
-            physics::LinkPtr link_pan, link_tilt;
+            physics::JointPtr joint_yaw, joint_pitch, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxYaw, maxPitch, maxRoll, minYaw, minPitch, minRoll;
+        std::string modelYaw, modelPitch, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,59 +14,61 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->modelPitch = std::string("joint_poserightknee_pitch");
 
         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";
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseRightKnee plugin missing <" << this->modelPitch << "> element\n";
+            
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseRightKnee plugin missing <" << elemPitch << "> element\n";
+            
+        this->rightknee.joint_pitch = _model->GetJoint(elemPitch);
 
-        this->rightknee.joint_tilt = _model->GetJoint("right_knee");
+        this->maxPitch = (float) this->rightknee.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->rightknee.joint_pitch->GetLowerLimit(0).Radian();
 
-        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        
+        // Load torque
         if (_sdf->HasElement("torque"))
             this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
         else {
-            gzwarn << "No torque value set for the DiffDrive plugin.\n";
+            gzwarn << "No torque value set for the right knee plugin.\n";
             this->stiffness = 5.0;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_RightKneeICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                     boost::bind(&PoseRightKnee::OnUpdate, this));
     }
 
-    void PoseRightKnee::Init () {}
+    void PoseRightKnee::Init () {
+        this->rightknee.encoders.tilt = 0.0;
+        
+        this->rightknee.motorsdata.tilt = 0.0;
+    }
 
     void PoseRightKnee::OnUpdate () {
         long totalb, totala, diff;
@@ -75,35 +77,27 @@
         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;
+        // GET pose3dencoders data from the right elbow (PAN&ROLL)
+        pthread_mutex_lock(&this->mutex_rightkneeencoders);
+        
+        this->rightknee.encoders.tilt = this->rightknee.joint_pitch->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_rightkneeencoders);
 
         //          ----------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);
-            }
-        }
+        this->rightknee.joint_pitch->SetMaxForce(0, this->stiffness);
+        
+        pthread_mutex_lock(&this->mutex_rightkneemotors);
+        
+        float pitchSpeed = - this->rightknee.motorsdata.tilt - this->rightknee.encoders.tilt;
+        if ((std::abs(pitchSpeed) < 0.1) && (std::abs(pitchSpeed) > 0.001))
+            pitchSpeed = 0.1;
+        
+        this->rightknee.joint_pitch->SetVelocity(0, pitchSpeed);
 
+        pthread_mutex_unlock(&this->mutex_rightkneemotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -113,18 +107,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersRK : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseRightKnee* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersRK ( gazebo::PoseRightKnee* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersRK () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightkneeencoders);
@@ -152,14 +145,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsRK : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseRightKnee* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsRK (gazebo::PoseRightKnee* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsRK () {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightkneemotors);
@@ -238,8 +231,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterRightKneeMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(rightknee);
-            Ice::ObjectPtr motors = new Pose3DMotors(rightknee);
+            Ice::ObjectPtr encoders = new Pose3DEncodersRK(rightknee);
+            Ice::ObjectPtr motors = new Pose3DMotorsRK(rightknee);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("RightKneeEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("RightKneeMotors"));
@@ -260,7 +253,5 @@
                 std::cerr << e << std::endl;
             }
         }
-
     }
-
 }

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseRightKneeEncoders.Endpoints=default -h localhost -p 8994
-PoseRightKneeMotors.Endpoints=default -h localhost -p 8995
+PoseRightKneeEncoders.Endpoints=default -h localhost -p 9030
+PoseRightKneeMotors.Endpoints=default -h localhost -p 9031

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightknee.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_rightkneemotors;
 
         struct rightknee_t {
-            physics::JointPtr joint_tilt, joint_pan;
-            physics::LinkPtr link_pan, link_tilt;
+            physics::JointPtr joint_pitch;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxPitch, minPitch;
+        std::string modelPitch;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cc	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cc	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,128 +14,109 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
 #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;
+        this->modelPitch = std::string("joint_poserightshoulder_pitch");
+        this->modelRoll = std::string("joint_poserightshoulder_roll");
 
         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";
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseRightShoulder plugin missing <" << this->modelPitch << "> element\n";
+        if (!_sdf->HasElement(this->modelRoll))
+            gzerr << "PoseRightShoulder plugin missing <" << this->modelRoll << "> element\n";
+        
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+        std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseRightShoulder plugin missing <" << elemPitch << "> element\n";
+        if (!_sdf->HasElement(elemRoll))
+            gzerr << "PoseRightShoulder plugin missing <" << elemRoll << "> element\n";
+            
+        this->rightshoulder.joint_pitch = _model->GetJoint(elemPitch);
+        this->rightshoulder.joint_roll = _model->GetJoint(elemRoll);
 
-        this->rightshoulder.joint_roll = _model->GetJoint("rshoulder_roll");
-        this->rightshoulder.joint_tilt = _model->GetJoint("rshoulder_pitch");
+        this->maxPitch = (float) this->rightshoulder.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->rightshoulder.joint_pitch->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->rightshoulder.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->rightshoulder.joint_roll->GetLowerLimit(0).Radian();
 
-        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        
+        // 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;
         }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_RightShoulderICE, (void*) this);
 
-        //LOAD POSE3DMOTORS
+        // Load OnUpdate method
         this->updateConnection = event::Events::ConnectWorldUpdateBegin(
             boost::bind(&PoseRightShoulder::OnUpdate, this));
 
     }
 
-    void PoseRightShoulder::Init () {}
+    void PoseRightShoulder::Init () {
+        this->rightshoulder.encoders.tilt = 0.0;
+        this->rightshoulder.encoders.roll = 0.0;
+        
+        this->rightshoulder.motorsdata.tilt = 0.0;
+        this->rightshoulder.motorsdata.roll = 0.0;
+    }
 
     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;
+        // GET pose3dencoders data from the right shoulder (ROLL&TILT)
+        pthread_mutex_lock(&this->mutex_rightshoulderencoders);
         
-        this->rightshoulder.encoders.tilt = - this->rightshoulder.joint_tilt->GetAngle(0).Radian();
-        this->rightshoulder.encoders.roll = - this->rightshoulder.joint_roll->GetAngle(0).Radian();
+        this->rightshoulder.encoders.tilt = this->rightshoulder.joint_pitch->GetAngle(0).Radian();
+        this->rightshoulder.encoders.roll = this->rightshoulder.joint_roll->GetAngle(0).Radian();
         
+        pthread_mutex_unlock(&this->mutex_rightshoulderencoders);
+        
         //          ----------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);
-            }            
-        }
+        this->rightshoulder.joint_pitch->SetMaxForce(0, this->stiffness);
+        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);
-            }
-        }
+        pthread_mutex_lock(&this->mutex_rightshouldermotors);
+        
+        float pitchSpeed = - this->rightshoulder.motorsdata.tilt - this->rightshoulder.encoders.tilt;
+        if ((std::abs(pitchSpeed) > 0.001) && (std::abs(pitchSpeed) < 0.1))
+            pitchSpeed = 0.1;
+        
+        float rollSpeed = - this->rightshoulder.motorsdata.roll - this->rightshoulder.encoders.roll;
+        if ((std::abs(rollSpeed) > 0.001) && (std::abs(rollSpeed) < 0.1))
+            rollSpeed = 0.1;
+        
+        this->rightshoulder.joint_pitch->SetVelocity(0, pitchSpeed);
+        this->rightshoulder.joint_roll->SetVelocity(0, rollSpeed);
 
+        pthread_mutex_unlock(&this->mutex_rightshouldermotors);
+
         gettimeofday(&b, NULL);
         totalb = b.tv_sec * 1000000 + b.tv_usec;
 
@@ -145,18 +126,17 @@
         if (diff < 10)
             diff = 10;
 
-        //usleep(diff*1000);
         sleep(diff / 1000);
     }
     
-    class Pose3DEncoders : virtual public jderobot::Pose3DEncoders {
+    class Pose3DEncodersRS : virtual public jderobot::Pose3DEncoders {
     public:
 
-        Pose3DEncoders ( gazebo::PoseRightShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+        Pose3DEncodersRS ( gazebo::PoseRightShoulder* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DEncoders () {}
+        virtual ~Pose3DEncodersRS () {}
 
         virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightshoulderencoders);
@@ -184,14 +164,14 @@
         jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
     };
 
-    class Pose3DMotors : virtual public jderobot::Pose3DMotors {
+    class Pose3DMotorsRS : virtual public jderobot::Pose3DMotors {
     public:
 
-        Pose3DMotors (gazebo::PoseRightShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+        Pose3DMotorsRS (gazebo::PoseRightShoulder* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
             this->pose = pose;
         }
 
-        virtual ~Pose3DMotors() {}
+        virtual ~Pose3DMotorsRS() {}
 
         virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
             pthread_mutex_lock(&pose->mutex_rightshouldermotors);
@@ -208,7 +188,6 @@
             pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
 
             return pose3DMotorsData;
-
         }
 
         virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
@@ -239,7 +218,6 @@
             pose->rightshoulder.motorsdata.tiltSpeed = data->tiltSpeed;
             
             pthread_mutex_unlock(&pose->mutex_rightshouldermotors);
-
         }
 
         gazebo::PoseRightShoulder* pose;
@@ -250,7 +228,6 @@
     };
 
     void* thread_RightShoulderICE ( void* v ) {
-
         gazebo::PoseRightShoulder* rightshoulder = (gazebo::PoseRightShoulder*)v;
         char* name = (char*) rightshoulder->cfgfile_rightshoulder.c_str();
         Ice::CommunicatorPtr ic;
@@ -272,8 +249,8 @@
             Ice::ObjectAdapterPtr AdapterMotors =
                     ic->createObjectAdapterWithEndpoints("AdapterRightShoulderMotors", EndpointsMotors);
 
-            Ice::ObjectPtr encoders = new Pose3DEncoders(rightshoulder);
-            Ice::ObjectPtr motors = new Pose3DMotors(rightshoulder);
+            Ice::ObjectPtr encoders = new Pose3DEncodersRS(rightshoulder);
+            Ice::ObjectPtr motors = new Pose3DMotorsRS(rightshoulder);
 
             AdapterEncoders->add(encoders, ic->stringToIdentity("RightShoulderEncoders"));
             AdapterMotors->add(motors, ic->stringToIdentity("RightShoulderMotors"));

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,3 +1,3 @@
 #without registry
-PoseRightShoulderEncoders.Endpoints=default -h localhost -p 9996
-PoseRightShoulderMotors.Endpoints=default -h localhost -p 9997
+PoseRightShoulderEncoders.Endpoints=default -h localhost -p 9090
+PoseRightShoulderMotors.Endpoints=default -h localhost -p 9091

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.h	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightshoulder.h	2014-01-26 10:32:52 UTC (rev 1148)
@@ -14,7 +14,8 @@
  *  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>
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
 
@@ -53,8 +54,7 @@
         pthread_mutex_t mutex_rightshouldermotors;
 
         struct rightshoulder_t {
-            physics::JointPtr joint_tilt, joint_roll;
-            physics::LinkPtr link_tilt, link_roll;
+            physics::JointPtr joint_pitch, joint_roll;
             encoders_t encoders;
             motorsdata_t motorsdata;
             motorsparams_t motorsparams;
@@ -67,8 +67,10 @@
         
         event::ConnectionPtr updateConnection;
         double stiffness;
-        int count;
         int cycle;
+        
+        float maxPitch, maxRoll, minPitch, minRoll;
+        std::string modelPitch, modelRoll;
     };
 }
 

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/worlds/naoworld.world
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/worlds/naoworld.world	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/worlds/naoworld.world	2014-01-26 10:32:52 UTC (rev 1148)
@@ -8,7 +8,7 @@
 
     <!-- My robots -->
     <include>
-      <uri>model://nao_robot</uri>
+      <uri>model://nao</uri>
       <pose>0.08 0 0 0 0 3.14</pose>
     </include>
 

Modified: trunk/src/stable/components/naoviewer/control.cpp
===================================================================
--- trunk/src/stable/components/naoviewer/control.cpp	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/naoviewer/control.cpp	2014-01-26 10:32:52 UTC (rev 1148)
@@ -157,14 +157,14 @@
         std::cout << "Head motors connected" << std::endl;
         
         // Contact to head motors (speed commands)
-        Ice::ObjectPrx headspeed = ic->propertyToProxy("naooperator.HeadSpeed.Proxy");
+/*        Ice::ObjectPrx headspeed = ic->propertyToProxy("naooperator.HeadSpeed.Proxy");
         if (headspeed == 0)
             throw "Could not create proxy with head speed";
         this->mapMotors[HEADSPEED] = jderobot::Pose3DMotorsPrx::checkedCast(headspeed);
         if (this->mapMotors[HEADSPEED] == 0)
             throw "Invalid proxy naooperator.HeadSpeed.Proxy";
         std::cout << "Head speed connected" << std::endl;
-            
+  */          
         // Contact to shoulder motors
         Ice::ObjectPrx leftshouldermotors = ic->propertyToProxy("naooperator.LeftShoulderMotors.Proxy");
         if (leftshouldermotors == 0)
@@ -198,7 +198,7 @@
         if (this->mapMotors[RIGHTELBOWMOTORS] == 0)
             throw "Invalid proxy naooperator.RightElbowMotors.Proxy";
         std::cout << "Right elbow motors connected" << std::endl;
-            
+/*            
         // Contact to hip motors
         Ice::ObjectPrx lefthipmotors = ic->propertyToProxy("naooperator.LeftHipMotors.Proxy");
         if (lefthipmotors == 0)
@@ -215,7 +215,7 @@
         if (this->mapMotors[RIGHTHIPMOTORS] == 0)
             throw "Invalid proxy naooperator.RightHipMotors.Proxy";
         std::cout << "Right hip motors connected" << std::endl;
-            
+*/            
         // Contact to knee motors
         Ice::ObjectPrx leftkneemotors = ic->propertyToProxy("naooperator.LeftKneeMotors.Proxy");
         if (leftkneemotors == 0)
@@ -260,7 +260,7 @@
         if (this->cameraprx == 0)
             throw "Invalid proxy naooperator.Camera.Proxy";
         std::cout << "Camera connected" << std::endl;
-*/
+
         // Contact to motors (for walking)
         Ice::ObjectPrx motors = ic->propertyToProxy("naooperator.Motors.Proxy");
         if (motors == 0)
@@ -278,7 +278,7 @@
         if (this->motions == 0)
             throw "Invalid proxy naooperator.Motions.Proxy";
         std::cout << "Motions connected" << std::endl;
-        
+*/
         std::cout << "All ICE interfaces connected!" << std::endl;
 
     } catch (const Ice::Exception& ex) {

Modified: trunk/src/stable/components/naoviewer/naooperator.cfg
===================================================================
--- trunk/src/stable/components/naoviewer/naooperator.cfg	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/naoviewer/naooperator.cfg	2014-01-26 10:32:52 UTC (rev 1148)
@@ -1,16 +1,16 @@
 #Curro
-naooperator.HeadMotors.Proxy=NeckMotors:default -h 192.168.14.113 -p 10000
-naooperator.HeadSpeed.Proxy=NeckSpeed:default -h 192.168.14.113 -p 10000
-naooperator.LeftShoulderMotors.Proxy=LeftShoulderMotors:default -h 192.168.14.113 -p 10000
-naooperator.RightShoulderMotors.Proxy=RightShoulderMotors:default -h 192.168.14.113 -p 10000
-naooperator.LeftElbowMotors.Proxy=LeftElbowMotors:default -h 192.168.14.113 -p 10000
-naooperator.RightElbowMotors.Proxy=RightElbowMotors:default -h 192.168.14.113 -p 10000
-naooperator.LeftHipMotors.Proxy=LeftHipMotors:default -h 192.168.14.113 -p 10000
-naooperator.RightHipMotors.Proxy=RightHipMotors:default -h 192.168.14.113 -p 10000
-naooperator.LeftKneeMotors.Proxy=LeftKneeMotors:default -h 192.168.14.113 -p 10000
-naooperator.RightKneeMotors.Proxy=RightKneeMotors:default -h 192.168.14.113 -p 10000
-naooperator.LeftAnkleMotors.Proxy=LeftAnkleMotors:default -h 192.168.14.113 -p 10000
-naooperator.RightAnkleMotors.Proxy=RightAnkleMotors:default -h 192.168.14.113 -p 10000
-naooperator.Camera.Proxy=Camera:default -h 192.168.14.113 -p 10000
-naooperator.Motors.Proxy=Motors:default -h 192.168.14.113 -p 10000
-naooperator.Motions.Proxy=Motions:default -h 192.168.14.113 -p 10000
+naooperator.HeadMotors.Proxy=NeckMotors:default -h localhost -p 9110
+#naooperator.HeadSpeed.Proxy=NeckSpeed:default -h 192.168.14.113 -p 10000
+naooperator.LeftShoulderMotors.Proxy=LeftShoulderMotors:default -h localhost -p 9081
+naooperator.RightShoulderMotors.Proxy=RightShoulderMotors:default -h localhost -p 9091
+naooperator.LeftElbowMotors.Proxy=LeftElbowMotors:default -h localhost -p 9061
+naooperator.RightElbowMotors.Proxy=RightElbowMotors:default -h localhost -p 9071
+#naooperator.LeftHipMotors.Proxy=LeftHipMotors:default -h 192.168.14.113 -p 10000
+#naooperator.RightHipMotors.Proxy=RightHipMotors:default -h 192.168.14.113 -p 10000
+naooperator.LeftKneeMotors.Proxy=LeftKneeMotors:default -h localhost -p 9021
+naooperator.RightKneeMotors.Proxy=RightKneeMotors:default -h localhost -p 9031
+naooperator.LeftAnkleMotors.Proxy=LeftAnkleMotors:default -h localhost -p 9001
+naooperator.RightAnkleMotors.Proxy=RightAnkleMotors:default -h localhost -p 9011
+naooperator.Camera.Proxy=cam_sensor_top:default -h localhost -p 10000
+#naooperator.Motors.Proxy=Motors:default -h 192.168.14.113 -p 10000
+#naooperator.Motions.Proxy=Motions:default -h 192.168.14.113 -p 10000

Modified: trunk/src/stable/components/naoviewer/naooperator.cpp
===================================================================
--- trunk/src/stable/components/naoviewer/naooperator.cpp	2014-01-25 20:27:32 UTC (rev 1147)
+++ trunk/src/stable/components/naoviewer/naooperator.cpp	2014-01-26 10:32:52 UTC (rev 1148)
@@ -250,7 +250,7 @@
     
     this->mapScales[ROLL_LEFT_SHOULDER]->signal_value_changed().connect(sigc::mem_fun(this,
                                                             &NaoOperator::on_scale_roll_left_shoulder));
-                                                            
+/*                                                            
     this->mapScales[YAWPITCH_LEFT_HIP]->signal_value_changed().connect(sigc::mem_fun(this,
                                                             &NaoOperator::on_scale_yawpitch_left_hip));
                                                             
@@ -268,7 +268,7 @@
     
     this->mapScales[ROLL_RIGHT_HIP]->signal_value_changed().connect(sigc::mem_fun(this,
                                                             &NaoOperator::on_scale_roll_right_hip));
-                                                            
+*/                                                            
     this->mapScales[PITCH_RIGHT_SHOULDER]->signal_value_changed().connect(sigc::mem_fun(this,
                                                             &NaoOperator::on_scale_pitch_right_shoulder));
                                                             
@@ -320,7 +320,7 @@
     this->head_right->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_head_right));
     this->head_left->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_head_left));
     this->head_stop->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_head_stop));
-    
+/*    
     this->move_front->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_move_front));
     this->move_back->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_move_back));
     this->move_right->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_move_right));
@@ -336,6 +336,7 @@
     this->intercept->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_intercept));
     this->change_camera->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_change_camera));
     this->reset_naoqi->signal_clicked().connect(sigc::mem_fun(this, &NaoOperator::on_button_reset_naoqi));
+*/
 }
 
 void NaoOperator::on_scale_pitch_left_shoulder () {



More information about the Jderobot-admin mailing list