test_balance_ctrl_openhrp.py 4.01 KB
Newer Older
1
2
3
4
5
6
7
8
# -*- coding: utf-8 -*-
"""
2014-2017, LAAS/CNRS
@author: Andrea Del Prete, Rohan Budhiraja
"""

import numpy as np
from dynamic_graph import plug
9
from dynamic_graph.sot.core import Selec_of_vector
10
from dynamic_graph.sot.torque_control.create_entities_utils import NJ
11
from dynamic_graph.sot.torque_control.utils.sot_utils import start_sot, stop_sot, Bunch
12
13
14
from dynamic_graph.ros import RosPublish
from dynamic_graph.sot.torque_control.create_entities_utils import create_topic
from dynamic_graph.sot.torque_control.main import main_v3
15
from time import sleep
16
17

#from dynamic_graph.sot.torque_control.hrp2.sot_utils import config_sot_to_urdf, joints_sot_to_urdf
18
    
19
20
def get_sim_conf():
    import dynamic_graph.sot.torque_control.hrp2.balance_ctrl_sim_conf as balance_ctrl_conf
21
    import dynamic_graph.sot.torque_control.hrp2.base_estimator_sim_conf as base_estimator_conf
22
    import dynamic_graph.sot.torque_control.hrp2.control_manager_sim_conf as control_manager_conf
23
    import dynamic_graph.sot.torque_control.hrp2.current_controller_sim_conf as current_controller_conf
24
25
26
27
    import dynamic_graph.sot.torque_control.hrp2.force_torque_estimator_conf as force_torque_estimator_conf
    import dynamic_graph.sot.torque_control.hrp2.joint_torque_controller_conf as joint_torque_controller_conf
    import dynamic_graph.sot.torque_control.hrp2.joint_pos_ctrl_gains_sim as pos_ctrl_gains
    import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params
28
    import dynamic_graph.sot.torque_control.hrp2.admittance_ctrl_conf as admittance_ctrl_conf
29
    conf = Bunch();
30
    conf.adm_ctrl                  = admittance_ctrl_conf;
31
32
33
    conf.balance_ctrl              = balance_ctrl_conf;
    conf.base_estimator            = base_estimator_conf;
    conf.control_manager           = control_manager_conf;
34
    conf.current_ctrl              = current_controller_conf;
35
36
37
38
39
    conf.force_torque_estimator    = force_torque_estimator_conf;
    conf.joint_torque_controller   = joint_torque_controller_conf;
    conf.pos_ctrl_gains            = pos_ctrl_gains;
    conf.motor_params              = motor_params;
    return conf;
40
    
41
def test_balance_ctrl_openhrp(robot, use_real_vel=True, use_real_base_state=False, startSoT=True):
42
43
44
    # BUILD THE STANDARD GRAPH
    conf = get_sim_conf();
    robot = main_v3(robot, startSoT=False, go_half_sitting=False, conf=conf);
45
    
46
    # force current measurements to zero
47
48
    robot.ctrl_manager.i_measured.value = NJ*(0.0,);
    robot.current_ctrl.i_measured.value = NJ*(0.0,);
49
    robot.filters.current_filter.x.value = NJ*(0.0,);
50
    
51
52
    # BYPASS TORQUE CONTROLLER
    plug(robot.inv_dyn.tau_des,     robot.ctrl_manager.ctrl_torque);
53
    
54
55
56
57
58
59
60
    # CREATE SIGNALS WITH ROBOT STATE WITH CORRECT SIZE (36)
    robot.q = Selec_of_vector("q");
    plug(robot.device.robotState, robot.q.sin);
    robot.q.selec(0, NJ+6);
    plug(robot.q.sout,              robot.pos_ctrl.base6d_encoders);
    plug(robot.q.sout,              robot.traj_gen.base6d_encoders);
    plug(robot.q.sout,              robot.estimator_ft.base6d_encoders);
61
    
62
63
64
    robot.ros = RosPublish('rosPublish');
    robot.device.after.addDownsampledSignal('rosPublish.trigger',1);
    
65
66
67
68
69
70
71
    # BYPASS JOINT VELOCITY ESTIMATOR
    if(use_real_vel):
        robot.dq = Selec_of_vector("dq");
        plug(robot.device.robotVelocity, robot.dq.sin);
        robot.dq.selec(6, NJ+6);
        plug(robot.dq.sout,             robot.pos_ctrl.jointsVelocities);
        plug(robot.dq.sout,             robot.base_estimator.joint_velocities);
72
        plug(robot.device.gyrometer,    robot.base_estimator.gyroscope);
73
74

    # BYPASS BASE ESTIMATOR
75
76
77
    robot.v = Selec_of_vector("v");
    plug(robot.device.robotVelocity, robot.v.sin);
    robot.v.selec(0, NJ+6);
78
79
80
    if(use_real_base_state):
        plug(robot.q.sout,              robot.inv_dyn.q);
        plug(robot.v.sout,              robot.inv_dyn.v);
81
    
82
    if(startSoT):
83
84
85
86
        start_sot();    
        # RESET FORCE/TORQUE SENSOR OFFSET
        sleep(10*robot.timeStep);
        robot.estimator_ft.setFTsensorOffsets(24*(0.0,));
87
    
88
    return robot;