Skip to content
Snippets Groups Projects
Commit d1a84d4b authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[CoM stabilization] Add impulsive test

parent 452dce7c
No related branches found
No related tags found
No related merge requests found
'''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))
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment