[Jderobot-admin] jderobot-r1150 - trunk/src/stable/components/gazeboserver/plugins/nao

bmenendez en jderobot.org bmenendez en jderobot.org
Mie Ene 29 22:36:02 CET 2014


Author: bmenendez
Date: 2014-01-29 22:36:02 +0100 (Wed, 29 Jan 2014)
New Revision: 1150

Added:
   trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.h
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cfg
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.h
Modified:
   trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc
   trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc
Log:
#148 Fixed the compile error.


Modified: trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt	2014-01-27 19:17:31 UTC (rev 1149)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/CMakeLists.txt	2014-01-29 21:36:02 UTC (rev 1150)
@@ -94,13 +94,13 @@
 	${ZeroCIce_LIBRARIES}
 )
 
-#add_library(poselefthip SHARED poselefthip.cc)
-#target_link_libraries(poselefthip ${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
@@ -113,14 +113,14 @@
 	${ZeroCIce_LIBRARIES}
 )
 
-#add_library(poseleftknee SHARED poseleftknee.cc)
-#target_link_libraries(poseleftknee ${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)

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc	2014-01-27 19:17:31 UTC (rev 1149)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftelbow.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -30,7 +30,7 @@
         pthread_mutex_init(&this->mutex_leftelbowmotors, NULL);
         this->cycle = 50;
         this->cfgfile_leftelbow = std::string("--Ice.Config=poseleftelbow.cfg");
-        this->modelYaw = std::string("joint_poseleftelbow_pan");
+        this->modelYaw = std::string("joint_poseleftelbow_yaw");
         this->modelRoll = std::string("joint_poseleftelbow_roll");
 
         std::cout << "Constructor PoseLeftElbow" << std::endl;

