[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