Commit 4a7588c6 authored by Gabriele Buondonno's avatar Gabriele Buondonno

[unittest] Fix unit tests

parent 6877cc7a
from sot_talos_balance.create_entities_utils import *
from IPython import embed
from sot_talos_balance.example import Example
from sot_talos_balance.parameter_server import ParameterServer
from sot_talos_balance.base_estimator import BaseEstimator
from sot_talos_balance.utils.sot_utils import Bunch
from sot_talos_balance.example import Example
import sot_talos_balance.talos.control_manager_conf as param_server_conf
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
dt = 0.001
conf = Bunch()
......@@ -14,10 +8,12 @@ robot_name = 'robot'
conf.param_server = param_server_conf
param_server = ParameterServer("param_server")
param_server.init(dt, conf.param_server.urdfFileName, robot_name)
param_server.setJointsUrdfToSot(conf.param_server.urdftosot)
param_server.setRightFootForceSensorXYZ(conf.param_server.rightFootSensorXYZ)
param_server.setRightFootSoleXYZ(conf.param_server.rightFootSoleXYZ)
param_server.setImuJointName(conf.param_server.ImuJointName)
base_estimator = BaseEstimator('base_estimator')
base_estimator = TalosBaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
example = Example('example')
......@@ -25,11 +21,3 @@ example.firstAddend.value = 0.
example.secondAddend.value = 0.
example.init(robot_name)
embed()
......@@ -78,7 +78,7 @@ param_server = create_parameter_server(param_server_conf,dt)
print("--- Base estimator ---")
conf = base_estimator_conf
base_estimator = BaseEstimator('base_estimator')
base_estimator = TalosBaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
base_estimator.joint_positions.value = halfSitting[7:]
......
......@@ -2,8 +2,6 @@ from __future__ import print_function
from sot_talos_balance.create_entities_utils import *
from IPython import embed
from sot_talos_balance.control_manager import ControlManager
from sot_talos_balance.parameter_server import ParameterServer
from sot_talos_balance.utils.sot_utils import Bunch
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
import sot_talos_balance.talos.control_manager_conf as control_manager_conf
......@@ -20,12 +18,13 @@ conf.control_manager = control_manager_conf
param_server = ParameterServer("param_server")
param_server.init(dt, conf.param_server.urdfFileName, robot_name)
param_server.setJointsUrdfToSot(conf.param_server.urdftosot)
param_server.setRightFootForceSensorXYZ(conf.param_server.rightFootSensorXYZ)
param_server.setRightFootSoleXYZ(conf.param_server.rightFootSoleXYZ)
for key in conf.param_server.mapJointNameToID:
param_server.setNameToId(key,conf.param_server.mapJointNameToID[key])
cm = ControlManager("ControlManager")
cm = TalosControlManager("TalosControlManager")
cm.init(dt, robot_name)
print("***Control manager initialized***")
cm.u_max.value = N_JOINTS*(u_max,)
......@@ -56,7 +55,7 @@ print("Safe control = zero")
print("Control set to 0 forever")
print("*****************")
cm2 = ControlManager("ControlManager")
cm2 = TalosControlManager("TalosControlManager")
cm2.init(dt, robot_name)
print("***New Control manager initialized***")
cm.u_max.value = N_JOINTS*(u_max,)
......
......@@ -40,6 +40,8 @@ print( "dcmDes: %s" % (controller.dcmDes.value,) )
print( "zmpDes: %s" % (controller.dcmDes.value,) )
print( "decayFactor: %s" % (controller.decayFactor.value,) )
print("\n--------------------")
dt = 1
controller.init(dt)
......@@ -55,13 +57,17 @@ assert controller.zmpRef.value == zmpRef
print( "wrenchRef: %s" % (controller.wrenchRef.value,) )
assert controller.wrenchRef.value == wrenchRef
print("\n--------------------")
dcmDes = [1.0,0.0,0.0]
controller.dcmDes.value = dcmDes
print( "dcmDes: %s" % (controller.dcmDes.value,) )
controller.wrenchRef.recompute(1)
zmpRef = tuple([-11.0, 0.0, 0.0])
wrenchRef = tuple([11.0, 0.0, 9.81, 0.0, 0.0, 0.0])
wrenchRef = tuple([11.0, 0.0, 9.81, 0.0, float(com[2]*11), 0.0])
print()
print( "zmpRef: %s" % (controller.zmpRef.value,) )
......@@ -69,13 +75,15 @@ assert controller.zmpRef.value == zmpRef
print( "wrenchRef: %s" % (controller.wrenchRef.value,) )
assert controller.wrenchRef.value == wrenchRef
print("\n--------------------")
controller.dcmDes.time += 1
controller.zmpRef.recompute(2)
controller.wrenchRef.recompute(2)
zmpRef = tuple([-12.0, 0.0, 0.0])
wrenchRef = tuple([12.0, 0.0, 9.81, 0.0, 0.0, 0.0])
wrenchRef = tuple([12.0, 0.0, 9.81, 0.0, float(com[2]*12), 0.0])
print()
print( "zmpRef: %s" % (controller.zmpRef.value,) )
......
from __future__ import print_function
from sot_talos_balance.create_entities_utils import *
from dynamic_graph import plug
import dynamic_graph as dg
from dynamic_graph.sot.core import SOT
import numpy as np
from time import sleep
import os
from IPython import embed
from sot_talos_balance.parameter_server import ParameterServer
from sot_talos_balance.utils.sot_utils import Bunch
from sot_talos_balance.dcm_estimator import DcmEstimator
from sot_talos_balance.utils.sot_utils import Bunch
import sot_talos_balance.talos.control_manager_conf as control_manager_conf
import sot_talos_balance.talos.parameter_server_conf as parameter_server_conf
from numpy.testing import assert_almost_equal as assertApprox
import pinocchio as pin
dt = 0.001
conf = Bunch()
robot_name = 'robot'
conf.param_server = control_manager_conf
from rospkg import RosPack
rospack = RosPack()
urdfPath = rospack.get_path('talos_data')+"/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data')+"/../"]
model = pin.buildModelFromUrdf(urdfPath, pin.JointModelFreeFlyer())
model.lowerPositionLimit = np.vstack( (np.matrix([-1.]*7).T, model.lowerPositionLimit[7:]) )
model.upperPositionLimit = np.vstack( (np.matrix([-1.]*7).T, model.upperPositionLimit[7:]) )
data = model.createData()
q = pin.randomConfiguration(model)
v = pin.utils.rand(model.nv)
pin.centerOfMass(model,data,q, v)
print("Expected:")
print("CoM position value: {0}".format(tuple(data.com[0].flat)) )
print("CoM velocity value: {0}".format(tuple(data.vcom[0].flat)) )
conf.param_server = parameter_server_conf
param_server = ParameterServer("param_server")
param_server.init(dt, conf.param_server.urdfFileName, robot_name)
param_server.setJointsUrdfToSot(conf.param_server.urdftosot)
param_server.setRightFootForceSensorXYZ(conf.param_server.rightFootSensorXYZ)
param_server.setRightFootSoleXYZ(conf.param_server.rightFootSoleXYZ)
dcm_estimator = DcmEstimator('dcm_estimator')
dcm_estimator.q.value = np.random.randn(38)
dcm_estimator.v.value = np.random.randn(38)
dcm_estimator.q.value = list(q.flat)
dcm_estimator.v.value = list(v.flat)
dcm_estimator.init(dt, robot_name)
dcm_estimator.c.recompute(1)
dcm_estimator.dc.recompute(1)
print "CoM position value: {0}".format(dcm_estimator.c.value)
print "CoM velocity value: {0}".format(dcm_estimator.dc.value)
print("Computed:")
print("CoM position value: {0}".format(dcm_estimator.c.value) )
assertApprox(data.com[0], np.matrix(dcm_estimator.c.value).T)
print("CoM velocity value: {0}".format(dcm_estimator.dc.value) )
assertApprox(data.vcom[0], np.matrix(dcm_estimator.dc.value).T, 6)
embed()
\ No newline at end of file
from __future__ import print_function
from sot_talos_balance.example import Example
from sot_talos_balance.create_entities_utils import *
import sot_talos_balance.talos.parameter_server_conf as param_server_conf
# --- Parameter server ---
print("--- Parameter server ---")
dt = 1e-3
robot_name = 'robot'
param_server = create_parameter_server(param_server_conf,dt)
# --- Example ---
print("--- Example ---")
ex = Example("ciao")
......@@ -15,10 +26,12 @@ ex.secondAddend.value = 0.3
print("\nSignals (after pugging):")
ex.displaySignals()
ex.init()
ex.init(robot_name)
ex.sum.recompute(1)
print( "\nInputs: %f, %f" % (ex.firstAddend.value, ex.secondAddend.value) )
print( "Output: %f" % ex.sum.value )
ex.nbJoints.recompute(1)
print( "nbJoints: %d" % ex.nbJoints.value )
from __future__ import print_function
from sot_talos_balance.joint_admittance_controller import JointAdmittanceController
from sot_talos_balance.simple_admittance_controller import SimpleAdmittanceController
controller = JointAdmittanceController("ciao")
controller = SimpleAdmittanceController("ciao")
print("Commands:")
print(controller.commands())
......
......@@ -68,7 +68,7 @@ param_server = create_parameter_server(param_server_conf,dt)
print("--- Base estimator ---")
conf = base_estimator_conf
base_estimator = BaseEstimator('base_estimator')
base_estimator = TalosBaseEstimator('base_estimator')
base_estimator.init(dt, robot_name)
base_estimator.joint_positions.value = halfSitting[7:]
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment