Merge Alberto's changes and master

Resolved Conflicts:
	CMakeLists.txt
	include/gazebo_yarp_plugins/ForceTorqueDriver.h
	src/yarp_drivers/ControlBoardDriver.cpp
	src/yarp_drivers/ControlBoardDriverDeviceDriver.cpp

CMakeLists.txt

@@ -20,30 +20,12 @@ link_directories(${GAZEBO_LIBRARY_DIRS})
 
 include_directories(${YARP_INCLUDE_DIRS} include)
 
+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 )
 
-add_library(gazebo_yarp_controlboard SHARED src/GazeboYarpControlBoard.cc include/GazeboYarpControlBoard.hh)
-add_library(gazebo_yarp_controlboard_driver SHARED
-                            include/GazeboYarpControlBoardDriver.h
-                            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  include/analogServer.h
-                                            include/GazeboYarpForceTorque.hh
-                                            include/GazeboYarpForceTorqueDriver.h
-                                            src/GazeboYarpForceTorqueDriver.cpp
-                                            src/GazeboYarpForceTorque.cc
-                                            src/analogServer.cpp)
-
-add_library(gazebo_yarp_imu SHARED include/GazeboYarpIMU.hh src/GazeboYarpIMU.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})
 

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,49 @@ 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 iCub sdf model
+------------------
+Follow the instruction provided in this repo: https://github.com/traversaro/icub_gazebo
+
+
+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 options CREATE_SHARED_LIBRARY and NEW_WRAPPERS_WIP . 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
@@ -195,8 +235,15 @@ 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
+
 - 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 +258,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)
+

include/GazeboYarpControlBoard.hh → include/gazebo_yarp_plugins/ControlBoard.hh

@@ -16,7 +16,7 @@
 #include <yarp/dev/PolyDriverList.h>
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
-#include <GazeboYarpControlBoardDriver.h>
+#include <gazebo_yarp_plugins/ControlBoardDriver.h>
 
 
 #define ms(X) (X * 1000.0)

include/GazeboYarpControlBoardDriver.h → include/gazebo_yarp_plugins/ControlBoardDriver.h

@@ -32,6 +32,7 @@
 #pragma GCC diagnostic pop
 
 #include <mutex>
+#include "../src/test/jointlogger.hpp"
 
 #define toRad(X) (X*M_PI/180.0)
 const double ROBOT_POSITION_TOLERANCE=0.9;
@@ -245,6 +246,8 @@ class yarp::dev::GazeboYarpControlBoardDriver :
     bool started;
     int _clock;
     int _T_controller;
+
+    jointLogger logger;
 
     yarp::os::Port _joint_torq_port;
     yarp::os::Port _joint_speed_port;

include/GazeboYarpForceTorque.hh → include/gazebo_yarp_plugins/ForceTorque.hh

@@ -16,9 +16,9 @@
 #include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/os/Network.h>
-#include <analogServer.h>
+#include <gazebo_yarp_plugins/analogServer.h>
 
-#include <GazeboYarpForceTorqueDriver.h>
+#include <gazebo_yarp_plugins/ForceTorqueDriver.h>
 
 
 namespace gazebo

include/GazeboYarpForceTorqueDriver.h → include/gazebo_yarp_plugins/ForceTorqueDriver.h

@@ -18,6 +18,13 @@
 #include <yarp/dev/DeviceDriver.h>
 #include <yarp/dev/IAnalogSensor.h>
 
+#include <yarp/os/Semaphore.h>
+#include <string>
+#include <list>
+#include <map>
+
+#include <boost/concept_check.hpp>
+
 #include <yarp/os/Semaphore.h>
 #include <yarp/os/RateThread.h>
 #include <string>