Added: trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -0,0 +1,294 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#include "poselefthip.h"
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseLeftHip)
+
+    PoseLeftHip::PoseLeftHip () {
+        pthread_mutex_init(&this->mutex_lefthipencoders, NULL);
+        pthread_mutex_init(&this->mutex_lefthipmotors, NULL);
+        this->cycle = 50;
+        this->cfgfile_lefthip = std::string("--Ice.Config=poselefthip.cfg");
+        this->modelYaw = std::string("joint_poselefthip_yaw");
+        this->modelPitch = std::string("joint_poselefthip_pitch");
+        this->modelRoll = std::string("joint_poselefthip_roll");
+
+        std::cout << "Constructor PoseLeftHip" << std::endl;
+    }
+
+    void PoseLeftHip::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        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->lefthip.joint_yaw = _model->GetJoint(elemYaw);
+        this->lefthip.joint_pitch = _model->GetJoint(elemPitch);
+        this->lefthip.joint_roll = _model->GetJoint(elemRoll);
+
+        this->maxYaw = (float) this->lefthip.joint_yaw->GetUpperLimit(0).Radian();
+        this->minYaw = (float) this->lefthip.joint_yaw->GetLowerLimit(0).Radian();
+        this->maxPitch = (float) this->lefthip.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->lefthip.joint_pitch->GetLowerLimit(0).Radian();
+        this->maxRoll = (float) this->lefthip.joint_roll->GetUpperLimit(0).Radian();
+        this->minRoll = (float) this->lefthip.joint_roll->GetLowerLimit(0).Radian();
+
+        // Load torque       
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            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_LeftHipICE, (void*) this);
+
+        // Load OnUpdate method
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseLeftHip::OnUpdate, this));
+    }
+
+    void PoseLeftHip::Init () {
+        this->lefthip.encoders.pan = 0.0;
+        this->lefthip.encoders.tilt = 0.0;
+        this->lefthip.encoders.roll = 0.0;
+        
+        this->lefthip.motorsdata.pan = 0.0;
+        this->lefthip.motorsdata.tilt = 0.0;
+        this->lefthip.motorsdata.roll = 0.0;
+    }
+
+    void PoseLeftHip::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+        
+        //          ----------ENCODERS----------
+        // GET pose3dencoders data from the right elbow (PAN&ROLL)
+        pthread_mutex_lock(&this->mutex_lefthipencoders);
+        
+        this->lefthip.encoders.pan = this->lefthip.joint_yaw->GetAngle(0).Radian();
+        this->lefthip.encoders.tilt = this->lefthip.joint_pitch->GetAngle(0).Radian();
+        this->lefthip.encoders.roll = this->lefthip.joint_roll->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_lefthipencoders);
+
+        //          ----------MOTORS----------
+        this->lefthip.joint_yaw->SetMaxForce(0, this->stiffness);
+        this->lefthip.joint_pitch->SetMaxForce(0, this->stiffness);
+        this->lefthip.joint_roll->SetMaxForce(0, this->stiffness);
+        
+        pthread_mutex_lock(&this->mutex_lefthipmotors);
+        
+        float yawSpeed = - this->lefthip.motorsdata.pan - this->lefthip.encoders.pan;
+        if ((std::abs(yawSpeed) < 0.1) && (std::abs(yawSpeed) > 0.001))
+            yawSpeed = 0.1;
+        
+        float pitchSpeed = - this->lefthip.motorsdata.tilt - this->lefthip.encoders.tilt;
+        if ((std::abs(pitchSpeed) < 0.1) && (std::abs(pitchSpeed) > 0.001))
+            pitchSpeed = 0.1;
+            
+        float rollSpeed = - this->lefthip.motorsdata.roll - this->lefthip.encoders.roll;
+        if ((std::abs(rollSpeed) < 0.1) && (std::abs(rollSpeed) > 0.001))
+            rollSpeed = 0.1;
+        
+        this->lefthip.joint_yaw->SetVelocity(0, yawSpeed);
+        this->lefthip.joint_pitch->SetVelocity(0, pitchSpeed);
+        this->lefthip.joint_roll->SetVelocity(0, rollSpeed);
+
+        pthread_mutex_unlock(&this->mutex_lefthipmotors);
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        //usleep(diff*1000);
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncodersRH : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncodersRH ( gazebo::PoseLeftHip* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncodersRH () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_lefthipencoders);
+            
+            pose3DEncodersData->x = pose->lefthip.encoders.x;
+            pose3DEncodersData->y = pose->lefthip.encoders.y;
+            pose3DEncodersData->z = pose->lefthip.encoders.z;
+            pose3DEncodersData->pan = pose->lefthip.encoders.pan;
+            pose3DEncodersData->tilt = pose->lefthip.encoders.tilt;
+            pose3DEncodersData->roll = pose->lefthip.encoders.roll;
+            pose3DEncodersData->clock = pose->lefthip.encoders.clock;
+            pose3DEncodersData->maxPan = pose->lefthip.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->lefthip.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->lefthip.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->lefthip.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_lefthipencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseLeftHip* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotorsRH : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotorsRH (gazebo::PoseLeftHip* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotorsRH () {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_lefthipmotors);
+            
+            pose3DMotorsData->x = pose->lefthip.motorsdata.x;
+            pose3DMotorsData->y = pose->lefthip.motorsdata.y;
+            pose3DMotorsData->z = pose->lefthip.motorsdata.z;
+            pose3DMotorsData->pan = pose->lefthip.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->lefthip.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->lefthip.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->lefthip.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->lefthip.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_lefthipmotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_lefthipmotors);
+            
+            pose3DMotorsParams->maxPan = pose->lefthip.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->lefthip.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->lefthip.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->lefthip.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->lefthip.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->lefthip.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_lefthipmotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_lefthipmotors);
+            
+            pose->lefthip.motorsdata.x = data->x;
+            pose->lefthip.motorsdata.y = data->y;
+            pose->lefthip.motorsdata.z = data->z;
+            pose->lefthip.motorsdata.pan = data->pan;
+            pose->lefthip.motorsdata.tilt = data->tilt;
+            pose->lefthip.motorsdata.roll = data->roll;
+            pose->lefthip.motorsdata.panSpeed = data->panSpeed;
+            pose->lefthip.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_lefthipmotors);
+        }
+
+        gazebo::PoseLeftHip* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_LeftHipICE ( void* v ) {
+
+        gazebo::PoseLeftHip* lefthip = (gazebo::PoseLeftHip*)v;
+        char* name = (char*) lefthip->cfgfile_lefthip.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseLeftHipEncoders.Endpoints");
+            std::cout << "PoseLeftHipEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseLeftHipkMotors.Endpoints");
+            std::cout << "PoseLeftHipMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftHipEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftHipkMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncodersRH(lefthip);
+            Ice::ObjectPtr motors = new Pose3DMotorsRH(lefthip);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("LeftHipEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("LeftHipMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.cfg	2014-01-29 21:36:02 UTC (rev 1150)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftHipEncoders.Endpoints=default -h localhost -p 9040
+PoseLeftHipMotors.Endpoints=default -h localhost -p 9041

Added: trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poselefthip.h	2014-01-29 21:36:02 UTC (rev 1150)
@@ -0,0 +1,77 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSELEFTHIP_H
+#define POSELEFTHIP_H
+
+namespace gazebo {
+
+    void* thread_LeftHipICE ( void* v );
+
+    class PoseLeftHip : public ModelPlugin {
+    public:
+
+        PoseLeftHip ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_lefthip;
+        pthread_mutex_t mutex_lefthipencoders;
+        pthread_mutex_t mutex_lefthipmotors;
+
+        struct lefthip_t {
+            physics::JointPtr joint_yaw, joint_pitch, joint_roll;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        lefthip_t lefthip;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int cycle;
+        
+        float maxYaw, maxPitch, maxRoll, minYaw, minPitch, minRoll;
+        std::string modelYaw, modelPitch, modelRoll;
+    };
+}
+
+#endif // POSERIGHTHIP_H

Added: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cc	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -0,0 +1,257 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#include "poseleftknee.h"
+
+namespace gazebo {
+    GZ_REGISTER_MODEL_PLUGIN(PoseLeftKnee)
+
+    PoseLeftKnee::PoseLeftKnee () {
+        pthread_mutex_init(&this->mutex_leftkneeencoders, NULL);
+        pthread_mutex_init(&this->mutex_leftkneemotors, NULL);
+        this->cycle = 50;
+        this->cfgfile_leftknee = std::string("--Ice.Config=poseleftknee.cfg");
+        this->modelPitch = std::string("joint_poseleftknee_pitch");
+
+        std::cout << "Constructor PoseLeftKnee" << std::endl;
+    }
+
+    void PoseLeftKnee::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
+        if (!_sdf->HasElement(this->modelPitch))
+            gzerr << "PoseLeftKnee plugin missing <" << this->modelPitch << "> element\n";
+            
+        std::string elemPitch = std::string(_sdf->GetElement(this->modelPitch)->GetValueString());
+            
+        if (!_sdf->HasElement(elemPitch))
+            gzerr << "PoseLeftKnee plugin missing <" << elemPitch << "> element\n";
+            
+        this->leftknee.joint_pitch = _model->GetJoint(elemPitch);
+
+        this->maxPitch = (float) this->leftknee.joint_pitch->GetUpperLimit(0).Radian();
+        this->minPitch = (float) this->leftknee.joint_pitch->GetLowerLimit(0).Radian();
+
+        // Load torque
+        if (_sdf->HasElement("torque"))
+            this->stiffness = _sdf->GetElement("torque")->GetValueDouble();
+        else {
+            gzwarn << "No torque value set for the left knee plugin.\n";
+            this->stiffness = 5.0;
+        }
+        
+        pthread_t thr_ice;
+        pthread_create(&thr_ice, NULL, &thread_LeftKneeICE, (void*) this);
+
+        // Load OnUpdate method
+        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
+                                    boost::bind(&PoseLeftKnee::OnUpdate, this));
+    }
+
+    void PoseLeftKnee::Init () {
+        this->leftknee.encoders.tilt = 0.0;
+        
+        this->leftknee.motorsdata.tilt = 0.0;
+    }
+
+    void PoseLeftKnee::OnUpdate () {
+        long totalb, totala, diff;
+        struct timeval a, b;
+        
+        gettimeofday(&a, NULL);
+        totala = a.tv_sec * 1000000 + a.tv_usec;
+
+        //          ----------ENCODERS----------
+        // GET pose3dencoders data from the right elbow (PAN&ROLL)
+        pthread_mutex_lock(&this->mutex_leftkneeencoders);
+        
+        this->leftknee.encoders.tilt = this->leftknee.joint_pitch->GetAngle(0).Radian();
+        
+        pthread_mutex_unlock(&this->mutex_leftkneeencoders);
+
+        //          ----------MOTORS----------
+        this->leftknee.joint_pitch->SetMaxForce(0, this->stiffness);
+        
+        pthread_mutex_lock(&this->mutex_leftkneemotors);
+        
+        float pitchSpeed = - this->leftknee.motorsdata.tilt - this->leftknee.encoders.tilt;
+        if ((std::abs(pitchSpeed) < 0.1) && (std::abs(pitchSpeed) > 0.001))
+            pitchSpeed = 0.1;
+        
+        this->leftknee.joint_pitch->SetVelocity(0, pitchSpeed);
+
+        pthread_mutex_unlock(&this->mutex_leftkneemotors);
+
+        gettimeofday(&b, NULL);
+        totalb = b.tv_sec * 1000000 + b.tv_usec;
+
+        diff = (totalb - totala) / 1000;
+        diff = cycle - diff;
+
+        if (diff < 10)
+            diff = 10;
+
+        sleep(diff / 1000);
+    }
+    
+    class Pose3DEncodersLK : virtual public jderobot::Pose3DEncoders {
+    public:
+
+        Pose3DEncodersLK ( gazebo::PoseLeftKnee* pose ) : pose3DEncodersData ( new jderobot::Pose3DEncodersData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DEncodersLK () {}
+
+        virtual jderobot::Pose3DEncodersDataPtr getPose3DEncodersData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftkneeencoders);
+            
+            pose3DEncodersData->x = pose->leftknee.encoders.x;
+            pose3DEncodersData->y = pose->leftknee.encoders.y;
+            pose3DEncodersData->z = pose->leftknee.encoders.z;
+            pose3DEncodersData->pan = pose->leftknee.encoders.pan;
+            pose3DEncodersData->tilt = pose->leftknee.encoders.tilt;
+            pose3DEncodersData->roll = pose->leftknee.encoders.roll;
+            pose3DEncodersData->clock = pose->leftknee.encoders.clock;
+            pose3DEncodersData->maxPan = pose->leftknee.encoders.maxPan;
+            pose3DEncodersData->minPan = pose->leftknee.encoders.minPan;
+            pose3DEncodersData->maxTilt = pose->leftknee.encoders.maxTilt;
+            pose3DEncodersData->minTilt = pose->leftknee.encoders.minTilt;
+            
+            pthread_mutex_unlock(&pose->mutex_leftkneeencoders);
+
+            return pose3DEncodersData;
+        }
+
+        gazebo::PoseLeftKnee* pose;
+
+    private:
+        jderobot::Pose3DEncodersDataPtr pose3DEncodersData;
+    };
+
+    class Pose3DMotorsLK : virtual public jderobot::Pose3DMotors {
+    public:
+
+        Pose3DMotorsLK (gazebo::PoseLeftKnee* pose) : pose3DMotorsData ( new jderobot::Pose3DMotorsData() ) {
+            this->pose = pose;
+        }
+
+        virtual ~Pose3DMotorsLK () {}
+
+        virtual jderobot::Pose3DMotorsDataPtr getPose3DMotorsData ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftkneemotors);
+            
+            pose3DMotorsData->x = pose->leftknee.motorsdata.x;
+            pose3DMotorsData->y = pose->leftknee.motorsdata.y;
+            pose3DMotorsData->z = pose->leftknee.motorsdata.z;
+            pose3DMotorsData->pan = pose->leftknee.motorsdata.pan;
+            pose3DMotorsData->tilt = pose->leftknee.motorsdata.tilt;
+            pose3DMotorsData->roll = pose->leftknee.motorsdata.roll;
+            pose3DMotorsData->panSpeed = pose->leftknee.motorsdata.panSpeed;
+            pose3DMotorsData->tiltSpeed = pose->leftknee.motorsdata.tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftkneemotors);
+
+            return pose3DMotorsData;
+        }
+
+        virtual jderobot::Pose3DMotorsParamsPtr getPose3DMotorsParams ( const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftkneemotors);
+            
+            pose3DMotorsParams->maxPan = pose->leftknee.motorsparams.maxPan;
+            pose3DMotorsParams->minPan = pose->leftknee.motorsparams.minPan;
+            pose3DMotorsParams->maxTilt = pose->leftknee.motorsparams.maxTilt;
+            pose3DMotorsParams->minTilt = pose->leftknee.motorsparams.minTilt;
+            pose3DMotorsParams->maxPanSpeed = pose->leftknee.motorsparams.maxPanSpeed;
+            pose3DMotorsParams->maxTiltSpeed = pose->leftknee.motorsparams.maxTiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftkneemotors);
+            
+            return pose3DMotorsParams;
+        }
+
+        virtual Ice::Int setPose3DMotorsData ( const jderobot::Pose3DMotorsDataPtr & data, const Ice::Current& ) {
+            pthread_mutex_lock(&pose->mutex_leftkneemotors);
+            
+            pose->leftknee.motorsdata.x = data->x;
+            pose->leftknee.motorsdata.y = data->y;
+            pose->leftknee.motorsdata.z = data->z;
+            pose->leftknee.motorsdata.pan = data->pan;
+            pose->leftknee.motorsdata.tilt = data->tilt;
+            pose->leftknee.motorsdata.roll = data->roll;
+            pose->leftknee.motorsdata.panSpeed = data->panSpeed;
+            pose->leftknee.motorsdata.tiltSpeed = data->tiltSpeed;
+            
+            pthread_mutex_unlock(&pose->mutex_leftkneemotors);
+        }
+
+        gazebo::PoseLeftKnee* pose;
+
+    private:
+        jderobot::Pose3DMotorsDataPtr pose3DMotorsData;
+        jderobot::Pose3DMotorsParamsPtr pose3DMotorsParams;
+    };
+
+    void* thread_LeftKneeICE ( void* v ) {
+
+        gazebo::PoseLeftKnee* leftknee = (gazebo::PoseLeftKnee*)v;
+        char* name = (char*) leftknee->cfgfile_leftknee.c_str();
+        Ice::CommunicatorPtr ic;
+        int argc = 1;
+        Ice::PropertiesPtr prop;
+        char* argv[] = {name};
+
+        try {
+            ic = Ice::initialize(argc, argv);
+
+            prop = ic->getProperties();
+            std::string EndpointsEncoders = prop->getProperty("PoseLeftKneeEncoders.Endpoints");
+            std::cout << "PoseLeftKneeEncoders Endpoints > " << EndpointsEncoders << std::endl;
+            std::string EndpointsMotors = prop->getProperty("PoseLeftKneeMotors.Endpoints");
+            std::cout << "PoseLeftKneeMotors Endpoints > " << EndpointsMotors << std::endl;
+
+            Ice::ObjectAdapterPtr AdapterEncoders =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftKneeEncoders", EndpointsEncoders);
+            Ice::ObjectAdapterPtr AdapterMotors =
+                    ic->createObjectAdapterWithEndpoints("AdapterLeftKneeMotors", EndpointsMotors);
+
+            Ice::ObjectPtr encoders = new Pose3DEncodersLK(leftknee);
+            Ice::ObjectPtr motors = new Pose3DMotorsLK(leftknee);
+
+            AdapterEncoders->add(encoders, ic->stringToIdentity("LeftKneeEncoders"));
+            AdapterMotors->add(motors, ic->stringToIdentity("LeftKneeMotors"));
+
+            AdapterEncoders->activate();
+            AdapterMotors->activate();
+
+            ic->waitForShutdown();
+        } catch (const Ice::Exception& e) {
+            std::cerr << e << std::endl;
+        } catch (const char* msg) {
+            std::cerr << msg << std::endl;
+        }
+        if (ic) {
+            try {
+                ic->destroy();
+            } catch (const Ice::Exception& e) {
+                std::cerr << e << std::endl;
+            }
+        }
+    }
+}

