[CoM stabilization] Add impulsive test

python/sot_talos_balance/test/run_test_utils.py

-'''This module contains utilities for running the tests'''
+'''This module contains utilities for running the tests.'''
 
 from __future__ import print_function
 
 import rospy
+
 from std_srvs.srv import *
 from dynamic_graph_bridge.srv import *
 from dynamic_graph_bridge_msgs.srv import *
 
+from gazebo_msgs.srv import ApplyBodyWrench
+from geometry_msgs.msg import Wrench
+
 runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
 runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
 
@@ -71,3 +75,16 @@ def run_test(module,main,*args,**kwargs):
         print()
     except rospy.ServiceException, e:
         rospy.logerr("Service call failed: %s" % e)
+
+def apply_force(force,duration, body_name = "talos::torso_2_link"):
+    '''Gazebo service call for applying a force on a body.'''
+    rospy.wait_for_service('/gazebo/apply_body_wrench')
+    apply_body_wrench_proxy = rospy.ServiceProxy('/gazebo/apply_body_wrench',ApplyBodyWrench)
+    wrench          = Wrench()
+    wrench.force.x  = force[0]
+    wrench.force.y  = force[1]
+    wrench.force.z  = force[2]
+    wrench.torque.x = 0
+    wrench.torque.y = 0
+    wrench.torque.z = 0
+    apply_body_wrench_proxy(body_name = body_name, wrench = wrench, duration = rospy.Duration(duration))

python/sot_talos_balance/test/test_dcmZmpControl.py

@@ -14,6 +14,8 @@ from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
 import matplotlib.pyplot as plt
 import numpy as np
 
+from sot_talos_balance.test.run_test_utils import apply_force
+
 def main(robot):
     dt = robot.timeStep;
 
@@ -92,6 +94,11 @@ def main(robot):
 
     sleep(5.0)
 
+    # kick the robot on the chest to test its stability
+    apply_force([-1000.0,0,0],0.01)
+
+    sleep(5.0)
+
     dump_tracer(robot.tracer)
 
 	  # --- DISPLAY