Commit 4e566d56 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[admittance-ctrl] Add admittance control entity in python code

parent c0e7f155
......@@ -24,6 +24,7 @@ from dynamic_graph.sot.torque_control.utils.sot_utils import Bunch
from dynamic_graph.sot.torque_control.utils.filter_utils import create_butter_lp_filter_Wn_05_N_3
#from dynamic_graph.sot.torque_control.hrp2.joint_pos_ctrl_gains import *
def create_encoders(robot):
from dynamic_graph.sot.core import Selec_of_vector
encoders = Selec_of_vector('qn')
......@@ -265,6 +266,7 @@ def create_estimators(robot, conf, motor_params, dt):
def create_torque_controller(robot, conf, motor_params, dt=0.001, robot_name="robot"):
torque_ctrl = JointTorqueController("jtc");
plug(robot.encoders.sout, torque_ctrl.jointsPositions);
plug(robot.filters.estimator_kin.dx, torque_ctrl.jointsVelocities);
plug(robot.filters.estimator_kin.ddx, torque_ctrl.jointsAccelerations);
plug(robot.estimator_ft.jointsTorques, torque_ctrl.jointsTorques);
......@@ -493,30 +495,24 @@ def create_current_controller(robot, conf, motor_params, dt, robot_name='robot')
return current_ctrl;
def create_admittance_ctrl(robot, dt=0.001):
def create_admittance_ctrl(robot, conf, dt=0.001, robot_name='robot'):
admit_ctrl = AdmittanceController("adm_ctrl");
plug(robot.device.robotState, admit_ctrl.base6d_encoders);
plug(robot.filters.estimator_kin.dx, admit_ctrl.jointsVelocities);
plug(robot.encoders.sout, admit_ctrl.encoders);
plug(robot.filters.estimator_kin.dx, admit_ctrl.jointsVelocities);
plug(robot.estimator_ft.contactWrenchRightSole, admit_ctrl.fRightFoot);
plug(robot.estimator_ft.contactWrenchLeftSole, admit_ctrl.fLeftFoot);
plug(robot.estimator_ft.contactWrenchRightHand, admit_ctrl.fRightHand);
plug(robot.estimator_ft.contactWrenchLeftHand, admit_ctrl.fLeftHand);
plug(robot.traj_gen.fRightFoot, admit_ctrl.fRightFootRef);
plug(robot.traj_gen.fLeftFoot, admit_ctrl.fLeftFootRef);
plug(robot.traj_gen.fRightHand, admit_ctrl.fRightHandRef);
plug(robot.traj_gen.fLeftHand, admit_ctrl.fLeftHandRef);
admit_ctrl.damping.value = 4*(0.05,);
admit_ctrl.Kd.value = NJ*(0,);
kf = -0.0005;
km = -0.008;
admit_ctrl.Kf.value = 3*(kf,)+3*(km,)+3*(kf,)+3*(km,)+3*(kf,)+3*(km,)+3*(kf,)+3*(km,);
plug(robot.inv_dyn.f_des_right_foot, admit_ctrl.fRightFootRef);
plug(robot.inv_dyn.f_des_left_foot, admit_ctrl.fLeftFootRef);
robot.ctrl_manager.addCtrlMode("adm");
plug(admit_ctrl.qDes, robot.ctrl_manager.ctrl_adm);
plug(robot.ctrl_manager.joints_ctrl_mode_adm, admit_ctrl.controlledJoints);
admit_ctrl.init(dt);
admit_ctrl.damping.value = 4*(0.05,);
admit_ctrl.controlledJoints.value = NJ*(1.0,);
admit_ctrl.kp_force.value = conf.kp_force;
admit_ctrl.ki_force.value = conf.ki_force;
admit_ctrl.kp_vel.value = conf.kp_vel;
admit_ctrl.ki_vel.value = conf.ki_vel;
admit_ctrl.force_integral_saturation.value = conf.force_integral_saturation;
admit_ctrl.init(dt, robot_name);
return admit_ctrl;
def create_topic(ros_import, signal, name, robot=None, entity=None, data_type='vector', sleep_time=0.1):
......
......@@ -13,7 +13,7 @@ from dynamic_graph.sot.torque_control.create_entities_utils import create_base_e
from dynamic_graph.sot.torque_control.create_entities_utils import create_balance_controller, create_ctrl_manager, create_ros_topics
from dynamic_graph.sot.torque_control.create_entities_utils import create_free_flyer_locator, create_flex_estimator, create_floatingBase
from dynamic_graph.sot.torque_control.create_entities_utils import create_current_controller, connect_ctrl_manager
from dynamic_graph.sot.torque_control.create_entities_utils import create_tracer, create_topic
from dynamic_graph.sot.torque_control.create_entities_utils import create_tracer, create_topic, create_admittance_ctrl
from dynamic_graph.ros import RosPublish
from dynamic_graph.sot.torque_control.utils.sot_utils import start_sot, stop_sot, go_to_position, Bunch
from dynamic_graph.sot.torque_control.utils.filter_utils import create_chebi2_lp_filter_Wn_03_N_4
......@@ -22,6 +22,7 @@ from time import sleep
def get_default_conf():
import dynamic_graph.sot.torque_control.hrp2.balance_ctrl_conf as balance_ctrl_conf
import dynamic_graph.sot.torque_control.hrp2.admittance_ctrl_conf as admittance_ctrl_conf
import dynamic_graph.sot.torque_control.hrp2.base_estimator_conf as base_estimator_conf
import dynamic_graph.sot.torque_control.hrp2.control_manager_conf as control_manager_conf
import dynamic_graph.sot.torque_control.hrp2.current_controller_conf as current_controller_conf
......@@ -31,6 +32,7 @@ def get_default_conf():
import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params
conf = Bunch();
conf.balance_ctrl = balance_ctrl_conf;
conf.adm_ctrl = admittance_ctrl_conf;
conf.base_estimator = base_estimator_conf;
conf.control_manager = control_manager_conf;
conf.current_ctrl = current_controller_conf;
......@@ -88,6 +90,7 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt);
robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt);
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt);
robot.adm_ctrl = create_admittance_ctrl(robot, conf.adm_ctrl, dt);
robot.current_ctrl = create_current_controller(robot, conf.current_ctrl, conf.motor_params, dt);
connect_ctrl_manager(robot);
......
......@@ -56,6 +56,7 @@ SET(plugins
device-torque-ctrl
trace-player
imu_offset_compensation
admittance-controller
)
#set(ADDITIONAL_feature-task_LIBS feature-generic task)
......
Supports Markdown
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