Added: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cfg
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cfg	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.cfg	2014-01-29 21:36:02 UTC (rev 1150)
@@ -0,0 +1,3 @@
+#without registry
+PoseLeftKneeEncoders.Endpoints=default -h localhost -p 9020
+PoseLeftKneeMotors.Endpoints=default -h localhost -p 9021

Added: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.h
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.h	                        (rev 0)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftknee.h	2014-01-29 21:36:02 UTC (rev 1150)
@@ -0,0 +1,77 @@
+/*
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Library General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ *  Author:     Borja Menéndez Moreno <b.menendez.moreno en gmail.com>
+ *  Co-author:  José María Cañas Plaza <jmplaza en gsyc.es>
+ *
+ */
+
+#include <boost/bind.hpp>
+#include "gazebo.hh"
+#include "physics/physics.hh"
+#include "common/common.hh"
+#include "transport/transport.hh"
+
+// ICE utils includes
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+
+// JDErobot general ice component includes
+#include <jderobot/pose3dencoders.h>
+#include <jderobot/pose3dmotors.h>
+
+#include "common.h"
+
+#ifndef POSELEFTKNEE_H
+#define POSELEFTKNEE_H
+
+namespace gazebo {
+
+    void* thread_LeftKneeICE ( void* v );
+
+    class PoseLeftKnee : public ModelPlugin {
+    public:
+
+        PoseLeftKnee ();
+        virtual void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
+        virtual void Init ();
+
+        std::string cfgfile_leftknee;
+        pthread_mutex_t mutex_leftkneeencoders;
+        pthread_mutex_t mutex_leftkneemotors;
+
+        struct leftknee_t {
+            physics::JointPtr joint_pitch;
+            encoders_t encoders;
+            motorsdata_t motorsdata;
+            motorsparams_t motorsparams;
+        };
+
+        leftknee_t leftknee;
+
+    private:
+        void OnUpdate ();
+        
+        event::ConnectionPtr updateConnection;
+        double stiffness;
+        int cycle;
+        
+        float maxPitch, minPitch;
+        std::string modelPitch;
+    };
+}
+
+#endif // POSELEFTKNEE_H

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc	2014-01-27 19:17:31 UTC (rev 1149)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseleftshoulder.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -29,7 +29,7 @@
         pthread_mutex_init(&this->mutex_leftshouldermotors, NULL);
         this->cycle = 50;
         this->cfgfile_leftshoulder = std::string("--Ice.Config=poseleftshoulder.cfg");
