[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