diff --git a/CMakeLists.txt b/CMakeLists.txt
index c5e5fa5916c6f1692ac3ebfafb8251284be01fca..e27916e2aae5d0e07092bb5d1cd4c45a8ad3cc8a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,17 @@ catkin_package(
 # Find xml_reflection
 ADD_PROJECT_DEPENDENCY(urdfdom REQUIRED)
 ADD_PROJECT_DEPENDENCY(gazebo REQUIRED)
-ADD_COMPILE_DEPENDENCY(roscpp)
+ADD_PROJECT_DEPENDENCY(roscpp)
+
+# Needed due to the CMakeTarget from gazebo.
+INCLUDE_DIRECTORIES(${GAZEBO_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS})
+
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+
+
+SET(LIBRARY_NAME ${PROJECT_NAME})
 
 ADD_LIBRARY(${PROJECT_NAME}
   SHARED
diff --git a/README.md b/README.md
index 2e4cbb6ab70c7cf01f53438329f2456946e4f861..010538c64bc04ee9e6aecf7d1c3b73f58b0be472 100644
--- a/README.md
+++ b/README.md
@@ -15,6 +15,9 @@ Some dummy data were added (rotor inertia) to help the dynamic regularization.
 For initial and data validated by PAL-Robotics please see the http://github.com/pal-robotics/talos_robot
 repository.
 
+If you have problem in starting the robot in a specific position you should look at the 
+[python section](#python)
+
 ## Fixed joint
 To start the robot in the air you can use:
 ```
@@ -95,7 +98,13 @@ Finally to have the state of the joint in the topic */joint_states* you need to
 
 This is telling to ros_control that the passive joint has Position and Effort interfaces.
 
+## Python for launching nodes
 
+In the directory scripts you can start a complete simulation by doing:
+```
+./start_talos_gazebo.py
+```
+In this specific case it will start the robot with a wide base.
 
 
 
diff --git a/launch/talos_gazebo_alone.launch b/launch/talos_gazebo_alone.launch
new file mode 100644
index 0000000000000000000000000000000000000000..b3dff9d6cf271d9177af1be155938981803e9cab
--- /dev/null
+++ b/launch/talos_gazebo_alone.launch
@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="start_half_sitting" default="true"/>
+  <arg name="world"  default="empty"/>  <!-- empty, small_office, ... (see ../worlds) -->
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.16 -R 0.0 -P 0.0 -Y 0.0" unless="$(arg start_half_sitting)"/>
+  <arg name="gui" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="recording" default="false"/>
+  <arg name="extra_gazebo_args" default=""/>
+
+  <arg name="load_model" default="true"/> <!-- AS: should probably be false by default -->
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  <arg name="foot_collision" default="thinbox"/>
+  <arg name="enable_leg_passive" default="false"/>
+  <arg name="enable_fixed_robot" default="false"/>
+  <arg name="default_configuration_type" default="zeros"/>
+
+  <env name="GAZEBO_MODEL_PATH" value="$(find talos_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
+
+  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
+  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
+  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.00 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" if="$(arg start_half_sitting)"/>
+
+  <!-- start up world -->
+  <include file="$(find pal_gazebo_worlds)/launch/pal_gazebo.launch">
+    <arg name="world" value="$(arg world)"/>
+    <arg name="gui" value="$(arg gui)"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="recording" value="$(arg recording)"/>
+    <arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
+  </include>
+
+
+  <group if="$(arg load_model)">
+    <include file="$(find talos_data)/robots/upload.launch" >
+      <arg name="robot" value="$(arg robot)"/>
+      <arg name="foot_collision" default="$(arg foot_collision)"/>
+      <arg name="enable_leg_passive" default="$(arg enable_leg_passive)"/>
+      <arg name="enable_fixed_robot" default="$(arg enable_fixed_robot)"/>
+      <arg name="default_configuration_type" default="$(arg default_configuration_type)"/>
+    </include>
+  </group>
+
+
+  <!-- spawn robot in simulation -->
+  <!-- <include file="$(find talos_gazebo)/launch/talos_spawn.launch"> -->
+  <!--   <arg name="robot"  value="$(arg robot)"/> -->
+  <!--   <arg name="gzpose" value="$(arg gzpose)"/> -->
+  <!-- </include> -->
+
+  <!-- default controllers -->
+  <!-- <include file="$(find talos_controller_configuration)/launch/talos_default_controllers.launch"/> -->
+  <!-- <include file="$(find talos_bringup)/launch/talos_bringup.launch" /> -->
+</launch>
diff --git a/launch/talos_gazebo_spawn_hs.launch b/launch/talos_gazebo_spawn_hs.launch
new file mode 100644
index 0000000000000000000000000000000000000000..36cecbc419a208ee4802490803a0ad40e12bf40c
--- /dev/null
+++ b/launch/talos_gazebo_spawn_hs.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
+  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
+  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.02 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" />
+
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  
+  <include file="$(find talos_gazebo)/launch/talos_spawn.launch">
+  <arg name="robot"  value="$(arg robot)"/>
+  <arg name="gzpose" value="$(arg gzpose)"/>
+  </include>
+  
+</launch>
diff --git a/launch/talos_gazebo_spawn_hs_wide.launch b/launch/talos_gazebo_spawn_hs_wide.launch
new file mode 100644
index 0000000000000000000000000000000000000000..36cecbc419a208ee4802490803a0ad40e12bf40c
--- /dev/null
+++ b/launch/talos_gazebo_spawn_hs_wide.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="left_leg_pose" default="-J leg_left_1_joint 0.0 -J leg_left_2_joint 0.0 -J leg_left_3_joint -0.411354 -J leg_left_4_joint 0.859395 -J leg_left_5_joint -0.448041 -J leg_left_6_joint -0.001708"/>
+  <arg name="right_leg_pose" default="-J leg_right_1_joint 0.0 -J leg_right_2_joint 0.0 -J leg_right_3_joint -0.411354 -J leg_right_4_joint 0.859395 -J leg_right_5_joint -0.448041 -J leg_right_6_joint -0.001708"/>
+  <arg name="left_arm_pose" default="-J arm_left_1_joint 0.25847 -J arm_left_2_joint 0.173046 -J arm_left_3_joint -0.0002 -J arm_left_4_joint -0.525366 -J arm_left_5_joint 0.0 -J arm_left_6_joint 0.0 -J arm_left_7_joint 0.1"/>
+  <arg name="right_arm_pose" default="-J arm_right_1_joint -0.25847 -J arm_right_2_joint -0.173046 -J arm_left_3_joint 0.0002 -J arm_right_4_joint -0.525366 -J arm_right_5_joint 0.0 -J arm_right_6_joint 0.0 -J arm_right_7_joint 0.1"/>
+  <arg name="torso_pose" default="-J torso_1_joint 0.0 -J torso_2_joint 0.006761"/>
+  <arg name="gzpose" default="-x 0.0 -y 0.0 -z 1.02 -R 0.0 -P 0.0 -Y 0.0 $(arg left_leg_pose) $(arg right_leg_pose) $(arg left_arm_pose) $(arg right_arm_pose) $(arg torso_pose) -u" />
+
+  <arg name="robot"  default="full_v2"/>          <!-- full, lower_body, foot -->
+  
+  <include file="$(find talos_gazebo)/launch/talos_spawn.launch">
+  <arg name="robot"  value="$(arg robot)"/>
+  <arg name="gzpose" value="$(arg gzpose)"/>
+  </include>
+  
+</launch>
diff --git a/package.xml b/package.xml
index 47bf14104a7de4c6ea002944bd31b2fee95a5e0c..00af3b492d3293ab5bc6e99f454063ccd1c7cd2b 100644
--- a/package.xml
+++ b/package.xml
@@ -14,10 +14,12 @@
 
   <exec_depend>talos_description_calibration</exec_depend>
   <exec_depend>talos_description_inertial</exec_depend>
-
+  <exec_depend>roscpp</exec_depend>
+  
   <test_depend>rostest</test_depend>
   <test_depend>urdf_test</test_depend>
-
+  <test_depend>roscpp</test_depend>
+  
   <export>
   </export>
 
diff --git a/scripts/start_talos_gazebo.py b/scripts/start_talos_gazebo.py
new file mode 100755
index 0000000000000000000000000000000000000000..7966da78749665290d224c28d8d872ba723335d8
--- /dev/null
+++ b/scripts/start_talos_gazebo.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+# O. Stasse 17/01/2020
+# LAAS, CNRS
+
+import os
+import rospy
+import time
+import roslaunch
+from std_srvs.srv import Empty
+
+# Start roscore
+import subprocess
+roscore = subprocess.Popen('roscore')
+time.sleep(1)
+
+# Start talos_gazebo
+rospy.init_node('starting_talos_gazebo', anonymous=True)
+uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
+roslaunch.configure_logging(uuid)
+
+launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_alone.launch"])
+launch_gazebo_alone.start()
+rospy.loginfo("talos_gazebo_alone started")
+
+rospy.wait_for_service("/gazebo/pause_physics")
+gazebo_pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
+gazebo_pause_physics()
+
+time.sleep(5)
+# Spawn talos model in gazebo
+launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_spawn_hs_wide.launch"])
+launch_gazebo_spawn_hs.start()
+rospy.loginfo("talos_gazebo_spawn_hs started")
+
+rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")
+time.sleep(5)
+gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
+gazebo_unpause_physics()
+
+# Start roscontrol
+launch_bringup = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/talos_bringup/launch/talos_bringup.launch"])
+launch_bringup.start()
+rospy.loginfo("talos_bringup started")
+
+# Start sot
+launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(uuid, ["/opt/openrobots/share/roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch"])
+launch_roscontrol_sot_talos.start()
+rospy.loginfo("roscontrol_sot_talos started")
+
+rospy.spin()
+
diff --git a/src/SpringPlugin.cc b/src/SpringPlugin.cc
index 957c8b5ee6facd92b6a1c8ac823a40bbab9470db..fe442ab276ac594eee1a190be71d51909ecb248b 100644
--- a/src/SpringPlugin.cc
+++ b/src/SpringPlugin.cc
@@ -15,9 +15,8 @@
  *
  */
 
-#include "gazebo/physics/physics.hh"
 #include "SpringPlugin.hh"
-#include <ros/ros.h>
+
 
 using namespace gazebo;
 
@@ -27,50 +26,61 @@ GZ_REGISTER_MODEL_PLUGIN(SpringPlugin)
 SpringPlugin::SpringPlugin() {}
 
 /////////////////////////////////////////////////
-void SpringPlugin::Load(physics::ModelPtr lmodel, sdf::ElementPtr lsdf) {
-  this->model = lmodel;
+void SpringPlugin::Load(physics::ModelPtr lmodel,
+                           sdf::ElementPtr lsdf)
+{
+  model_ = lmodel;
 
   // hardcoded params for this test
   if (!lsdf->HasElement("joint_spring"))
     ROS_ERROR_NAMED("SpringPlugin", "No field joint_spring for SpringPlugin");
   else
-    this->jointExplicitName = lsdf->Get<std::string>("joint_spring");
+    jointExplicitName_ = lsdf->Get<std::string>("joint_spring");
+
+  kpExplicit_ = lsdf->Get<double>("kp");
 
-  this->kpExplicit = lsdf->Get<double>("kp");
+  kdExplicit_ = lsdf->Get<double>("kd");
 
-  this->kdExplicit = lsdf->Get<double>("kd");
+  axisExplicit_ = lsdf->Get<int>("axis");
 
-  ROS_INFO_NAMED("SpringPlugin", "Loading joint : %s kp: %f kd: %f", this->jointExplicitName.c_str(), this->kpExplicit,
-                 this->kdExplicit);
+  ROS_INFO_NAMED("SpringPlugin",
+                 "Loading joint : %s kp: %f kd: %f alongs %d axis",
+                 jointExplicitName_.c_str(),
+                 kpExplicit_,
+                 kdExplicit_,
+                 axisExplicit_);
 }
 
 /////////////////////////////////////////////////
-void SpringPlugin::Init() {
-  this->jointExplicit = this->model->GetJoint(this->jointExplicitName);
+void SpringPlugin::Init()
+{
+  jointExplicit_ = model_->GetJoint(jointExplicitName_);
 
-  /*  this->jointImplicit->SetStiffnessDamping(0, this->kpImplicit,
-      this->kdImplicit); */
+  /*  jointImplicit->SetStiffnessDamping(0, kpImplicit,
+      kdImplicit); */
 
-  this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&SpringPlugin::ExplicitUpdate, this));
+  updateConnection_ = event::Events::ConnectWorldUpdateBegin(
+          boost::bind(&SpringPlugin::ExplicitUpdate, this));
 }
 
 /////////////////////////////////////////////////
 void SpringPlugin::ExplicitUpdate() {
 #if GAZEBO_MAJOR_VERSION < 9
-  common::Time currTime = this->model->GetWorld()->GetSimTime();
+  common::Time currTime = model_->GetWorld()->GetSimTime();
 #else
-  common::Time currTime = this->model->GetWorld()->SimTime();
+  common::Time currTime = model_->GetWorld()->SimTime();
 #endif
 
-  common::Time stepTime = currTime - this->prevUpdateTime;
-  this->prevUpdateTime = currTime;
+  common::Time stepTime = currTime - prevUpdateTime_;
+  prevUpdateTime_ = currTime;
 
 #if GAZEBO_MAJOR_VERSION < 9
-  double pos = this->jointExplicit->GetAngle(0).Radian();
+  double pos = jointExplicit_->GetAngle(axisExplicit_).Radian();
 #else
-  double pos = this->jointExplicit->Position(0);
+  double pos = jointExplicit_->Position(axisExplicit_);
 #endif
-  double vel = this->jointExplicit->GetVelocity(0);
-  double force = -this->kpExplicit * pos - this->kdExplicit * vel;
-  this->jointExplicit->SetForce(0, force);
+  double vel = jointExplicit_->GetVelocity(axisExplicit_);
+  double force = -kpExplicit_ * pos
+                 -kdExplicit_ * vel;
+  jointExplicit_->SetForce(axisExplicit_, force);
 }
diff --git a/src/SpringPlugin.hh b/src/SpringPlugin.hh
index 19d5bbd774ffa12958637f960e6486bfb0c3c4f2..3ded1d20dfdcb8b6a17b87e8bb6b6771b4e6122c 100644
--- a/src/SpringPlugin.hh
+++ b/src/SpringPlugin.hh
@@ -18,47 +18,43 @@
 #define __GAZEBO_SPRING_TEST_PLUGIN_HH__
 
 #include <string>
+#pragma GCC diagnostic push
+#pragma GCC system_header
 
 #include "gazebo/common/Plugin.hh"
 #include "gazebo/physics/physics.hh"
 #include "gazebo/util/system.hh"
+#include <ros/ros.h>
 
-namespace gazebo {
-class GAZEBO_VISIBLE SpringPlugin : public ModelPlugin {
- public:
-  SpringPlugin();
+#pragma GCC diagnostic pop
 
- public:
-  virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+namespace gazebo
+{
+class GAZEBO_VISIBLE SpringPlugin : public ModelPlugin
+{
+public: SpringPlugin();
+public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+public: virtual void Init();
 
- public:
-  virtual void Init();
+private: void ExplicitUpdate();
 
- private:
-  void ExplicitUpdate();
+private: event::ConnectionPtr updateConnection_;
 
- private:
-  event::ConnectionPtr updateConnection;
+private: physics::ModelPtr model_;
 
- private:
-  physics::ModelPtr model;
+private: common::Time prevUpdateTime_;
 
- private:
-  common::Time prevUpdateTime;
-
- private:
-  physics::JointPtr jointExplicit;
-
- private:
-  std::string jointExplicitName;
+private: physics::JointPtr jointExplicit_;
+private: std::string jointExplicitName_;
 
   /// \brief simulate spring/damper with ExplicitUpdate function
- private:
-  double kpExplicit;
+private: double kpExplicit_;
 
   /// \brief simulate spring/damper with ExplicitUpdate function
- private:
-  double kdExplicit;
+private: double kdExplicit_;
+    
+  /// \brief Specify on which axis the spring is applied.
+private: int axisExplicit_;
 };
-}  // namespace gazebo
+}
 #endif
diff --git a/srdf/talos_wpg.srdf b/srdf/talos_wpg.srdf
index 9f1d6a84c10e835481fb7dbe51a04824bb7efc39..05d248b9af6897857814fc991f439fdb0f02de16 100644
--- a/srdf/talos_wpg.srdf
+++ b/srdf/talos_wpg.srdf
@@ -163,7 +163,42 @@
         <joint name="torso_1_joint" value="0" />
         <joint name="torso_2_joint" value="0.006761" />
     </group_state>
- 
+
+    <group_state name="half_sitting_wide" group="all">
+        <joint name="root_joint" value="0. 0. 1.01927 0. 0. 0. 1." />
+        <joint name="arm_left_1_joint" value="0.25" />
+        <joint name="arm_left_2_joint" value="0.173046" />
+        <joint name="arm_left_3_joint" value="-0.0002" />
+        <joint name="arm_left_4_joint" value="-0.525366" />
+        <joint name="arm_left_5_joint" value="0" />
+        <joint name="arm_left_6_joint" value="0" />
+        <joint name="arm_left_7_joint" value="0.1" />
+        <joint name="arm_right_1_joint" value="-0.25" />
+        <joint name="arm_right_2_joint" value="-0.173046" />
+        <joint name="arm_right_3_joint" value="0.0002" />
+        <joint name="arm_right_4_joint" value="-0.525366" />
+        <joint name="arm_right_5_joint" value="0" />
+        <joint name="arm_right_6_joint" value="0" />
+        <joint name="arm_right_7_joint" value="0.1" />
+        <joint name="gripper_left_joint" value="0" />
+        <joint name="gripper_right_joint" value="0" />	
+        <joint name="head_1_joint" value="0" />
+        <joint name="head_2_joint" value="0" />
+        <joint name="leg_left_1_joint" value="0.0" />
+        <joint name="leg_left_2_joint" value="0.06" />
+        <joint name="leg_left_3_joint" value="-0.4096" />
+        <joint name="leg_left_4_joint" value="0.8568" />
+        <joint name="leg_left_5_joint" value="-0.4472" />
+        <joint name="leg_left_6_joint" value="-0.0600" />
+        <joint name="leg_right_1_joint" value="0.0" />
+        <joint name="leg_right_2_joint" value="-0.06" />
+        <joint name="leg_right_3_joint" value="-0.4096" />
+        <joint name="leg_right_4_joint" value="0.8568" />
+        <joint name="leg_right_5_joint" value="-0.4472" />
+        <joint name="leg_right_6_joint" value="0.060" />
+        <joint name="torso_1_joint" value="0" />
+        <joint name="torso_2_joint" value="0.006761" />
+    </group_state>
    
     <rotor_params>
         <joint name="arm_left_1_joint" mass="1e-5" gear_ratio="100."  />