From 3d82ddb240c24db1164f50639612b15a3a3f90b7 Mon Sep 17 00:00:00 2001 From: Enrico Date: Tue, 12 Nov 2013 12:47:59 +0100 Subject: [PATCH 01/11] Set magnetometer measurement to 0.0 --- src/fakebotIMUplugin.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fakebotIMUplugin.cc b/src/fakebotIMUplugin.cc index 85a37982e..9134ef4e9 100644 --- a/src/fakebotIMUplugin.cc +++ b/src/fakebotIMUplugin.cc @@ -94,9 +94,9 @@ void fakebotIMUplugin::OnUpdate() // imu_data.mutable_linear_acceleration()->y()<<" "<< // imu_data.mutable_linear_acceleration()->z()<sec()); From 42e21ba4432067f1aeab365da6f8d8f6f4b489d9 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 14 Nov 2013 00:49:47 +0900 Subject: [PATCH 02/11] Implemented modification to the naming scheme propose in hangout --- CMakeLists.txt | 9 ++++----- .../ControlBoard.hh} | 2 +- .../ControlBoardDriver.h} | 0 .../ForceTorque.hh} | 4 ++-- .../ForceTorqueDriver.h} | 0 include/{GazeboYarpIMU.hh => gazebo_yarp_plugins/IMU.hh} | 0 include/{ => gazebo_yarp_plugins}/analogServer.h | 0 .../ControlBoard.cc} | 2 +- .../ForceTorque.cc} | 6 +++--- src/{GazeboYarpIMU.cc => gazebo_plugins/IMU.cc} | 2 +- .../ControlBoardDriver.cpp} | 2 +- .../ControlBoardDriverControlMode.cpp} | 2 +- .../ControlBoardDriverDeviceDriver.cpp} | 2 +- .../ControlBoardDriverEncoders.cpp} | 2 +- .../ControlBoardDriverOthers.cpp} | 2 +- .../ControlBoardDriverPositionControl.cpp} | 2 +- .../ControlBoardDriverVelocityControl.cpp} | 2 +- .../ControlBoardTorqueDriverControl.cpp} | 2 +- .../ForceTorqueDriver.cpp} | 2 +- src/{ => yarp_drivers}/analogServer.cpp | 2 +- 20 files changed, 22 insertions(+), 23 deletions(-) rename include/{GazeboYarpControlBoard.hh => gazebo_yarp_plugins/ControlBoard.hh} (97%) rename include/{GazeboYarpControlBoardDriver.h => gazebo_yarp_plugins/ControlBoardDriver.h} (100%) rename include/{GazeboYarpForceTorque.hh => gazebo_yarp_plugins/ForceTorque.hh} (96%) rename include/{GazeboYarpForceTorqueDriver.h => gazebo_yarp_plugins/ForceTorqueDriver.h} (100%) rename include/{GazeboYarpIMU.hh => gazebo_yarp_plugins/IMU.hh} (100%) rename include/{ => gazebo_yarp_plugins}/analogServer.h (100%) rename src/{GazeboYarpControlBoard.cc => gazebo_plugins/ControlBoard.cc} (98%) rename src/{GazeboYarpForceTorque.cc => gazebo_plugins/ForceTorque.cc} (98%) rename src/{GazeboYarpIMU.cc => gazebo_plugins/IMU.cc} (98%) rename src/{GazeboYarpControlBoardDriver.cpp => yarp_drivers/ControlBoardDriver.cpp} (99%) rename src/{GazeboYarpControlBoardDriverControlMode.cpp => yarp_drivers/ControlBoardDriverControlMode.cpp} (98%) rename src/{GazeboYarpControlBoardDriverDeviceDriver.cpp => yarp_drivers/ControlBoardDriverDeviceDriver.cpp} (98%) rename src/{GazeboYarpControlBoardDriverEncoders.cpp => yarp_drivers/ControlBoardDriverEncoders.cpp} (97%) rename src/{GazeboYarpControlBoardDriverOthers.cpp => yarp_drivers/ControlBoardDriverOthers.cpp} (97%) rename src/{GazeboYarpControlBoardDriverPositionControl.cpp => yarp_drivers/ControlBoardDriverPositionControl.cpp} (98%) rename src/{GazeboYarpControlBoardDriverVelocityControl.cpp => yarp_drivers/ControlBoardDriverVelocityControl.cpp} (94%) rename src/{GazeboYarpControlBoardTorqueDriverControl.cpp => yarp_drivers/ControlBoardTorqueDriverControl.cpp} (98%) rename src/{GazeboYarpForceTorqueDriver.cpp => yarp_drivers/ForceTorqueDriver.cpp} (98%) rename src/{ => yarp_drivers}/analogServer.cpp (95%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1377a4f5a..d43b1fd8c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,13 +20,12 @@ link_directories(${GAZEBO_LIBRARY_DIRS}) include_directories(${YARP_INCLUDE_DIRS} include) -add_library(gazebo_yarp_controlboard SHARED src/GazeboYarpControlBoard.cc) -add_library(gazebo_yarp_controlboard_driver SHARED src/GazeboYarpControlBoardDriver.cpp src/GazeboYarpControlBoardDriverControlMode.cpp src/GazeboYarpControlBoardDriverDeviceDriver.cpp src/GazeboYarpControlBoardDriverEncoders.cpp src/GazeboYarpControlBoardDriverOthers.cpp src/GazeboYarpControlBoardDriverPositionControl.cpp src/GazeboYarpControlBoardTorqueDriverControl.cpp src/GazeboYarpControlBoardDriverVelocityControl.cpp) -add_library(gazebo_yarp_forcetorque SHARED src/GazeboYarpForceTorqueDriver.cpp src/GazeboYarpForceTorque.cc src/analogServer.cpp) -add_library(gazebo_yarp_imu SHARED src/GazeboYarpIMU.cc ) +add_library(gazebo_yarp_controlboard SHARED src/gazebo_plugins/ControlBoard.cc src/yarp_drivers/ControlBoardDriver.cpp src/yarp_drivers/ControlBoardDriverControlMode.cpp src/yarp_drivers/ControlBoardDriverDeviceDriver.cpp src/yarp_drivers/ControlBoardDriverEncoders.cpp src/yarp_drivers/ControlBoardDriverOthers.cpp src/yarp_drivers/ControlBoardDriverPositionControl.cpp src/yarp_drivers/ControlBoardTorqueDriverControl.cpp src/yarp_drivers/ControlBoardDriverVelocityControl.cpp) +add_library(gazebo_yarp_forcetorque SHARED src/gazebo_plugins/ForceTorque.cc src/yarp_drivers/ForceTorqueDriver.cpp src/yarp_drivers/analogServer.cpp) +add_library(gazebo_yarp_imu SHARED src/gazebo_plugins/IMU.cc ) -target_link_libraries(gazebo_yarp_controlboard gazebo_yarp_controlboard_driver ${YARP_LIBRARIES} ${GAZEBO_libraries} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_yarp_controlboard ${YARP_LIBRARIES} ${GAZEBO_libraries} ${Boost_LIBRARIES}) target_link_libraries(gazebo_yarp_forcetorque ${YARP_LIBRARIES} ${GAZEBO_libraries} ForceTorquePlugin ${Boost_LIBRARIES}) target_link_libraries(gazebo_yarp_imu ${YARP_LIBRARIES} ${GAZEBO_libraries} ${Boost_LIBRARIES}) diff --git a/include/GazeboYarpControlBoard.hh b/include/gazebo_yarp_plugins/ControlBoard.hh similarity index 97% rename from include/GazeboYarpControlBoard.hh rename to include/gazebo_yarp_plugins/ControlBoard.hh index edf3b7f49..c2a7956f4 100644 --- a/include/GazeboYarpControlBoard.hh +++ b/include/gazebo_yarp_plugins/ControlBoard.hh @@ -14,7 +14,7 @@ #include #include #include -#include +#include #define ms(X) (X * 1000.0) diff --git a/include/GazeboYarpControlBoardDriver.h b/include/gazebo_yarp_plugins/ControlBoardDriver.h similarity index 100% rename from include/GazeboYarpControlBoardDriver.h rename to include/gazebo_yarp_plugins/ControlBoardDriver.h diff --git a/include/GazeboYarpForceTorque.hh b/include/gazebo_yarp_plugins/ForceTorque.hh similarity index 96% rename from include/GazeboYarpForceTorque.hh rename to include/gazebo_yarp_plugins/ForceTorque.hh index 27235223e..ab5662b7d 100644 --- a/include/GazeboYarpForceTorque.hh +++ b/include/gazebo_yarp_plugins/ForceTorque.hh @@ -16,9 +16,9 @@ #include #include #include -#include +#include -#include +#include namespace gazebo diff --git a/include/GazeboYarpForceTorqueDriver.h b/include/gazebo_yarp_plugins/ForceTorqueDriver.h similarity index 100% rename from include/GazeboYarpForceTorqueDriver.h rename to include/gazebo_yarp_plugins/ForceTorqueDriver.h diff --git a/include/GazeboYarpIMU.hh b/include/gazebo_yarp_plugins/IMU.hh similarity index 100% rename from include/GazeboYarpIMU.hh rename to include/gazebo_yarp_plugins/IMU.hh diff --git a/include/analogServer.h b/include/gazebo_yarp_plugins/analogServer.h similarity index 100% rename from include/analogServer.h rename to include/gazebo_yarp_plugins/analogServer.h diff --git a/src/GazeboYarpControlBoard.cc b/src/gazebo_plugins/ControlBoard.cc similarity index 98% rename from src/GazeboYarpControlBoard.cc rename to src/gazebo_plugins/ControlBoard.cc index fb98705f7..da52cf8f6 100644 --- a/src/GazeboYarpControlBoard.cc +++ b/src/gazebo_plugins/ControlBoard.cc @@ -3,7 +3,7 @@ * Authors: Enrico Mingo, Alessio Rocchi, Mirko Ferrati, Silvio Traversaro and Alessandro Settimi * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT */ -#include +#include namespace gazebo { diff --git a/src/GazeboYarpForceTorque.cc b/src/gazebo_plugins/ForceTorque.cc similarity index 98% rename from src/GazeboYarpForceTorque.cc rename to src/gazebo_plugins/ForceTorque.cc index 276d520e0..120995a09 100644 --- a/src/GazeboYarpForceTorque.cc +++ b/src/gazebo_plugins/ForceTorque.cc @@ -13,9 +13,9 @@ #include #include -#include -#include -#include +#include +#include +#include using std::string; diff --git a/src/GazeboYarpIMU.cc b/src/gazebo_plugins/IMU.cc similarity index 98% rename from src/GazeboYarpIMU.cc rename to src/gazebo_plugins/IMU.cc index a6d2cd868..72347bba0 100644 --- a/src/GazeboYarpIMU.cc +++ b/src/gazebo_plugins/IMU.cc @@ -5,7 +5,7 @@ */ -#include "GazeboYarpIMU.hh" +#include #include #include diff --git a/src/GazeboYarpControlBoardDriver.cpp b/src/yarp_drivers/ControlBoardDriver.cpp similarity index 99% rename from src/GazeboYarpControlBoardDriver.cpp rename to src/yarp_drivers/ControlBoardDriver.cpp index 13dc01e2f..2feb833f2 100644 --- a/src/GazeboYarpControlBoardDriver.cpp +++ b/src/yarp_drivers/ControlBoardDriver.cpp @@ -5,7 +5,7 @@ */ -#include +#include #include diff --git a/src/GazeboYarpControlBoardDriverControlMode.cpp b/src/yarp_drivers/ControlBoardDriverControlMode.cpp similarity index 98% rename from src/GazeboYarpControlBoardDriverControlMode.cpp rename to src/yarp_drivers/ControlBoardDriverControlMode.cpp index f7e655fae..9a03614a3 100644 --- a/src/GazeboYarpControlBoardDriverControlMode.cpp +++ b/src/yarp_drivers/ControlBoardDriverControlMode.cpp @@ -5,7 +5,7 @@ */ -#include +#include diff --git a/src/GazeboYarpControlBoardDriverDeviceDriver.cpp b/src/yarp_drivers/ControlBoardDriverDeviceDriver.cpp similarity index 98% rename from src/GazeboYarpControlBoardDriverDeviceDriver.cpp rename to src/yarp_drivers/ControlBoardDriverDeviceDriver.cpp index c3937e61d..372d59387 100644 --- a/src/GazeboYarpControlBoardDriverDeviceDriver.cpp +++ b/src/yarp_drivers/ControlBoardDriverDeviceDriver.cpp @@ -5,7 +5,7 @@ */ -#include +#include #include diff --git a/src/GazeboYarpControlBoardDriverEncoders.cpp b/src/yarp_drivers/ControlBoardDriverEncoders.cpp similarity index 97% rename from src/GazeboYarpControlBoardDriverEncoders.cpp rename to src/yarp_drivers/ControlBoardDriverEncoders.cpp index 808b5e3d0..d143f7106 100644 --- a/src/GazeboYarpControlBoardDriverEncoders.cpp +++ b/src/yarp_drivers/ControlBoardDriverEncoders.cpp @@ -6,7 +6,7 @@ -#include +#include using namespace yarp::dev; diff --git a/src/GazeboYarpControlBoardDriverOthers.cpp b/src/yarp_drivers/ControlBoardDriverOthers.cpp similarity index 97% rename from src/GazeboYarpControlBoardDriverOthers.cpp rename to src/yarp_drivers/ControlBoardDriverOthers.cpp index 2be9f2858..21ec4f4d8 100644 --- a/src/GazeboYarpControlBoardDriverOthers.cpp +++ b/src/yarp_drivers/ControlBoardDriverOthers.cpp @@ -4,7 +4,7 @@ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT */ -#include +#include using namespace yarp::dev; diff --git a/src/GazeboYarpControlBoardDriverPositionControl.cpp b/src/yarp_drivers/ControlBoardDriverPositionControl.cpp similarity index 98% rename from src/GazeboYarpControlBoardDriverPositionControl.cpp rename to src/yarp_drivers/ControlBoardDriverPositionControl.cpp index dea415139..fcc8b512d 100644 --- a/src/GazeboYarpControlBoardDriverPositionControl.cpp +++ b/src/yarp_drivers/ControlBoardDriverPositionControl.cpp @@ -6,7 +6,7 @@ -#include +#include using namespace yarp::dev; diff --git a/src/GazeboYarpControlBoardDriverVelocityControl.cpp b/src/yarp_drivers/ControlBoardDriverVelocityControl.cpp similarity index 94% rename from src/GazeboYarpControlBoardDriverVelocityControl.cpp rename to src/yarp_drivers/ControlBoardDriverVelocityControl.cpp index a24768def..f9764c9ab 100644 --- a/src/GazeboYarpControlBoardDriverVelocityControl.cpp +++ b/src/yarp_drivers/ControlBoardDriverVelocityControl.cpp @@ -5,7 +5,7 @@ */ -#include +#include using namespace yarp::dev; diff --git a/src/GazeboYarpControlBoardTorqueDriverControl.cpp b/src/yarp_drivers/ControlBoardTorqueDriverControl.cpp similarity index 98% rename from src/GazeboYarpControlBoardTorqueDriverControl.cpp rename to src/yarp_drivers/ControlBoardTorqueDriverControl.cpp index 47e2192e3..2b1f5d03b 100644 --- a/src/GazeboYarpControlBoardTorqueDriverControl.cpp +++ b/src/yarp_drivers/ControlBoardTorqueDriverControl.cpp @@ -5,7 +5,7 @@ */ -#include +#include using namespace yarp::dev; diff --git a/src/GazeboYarpForceTorqueDriver.cpp b/src/yarp_drivers/ForceTorqueDriver.cpp similarity index 98% rename from src/GazeboYarpForceTorqueDriver.cpp rename to src/yarp_drivers/ForceTorqueDriver.cpp index ef47a564b..51f713394 100644 --- a/src/GazeboYarpForceTorqueDriver.cpp +++ b/src/yarp_drivers/ForceTorqueDriver.cpp @@ -21,7 +21,7 @@ /// specific to this device driver. -#include +#include #ifdef WIN32 diff --git a/src/analogServer.cpp b/src/yarp_drivers/analogServer.cpp similarity index 95% rename from src/analogServer.cpp rename to src/yarp_drivers/analogServer.cpp index 06dc8ef93..81d5f3a3f 100644 --- a/src/analogServer.cpp +++ b/src/yarp_drivers/analogServer.cpp @@ -1,7 +1,7 @@ -#include "analogServer.h" +#include #define yTrace(); using namespace yarp::sig; From 1c86a048c3c9fc42eeeb6a3b8164968f266126fb Mon Sep 17 00:00:00 2001 From: MirkoFerrati Date: Thu, 14 Nov 2013 12:13:53 +0100 Subject: [PATCH 03/11] Update README.md Added some help for the custom yarp branch compilation --- README.md | 45 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 69a118993..696600e33 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,10 @@ Gazebo plugin to interface a Coman in GAZEBO with a YARP interface. Ubuntu 13.04 system with gazebo 1.9 and Ros hydro -Another way to get coman sdf from the URDF sources +Get coman sdf from the URDF sources ------------------------ -clone the repository iit-coman-ros-pkg +Clone the repository iit-coman-ros-pkg Inside the cloned folder do the following: ``` @@ -23,9 +23,45 @@ source /usr/share/ros-xxx/setup.sh ``` Note: we need to fix the use of ROS to get the sdf file +Another way to get coman sdf without having to use ROS +----------------------- + +Clone the repository github.com/EnricoMingo/iit-coman-ros-pkg + +Inside the cloned folder search for coman_gazebo/sdf folder. +Copy all the folder contents to ~/.gazebo/models/coman_urdf/ + +So you should have something like this: +~/.gazebo/models/coman_urdf/coman.sdf +~/.gazebo/models/coman_urdf/conf/... +~/.gazebo/models/coman_urdf/meshes/... +... + +Done. + + +Get and compile the custom yarp branch +------------------------- + +Clone the repository github.com/barbalberto/yarp/tree/wrappers +Inside the cloned folder do: +``` +mkdir build +cd build +cmake .. +ccmake .. +``` + +Now enable the option "Shared library". Press c to configure. Press g to confirm. + +``` +make +sudo make install +``` + # Compile the libyarp_gazebo -clone the repository +Finally clone this repository ``` mkdir build cd build @@ -197,6 +233,8 @@ yarpscope --xml FT_yarp_scope.xml Troubleshooting ============= +- If the plugin does not compile complaning about an -fPIC option missing, check that you compiled yarp with shared library option enabled + - If gazebo complains about not finding the libyarp, check if you exported the GAZEBO_PLUGIN_PATH in the same shell where you are launching gazebo - If gazebo complains about not finding coman model, maybe you forgot to compile the URDF and run the script provided in the URDF git repository (or to extract the files from the coman.tar.gz) @@ -211,3 +249,4 @@ and check what is the startup configuration. The starting position can be change At the moment it is not so easy to change PID gains, we will implement this and other control modes soon. - Remember to start yarpserver in another shell, otherwise gazebo will run without a working coman interface (we will make gazebo crash in the next versions) + From d8cf5e0200a1f7be089f095bc9efc7b8ebe4e1e9 Mon Sep 17 00:00:00 2001 From: Mirko Date: Thu, 14 Nov 2013 17:55:07 +0100 Subject: [PATCH 04/11] Changed the way velocityControl is implemented, now uses gazebo JointCmd --- src/coman.cpp | 25 ++++++++++++++----------- src/comanControlMode.cpp | 8 ++++---- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/coman.cpp b/src/coman.cpp index e7d993fe8..996c20f07 100644 --- a/src/coman.cpp +++ b/src/coman.cpp @@ -272,6 +272,9 @@ void coman::prepareJointMsg(gazebo::msgs::JointCmd& j_cmd, const int joint_index j_cmd.mutable_position()->set_p_gain(_p[joint_index]); j_cmd.mutable_position()->set_i_gain(_i[joint_index]); j_cmd.mutable_position()->set_d_gain(_d[joint_index]); + j_cmd.mutable_velocity()->set_p_gain(0.0); + j_cmd.mutable_velocity()->set_i_gain(0); + j_cmd.mutable_velocity()->set_d_gain(0); } bool coman::sendVelocitiesToGazebo(yarp::sig::Vector& refs) //NOT TESTED @@ -286,22 +289,22 @@ bool coman::sendVelocitiesToGazebo(yarp::sig::Vector& refs) //NOT TESTED bool coman::sendVelocityToGazebo(int j,double ref) //NOT TESTED { /* SetVelocity method */ - gazebo::physics::JointPtr joint = this->_robot->GetJoint(joint_names[j]); + /*gazebo::physics::JointPtr joint = this->_robot->GetJoint(joint_names[j]); joint->SetMaxForce(0, joint->GetEffortLimit(0)*1.1); //<-- MAGIC NUMBER!!!! // std::cout<<"MaxForce:" <GetMaxForce(0)<SetVelocity(0,toRad(ref)); - + */ /* JointController method. If you pick this control method for control * of joint velocities, you should also take care of the switching logic * in setVelocityMode, setTorqueMode and setPositionMode: * that is, the SetMarxForce(0,0) and SetVelocity(0,0) are no longer * needed, but the JointController::AddJoint() method needs to be called - * when you switch to velocity mode, to make sure the PIDs get reset - // gazebo::msgs::JointCmd j_cmd; - // prepareJointVelocityMsg(j_cmd,j,ref); - // jointCmdPub->WaitForConnection(); - // jointCmdPub->Publish(j_cmd); - */ + * when you switch to velocity mode, to make sure the PIDs get reset */ + gazebo::msgs::JointCmd j_cmd; + prepareJointVelocityMsg(j_cmd,j,ref); + jointCmdPub->WaitForConnection(); + jointCmdPub->Publish(j_cmd); + /**/ return true; } @@ -311,9 +314,9 @@ void coman::prepareJointVelocityMsg(gazebo::msgs::JointCmd& j_cmd, const int j, j_cmd.mutable_position()->set_p_gain(0.0); j_cmd.mutable_position()->set_i_gain(0.0); j_cmd.mutable_position()->set_d_gain(0.0); - j_cmd.mutable_velocity()->set_p_gain(5000); - j_cmd.mutable_velocity()->set_i_gain(0.0); - j_cmd.mutable_velocity()->set_d_gain(10); + j_cmd.mutable_velocity()->set_p_gain(0.200); + j_cmd.mutable_velocity()->set_i_gain(0.02); + j_cmd.mutable_velocity()->set_d_gain(0); j_cmd.mutable_velocity()->set_target(toRad(ref)); } diff --git a/src/comanControlMode.cpp b/src/comanControlMode.cpp index c4191296d..a1f5b00c4 100644 --- a/src/comanControlMode.cpp +++ b/src/comanControlMode.cpp @@ -14,12 +14,12 @@ using namespace yarp::dev; bool coman::setPositionMode(int j) //WORKS { /* WARNING: disabling velocity mode. This is needed as long as we use - * the SetVelocity method for velocity control*/ + * the SetVelocity method for velocity control if(control_mode[j]==VOCAB_CM_VELOCITY) { gazebo::physics::JointPtr joint = this->_robot->GetJoint(joint_names[j]); joint->SetMaxForce(0, 0); joint->SetVelocity(0,0); - } + }*/ // resetting controller PIDs this->_robot->GetJointController()->AddJoint(this->_robot->GetJoint(joint_names[j])); @@ -32,8 +32,8 @@ bool coman::setVelocityMode(int j) //WORKS { /* TODO: this is needed if we want to control velocities using JointController * // resetting controller PIDs - * this->_robot->GetJointController()->AddJoint(this->_robot->GetJoint(joint_names[j])); - */ + */ this->_robot->GetJointController()->AddJoint(this->_robot->GetJoint(joint_names[j])); + /**/ control_mode[j]=VOCAB_CM_VELOCITY; std::cout<<"control mode = speed "< Date: Thu, 14 Nov 2013 17:56:03 +0100 Subject: [PATCH 05/11] Added a simple jointLogger and commented some lines that show how to use it --- include/coman.h | 3 +++ src/coman.cpp | 8 ++++++-- src/test/jointlogger.hpp | 37 +++++++++++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+), 2 deletions(-) create mode 100644 src/test/jointlogger.hpp diff --git a/include/coman.h b/include/coman.h index 921c22b1a..d50c2bdb6 100644 --- a/include/coman.h +++ b/include/coman.h @@ -31,6 +31,7 @@ #pragma GCC diagnostic pop #include +#include "../src/test/jointlogger.hpp" #define toRad(X) (X*M_PI/180.0) const double ROBOT_POSITION_TOLERANCE=0.9; @@ -229,6 +230,8 @@ class yarp::dev::coman : bool started; int _clock; int _T_controller; + + jointLogger logger; yarp::os::Port _joint_torq_port; yarp::os::Port _joint_speed_port; diff --git a/src/coman.cpp b/src/coman.cpp index 996c20f07..7d57d847e 100644 --- a/src/coman.cpp +++ b/src/coman.cpp @@ -6,11 +6,13 @@ #include +#include "test/jointlogger.hpp" #include #include #include #include +#include using namespace yarp::os; @@ -55,7 +57,6 @@ void coman::gazebo_init() setMinMaxPos(); setPIDs(); - pos = 0; zero_pos=0; vel = 0; @@ -82,6 +83,7 @@ void coman::gazebo_init() _T_controller = 10; +// logger.initialize(joint_names[2],"speed"); } @@ -107,7 +109,9 @@ void coman::onUpdate ( const gazebo::common::UpdateInfo & /*_info*/ ) torque[jnt_cnt] = this->_robot->GetJoint ( joint_names[jnt_cnt] )->GetForce ( 0 ); } pos_lock.unlock(); - + +// logger.log(speed[2]); + for ( unsigned int j=0; j<_robot_number_of_joints; ++j ) { if ( control_mode[j]==VOCAB_CM_POSITION ) //set pos joint value, set vel joint value diff --git a/src/test/jointlogger.hpp b/src/test/jointlogger.hpp new file mode 100644 index 000000000..6bbcd9013 --- /dev/null +++ b/src/test/jointlogger.hpp @@ -0,0 +1,37 @@ +#ifndef JOINT_LOGGER_HPP +#define JOINT_LOGGER_HPP +#include + + + + +class jointLogger{ + +public: +jointLogger(){ + +} + +void log(double d) +{ + if (logger!=0) + { + fprintf(logger,"%f \n",d); + } +} + +bool initialize(std::string joint_name,const char* filename) +{ + this->joint_name=joint_name; + char temp[100]; + sprintf(temp,"%s%s.txt",filename,joint_name.c_str()); + logger=fopen(temp,"w"); +} + +private: +std::string joint_name=""; +FILE* logger=0; +}; + + +#endif //JOINT_LOGGER_HPP \ No newline at end of file From 89928951ba6366bd72222521ed175d4e8af278a7 Mon Sep 17 00:00:00 2001 From: Mirko Date: Fri, 15 Nov 2013 12:00:13 +0100 Subject: [PATCH 06/11] Deleted c++11 features from code, the code works again with 12.04 --- src/test/jointlogger.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/test/jointlogger.hpp b/src/test/jointlogger.hpp index 6bbcd9013..ae7418067 100644 --- a/src/test/jointlogger.hpp +++ b/src/test/jointlogger.hpp @@ -9,7 +9,8 @@ class jointLogger{ public: jointLogger(){ - + joint_name=""; + logger=0; } void log(double d) @@ -29,8 +30,8 @@ bool initialize(std::string joint_name,const char* filename) } private: -std::string joint_name=""; -FILE* logger=0; +std::string joint_name; +FILE* logger; }; From 4477b59c60eff38bc0a237b96f79698730e928c2 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 15 Nov 2013 23:38:29 +0900 Subject: [PATCH 07/11] Fixed include of joint logger --- src/yarp_drivers/ControlBoardDriver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/yarp_drivers/ControlBoardDriver.cpp b/src/yarp_drivers/ControlBoardDriver.cpp index 84b93a81f..1a49e1a42 100644 --- a/src/yarp_drivers/ControlBoardDriver.cpp +++ b/src/yarp_drivers/ControlBoardDriver.cpp @@ -6,7 +6,7 @@ #include -#include "test/jointlogger.hpp" +#include "../test/jointlogger.hpp" #include #include From 71a3fcfd468e3d51d1a7398c0cce9d130eda1bf1 Mon Sep 17 00:00:00 2001 From: Alberto Cardellino Date: Fri, 15 Nov 2013 16:31:32 +0100 Subject: [PATCH 08/11] removed include --- include/gazebo_yarp_plugins/ForceTorqueDriver.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/include/gazebo_yarp_plugins/ForceTorqueDriver.h b/include/gazebo_yarp_plugins/ForceTorqueDriver.h index f3c80b611..58da0bc8c 100644 --- a/include/gazebo_yarp_plugins/ForceTorqueDriver.h +++ b/include/gazebo_yarp_plugins/ForceTorqueDriver.h @@ -18,7 +18,6 @@ #include #include -#include #include #include #include @@ -27,8 +26,7 @@ #include -#include -#include + namespace yarp{ namespace dev{ From ce82dd0d679c851b1bcb897f3a093542f834ee25 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sat, 16 Nov 2013 23:38:33 -0800 Subject: [PATCH 09/11] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 696600e33..a09a4e526 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,10 @@ So you should have something like this: Done. +Get iCub sdf model +------------------ +Follow the instruction provided in this repo: https://github.com/traversaro/icub_gazebo + Get and compile the custom yarp branch ------------------------- From e56f22178a65987b3dc58a6db96c3020af965178 Mon Sep 17 00:00:00 2001 From: MirkoFerrati Date: Mon, 18 Nov 2013 13:30:17 +0100 Subject: [PATCH 10/11] Update README.md Added some basic instructions to install iDynTree --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index a09a4e526..552b80bed 100644 --- a/README.md +++ b/README.md @@ -235,6 +235,11 @@ yarpscope --xml FT_yarp_scope.xml ``` +Install iDynTree to do inverse dynamics and kinematics +--------------- +https://github.com/robotology/gazebo_yarp_plugins/wiki/Setup-iDynTree + + Troubleshooting ============= - If the plugin does not compile complaning about an -fPIC option missing, check that you compiled yarp with shared library option enabled From 55ae1051d4409471f06b67e49d2bfaa8b0cd9062 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 19 Nov 2013 02:14:12 +0900 Subject: [PATCH 11/11] Added instruction on compiling the new branch --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 552b80bed..387b3cbaa 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,7 @@ cmake .. ccmake .. ``` -Now enable the option "Shared library". Press c to configure. Press g to confirm. +Now enable the options CREATE_SHARED_LIBRARY and NEW_WRAPPERS_WIP . Press c to configure. Press g to confirm. ``` make