src/GazeboYarpControlBoard.cc → 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 <GazeboYarpControlBoard.hh>
+#include <gazebo_yarp_plugins/ControlBoard.hh>
 
 namespace gazebo
 {

src/GazeboYarpForceTorque.cc → src/gazebo_plugins/ForceTorque.cc

@@ -13,9 +13,9 @@
 #include <gazebo/transport/Node.hh>
 #include <gazebo/common/Assert.hh>
 
-#include <GazeboYarpForceTorque.hh>
-#include <GazeboYarpForceTorqueDriver.h>
-#include <analogServer.h>
+#include <gazebo_yarp_plugins/ForceTorque.hh>
+#include <gazebo_yarp_plugins/ForceTorqueDriver.h>
+#include <gazebo_yarp_plugins/analogServer.h>
 
 using std::string;
 

src/GazeboYarpIMU.cc → src/gazebo_plugins/IMU.cc

@@ -5,7 +5,7 @@
  */
 
 
-#include "GazeboYarpIMU.hh"
+#include <gazebo_yarp_plugins/IMU.hh>
 #include <yarp/dev/ServerInertial.h>
 #include <yarp/dev/PolyDriver.h>
 
@@ -94,9 +94,9 @@ void GazeboYarpIMU::OnUpdate()
 //               imu_data.mutable_linear_acceleration()->y()<<" "<<
 //               imu_data.mutable_linear_acceleration()->z()<<std::endl;
 
-    _bot.addDouble( toDeg(orient.GetRoll()));
-    _bot.addDouble( toDeg(orient.GetPitch()));
-    _bot.addDouble( toDeg(orient.GetYaw()));
+    _bot.addDouble( toDeg(0.0));
+    _bot.addDouble( toDeg(0.0));
+    _bot.addDouble( toDeg(0.0));
 
     yarp::os::Stamp ts;
     ts = yarp::os::Stamp(0, imu_data.mutable_stamp()->sec());

src/test/jointlogger.hpp

@@ -0,0 +1,38 @@
+#ifndef JOINT_LOGGER_HPP
+#define JOINT_LOGGER_HPP
+#include <string>
+
+
+
+
+class jointLogger{
+
+public:
+jointLogger(){
+    joint_name="";
+    logger=0;
+}    
+
+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;    
+};
+
+
+#endif //JOINT_LOGGER_HPP

src/GazeboYarpControlBoardDriver.cpp → src/yarp_drivers/ControlBoardDriver.cpp

@@ -5,13 +5,14 @@
  */
 
 
-#include <GazeboYarpControlBoardDriver.h>
-
+#include <gazebo_yarp_plugins/ControlBoardDriver.h>
+#include "../test/jointlogger.hpp"
 
 #include <yarp/sig/all.h>
 #include <yarp/sig/ImageFile.h>
 #include <yarp/os/all.h>
 #include <boost/archive/text_iarchive.hpp>
+#include <stdio.h>
 
 
 using namespace yarp::os;
@@ -56,7 +57,6 @@ void GazeboYarpControlBoardDriver::gazebo_init()
 
     setMinMaxPos();
     setPIDs();
-
     pos = 0;
     zero_pos=0;
     vel = 0;
@@ -84,7 +84,12 @@ void GazeboYarpControlBoardDriver::gazebo_init()
                   ( std::string ( "~/" ) + this->_robot->GetName() + "/joint_cmd" );
 
     _T_controller = 10;
+<<<<<<< HEAD:src/GazeboYarpControlBoardDriver.cpp
     std::cout << "gazebo_init ended!!" << std::endl;
+=======
+
+//     logger.initialize(joint_names[2],"speed");
+>>>>>>> master:src/yarp_drivers/ControlBoardDriver.cpp
 }
 
 
@@ -110,7 +115,9 @@ void GazeboYarpControlBoardDriver::onUpdate ( const gazebo::common::UpdateInfo &
         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
@@ -282,6 +289,9 @@ void GazeboYarpControlBoardDriver::prepareJointMsg(gazebo::msgs::JointCmd& j_cmd
     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 GazeboYarpControlBoardDriver::sendVelocitiesToGazebo(yarp::sig::Vector& refs) //NOT TESTED
@@ -296,22 +306,22 @@ bool GazeboYarpControlBoardDriver::sendVelocitiesToGazebo(yarp::sig::Vector& ref
 bool GazeboYarpControlBoardDriver::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:" <<joint->GetMaxForce(0)<<std::endl;
     joint->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;
 }
 
@@ -321,9 +331,9 @@ void GazeboYarpControlBoardDriver::prepareJointVelocityMsg(gazebo::msgs::JointCm
     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));
 }
 

src/GazeboYarpControlBoardDriverControlMode.cpp → src/yarp_drivers/ControlBoardDriverControlMode.cpp

@@ -5,7 +5,7 @@
  */
 
 
-#include <GazeboYarpControlBoardDriver.h>
+#include <gazebo_yarp_plugins/ControlBoardDriver.h>
 
 
 
@@ -14,12 +14,12 @@ using namespace yarp::dev;
 bool GazeboYarpControlBoardDriver::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 GazeboYarpControlBoardDriver::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 "<<j<<std::endl;
      return true;