Commit bef2bc20 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Clean-up command files

parent db24c7c5
create_topic(robot.ros, robot.device.currents, 'i');
create_topic(robot.ros, robot.device.control, 'ctrl');
create_topic(robot.ros, robot.ctrl_manager.currents_real, 'i_real');
create_topic(robot.ros, robot.ctrl_manager.pwmDes, 'i_des')
create_topic(robot.ros, robot.device.robotState, 'robotState')
......
......@@ -10,12 +10,13 @@ robot.timeStep=0.0015
robot = main_v3(robot, startSoT=True, go_half_sitting=0)
go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
robot.inv_dyn.w_forces.value = 1e-4
#robot.base_estimator.set_imu_weight(0.0)
plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities);
#plug(robot.filters.estimator_kin.dx, robot.current_ctrl.dq);
#plug(robot.filters.estimator_kin.dx, robot.torque_ctrl.jointsVelocities);
robot.inv_dyn.kp_com.value = (30.0, 30.0, 50.0)
robot.inv_dyn.kd_com.value = (8.0, 8.0, 0.0)
robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
robot.inv_dyn.kd_com.value = (0.0, 0.0, 0.0)
robot.torque_ctrl.KpTorque.value = 30*(0.0,)
#plug(robot.filters_sg.ft_LF_filter.x_filtered, robot.base_estimator.forceLLEG)
#plug(robot.filters_sg.ft_RF_filter.x_filtered, robot.base_estimator.forceRLEG)
......@@ -58,7 +59,7 @@ go_to_position(robot.traj_gen, robot.halfSitting[6:], 5.0);
robot.base_estimator.reset_foot_positions();
robot.inv_dyn.updateComOffset()
# start torque control on leg joints
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lk-lar-lap','torque')
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
# enable integral feedback in torque control
import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params
......@@ -108,7 +109,8 @@ create_topic(robot.ros, robot.device.accelerometer, 'accelerometer');
robot.com_traj_gen.move(2, 0.85, 2.0)
smoothly_set_signal(robot.torque_ctrl.KpTorque,30*(1.,))
smoothly_set_signal(robot.inv_dyn.kp_posture,30*(5.,))
smoothly_set_signal(robot.inv_dyn.kp_posture,30*(20.,))
smoothly_set_signal(robot.inv_dyn.kp_pos,30*(50.,))
robot.com_traj_gen.stop(-1)
robot.com_traj_gen.move(1, 0.03, 1.5)
......
from dynamic_graph.sot.torque_control.main import *
from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal, stop_sot
from dynamic_graph import plug
import numpy as np
robot.timeStep=0.0015
robot = main_v3(robot, startSoT=True, go_half_sitting=1)
robot = main_v3(robot, startSoT=1, go_half_sitting=1)
plug(robot.filters.estimator_kin.dx, robot.base_estimator.joint_velocities);
robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
robot.inv_dyn.kd_com.value = (0.0, 0.0, 0.0)
robot.torque_ctrl.KpTorque.value = 30*(0.0,)
robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
robot.inv_dyn.kd_com.value = (5.0, 5.0, 0.0)
robot.inv_dyn.kp_posture.value = 30*(30.,)
robot.base_estimator.reset_foot_positions();
robot.inv_dyn.updateComOffset()
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
robot.traj_gen.moveJoint('lk', 0.8, 3.0)
robot.com_traj_gen.move(2, 0.85, 3.0)
create_topic(robot.ros, robot.base_estimator.q, 'q');
create_topic(robot.ros, robot.adm_ctrl.fRightFootRef, 'f_des_R')
create_topic(robot.ros, robot.adm_ctrl.fLeftFootRef, 'f_des_L')
create_topic(robot.ros, robot.adm_ctrl.u, 'u_adm')
create_topic(robot.ros, robot.adm_ctrl.dqDes, 'dq_des')
create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole, 'f_L');
create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_R');
create_topic(robot.ros, robot.inv_dyn.com, 'com')
create_topic(robot.ros, robot.inv_dyn.com_vel, 'com_vel')
create_topic(robot.ros, robot.inv_dyn.com_ref_pos, 'com_ref_pos')
create_topic(robot.ros, robot.inv_dyn.com_ref_vel, 'com_ref_vel')
create_topic(robot.ros, robot.inv_dyn.com_acc_des, 'com_acc_des')
create_topic(robot.ros, robot.base_estimator.q, 'q');
create_topic(robot.ros, robot.base_estimator.v, 'v');
create_topic(robot.ros, robot.base_estimator.v_flex, 'v_flex');
create_topic(robot.ros, robot.base_estimator.v_kin, 'v_kin');
create_topic(robot.ros, robot.base_estimator.v_gyr, 'v_gyr');
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'cop_des')
create_topic(robot.ros, robot.inv_dyn.zmp, 'cop')
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot, 'cop_des_R')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot, 'cop_des_L')
create_topic(robot.ros, robot.inv_dyn.zmp_right_foot, 'cop_R')
create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'cop_L')
create_topic(robot.ros, robot.inv_dyn.dq_admittance, 'dq_adm')
create_topic(robot.ros, robot.ctrl_manager.u, 'i_des');
create_topic(robot.ros, robot.filters.ft_LF_filter.x_filtered, 'f_LF_filt')
create_topic(robot.ros, robot.filters_sg.ft_LF_filter.x_filtered, 'f_LF_filt_sg', robot, robot.filters_sg.ft_LF_filter)
create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot, 'cop_des_R')
create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot, 'cop_des_L')
create_topic(robot.ros, robot.ctrl_manager.u, 'i_des');
create_topic(robot.ros, robot.current_ctrl.i_real, 'i_real');
robot.adm_ctrl.force_integral_saturation.value = (0,0,160.0,20.0,20.0,0)
kp_vel = 12*[0.5,]+18*[0.0,]
smoothly_set_signal(robot.adm_ctrl.kp_vel, tuple(kp_vel))
#smoothly_set_signal(robot.adm_ctrl.kp_vel, 30*(0.0,))
smoothly_set_signal(robot.adm_ctrl.ki_force, (0, 0, 20.0, 2, 0, 0))
b = (0.00554272, 0.01108543, 0.00554272)
a = (1., -1.77863178, 0.80080265)
b = (1.0, 0.0)
a = (1.0, 0.0)
robot.filters.ft_RF_filter.switch_filter(b, a)
robot.filters.ft_LF_filter.switch_filter(b, a)
robot.filters.ft_RH_filter.switch_filter(b, a)
robot.filters.ft_LH_filter.switch_filter(b, a)
robot.filters.acc_filter.switch_filter(b, a)
b = (2.16439898e-05, 4.43473520e-05, -1.74065002e-05, -8.02197247e-05, -1.74065002e-05, 4.43473520e-05, 2.16439898e-05)
a = (1.,-5.32595322,11.89749109,-14.26803139, 9.68705647, -3.52968633, 0.53914042)
robot.filters.gyro_filter.switch_filter(b, a)
robot.filters.estimator_kin.switch_filter(b, a)
K = (1e10, 1e10, 1e10, 707, 502, 1e10);
robot.base_estimator.set_stiffness_left_foot(K)
robot.base_estimator.set_stiffness_right_foot(K)
robot.base_estimator.set_imu_weight(0.0)
robot.inv_dyn.w_forces.value = 1e-4
robot.base_estimator.reset_foot_positions();
sleep(1)
robot.inv_dyn.updateComOffset()
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lk-lar-lap','torque')
robot.com_traj_gen.move(1, 0.03, 1.5)
robot.com_traj_gen.startSinusoid(1,-0.03,2.0)
robot.torque_ctrl.KdVel.value = 30*(0.1,)
ki_vel = 30*[0.0,]
ki_vel[5] = 0.1
ki_vel[11] = 0.1
robot.torque_ctrl.KiVel.value = tuple(ki_vel)
def print_cop_stats(robot):
cop_L = robot.inv_dyn.zmp_left_foot.value
......
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