-        this->modelPitch = std::string("joint_poseleftshoulder_tilt");
+        this->modelPitch = std::string("joint_poseleftshoulder_pitch");
         this->modelRoll = std::string("joint_poseleftshoulder_roll");
 
         std::cout << "Constructor PoseLeftShoulder" << std::endl;

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc	2014-01-27 19:17:31 UTC (rev 1149)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poseneck.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -38,9 +38,9 @@
 
     void PoseNeck::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf ) {
         if (!_sdf->HasElement(this->modelPan))
-            gzerr << "PoseNeck plugin missing <" << this->modelPan << "> element first\n";
+            gzerr << "PoseNeck plugin missing <" << this->modelPan << "> element\n";
         if (!_sdf->HasElement(this->modelTilt))
-            gzerr << "PoseNeck plugin missing <" << this->modelTilt << "> element first\n";
+            gzerr << "PoseNeck 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());

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc	2014-01-27 19:17:31 UTC (rev 1149)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserightelbow.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -41,7 +41,7 @@
         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 elemPan = std::string(_sdf->GetElement(this->modelPan)->GetValueString());
         std::string elemRoll = std::string(_sdf->GetElement(this->modelRoll)->GetValueString());
             
         if (!_sdf->HasElement(elemPan))
@@ -49,10 +49,10 @@
         if (!_sdf->HasElement(elemRoll))
             gzerr << "PoseRightElbow plugin missing <" << elemRoll << "> element\n";
             
-        this->rightelbow.joint_yaw = _model->GetJoint(elemPan);
+        this->rightelbow.joint_pan = _model->GetJoint(elemPan);
         this->rightelbow.joint_roll = _model->GetJoint(elemRoll);
         
-//        this->rightelbow.link_pan = this->rightelbow.joint_yaw->GetParent();
+//        this->rightelbow.link_pan = this->rightelbow.joint_pan->GetParent();
 //        this->rightelbow.link_roll = this->rightelbow.joint_roll->GetParent();
 
         this->maxYaw = (float) this->rightelbow.joint_yaw->GetUpperLimit(0).Radian();

Modified: trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc	2014-01-27 19:17:31 UTC (rev 1149)
+++ trunk/src/stable/components/gazeboserver/plugins/nao/poserighthip.cc	2014-01-29 21:36:02 UTC (rev 1150)
@@ -260,13 +260,13 @@
             prop = ic->getProperties();
             std::string EndpointsEncoders = prop->getProperty("PoseRightHipEncoders.Endpoints");
             std::cout << "PoseRightHipEncoders Endpoints > " << EndpointsEncoders << std::endl;
-            std::string EndpointsMotors = prop->getProperty("PoseRightHipMotors.Endpoints");
+            std::string EndpointsMotors = prop->getProperty("PoseRightHipkMotors.Endpoints");
             std::cout << "PoseRightHipMotors Endpoints > " << EndpointsMotors << std::endl;
 
             Ice::ObjectAdapterPtr AdapterEncoders =
                     ic->createObjectAdapterWithEndpoints("AdapterRightHipEncoders", EndpointsEncoders);
             Ice::ObjectAdapterPtr AdapterMotors =
-                    ic->createObjectAdapterWithEndpoints("AdapterRightHipMotors", EndpointsMotors);
+                    ic->createObjectAdapterWithEndpoints("AdapterRightHipkMotors", EndpointsMotors);
 
             Ice::ObjectPtr encoders = new Pose3DEncodersRH(righthip);
             Ice::ObjectPtr motors = new Pose3DMotorsRH(righthip);



More information about the Jderobot-admin mailing list