From 17f63d9d725daafd1defa8249cef5d5da323c41d Mon Sep 17 00:00:00 2001 From: junhyeokahn Date: Wed, 3 Oct 2018 10:50:55 -0500 Subject: [PATCH] before testing force control in fixed configuration & before pass it to apptronik --- .gitignore | 1 + Addition/MatlabPlotter/Generic.m | 48 +++++++++++--- Addition/MatlabPlotter/RfXYZ.m | 16 +++-- Config/Draco/LOW_LEVEL_CONFIG.yaml | 70 ++++++++++++++++++-- Config/FixedDraco/INTERFACE.yaml | 4 +- Config/FixedDraco/TEST/JOINT_TEST.yaml | 34 +++++++++- Config/FixedDraco/TEST/OSC_TEST.yaml | 19 +++--- PnC/FixedDracoPnC/FixedDracoInterface.cpp | 13 +++- PnC/FixedDracoPnC/FixedDracoInterface.hpp | 8 +++ PnC/FixedDracoPnC/TestSet/OSCTest.cpp | 12 +++- Simulator/FixedDraco/FixedDracoWorldNode.cpp | 4 ++ Simulator/FixedDraco/Main.cpp | 2 +- 12 files changed, 191 insertions(+), 40 deletions(-) diff --git a/.gitignore b/.gitignore index e32b85d4..32749b74 100644 --- a/.gitignore +++ b/.gitignore @@ -41,3 +41,4 @@ Configuration.h drake_binary *_RESULTS.yaml *.pyc +octave-workspace diff --git a/Addition/MatlabPlotter/Generic.m b/Addition/MatlabPlotter/Generic.m index cb2ca650..1fc02894 100644 --- a/Addition/MatlabPlotter/Generic.m +++ b/Addition/MatlabPlotter/Generic.m @@ -8,7 +8,8 @@ data_path = '/home/apptronik/Repository/PnC/ExperimentDataCheck'; %% -targetJointIdx = 1:10; +targetJointIdx = [1,2,3,4,5]; +targetJointIdx = 6:8; numJoint = 10; numTarget = length(targetJointIdx); fig = fn_open_figures(numTarget); @@ -20,28 +21,55 @@ JPosAct = fn_read_file(data_path, 'JPosAct', numJoint); JVelAct = fn_read_file(data_path, 'JVelAct', numJoint); JTrqAct = fn_read_file(data_path, 'JTrqAct', numJoint); +BusVoltage = fn_read_file(data_path, 'BusVoltage', numJoint); +BusCurrent = fn_read_file(data_path, 'BusCurrent', numJoint); +Temperature = fn_read_file(data_path, 'Temperature', numJoint); +MotorCurrent = fn_read_file(data_path, 'motorCurrent', numJoint); startIdx = 1; -endIdx = length(Time)-5; +endIdx = length(Time)-15; %% Plot -for i = 1:numTarget + +for i = 1 : numTarget figure(fig(i)) - subplot(3,1,1) + subplot(4,2,1) hold on plot(Time(startIdx:endIdx), JPosDes(targetJointIdx(i), startIdx:endIdx),'r', 'linewidth', 3); - plot(Time(startIdx:endIdx), JPosAct(targetJointIdx(i), startIdx:endIdx),'b', 'linewidth', 3); - grid on + plot(Time(startIdx:endIdx), JPosAct(targetJointIdx(i), startIdx:endIdx),'b', 'linewidth', 1); hold off - subplot(3,1,2) + grid on + title('jpos') + subplot(4,2,2) + plot(Time(startIdx:endIdx), BusVoltage(targetJointIdx(i), startIdx:endIdx).*BusCurrent(targetJointIdx(i), startIdx:endIdx), 'linewidth', 3); + grid on + title('electronic power') + subplot(4,2,3) hold on plot(Time(startIdx:endIdx), JVelDes(targetJointIdx(i), startIdx:endIdx),'r', 'linewidth', 3); - plot(Time(startIdx:endIdx), JVelAct(targetJointIdx(i), startIdx:endIdx),'b', 'linewidth', 3); + plot(Time(startIdx:endIdx), JVelAct(targetJointIdx(i), startIdx:endIdx),'b', 'linewidth', 1); grid on hold off - subplot(3,1,3) + subplot(4,2,4) + plot(Time(startIdx:endIdx), JTrqAct(targetJointIdx(i), startIdx:endIdx).*JVelAct(targetJointIdx(i), startIdx:endIdx), 'linewidth', 3); + title('mechanical power') + grid on + subplot(4,2,5) hold on plot(Time(startIdx:endIdx), JTrqDes(targetJointIdx(i), startIdx:endIdx),'r', 'linewidth', 3); - plot(Time(startIdx:endIdx), JTrqAct(targetJointIdx(i), startIdx:endIdx),'b', 'linewidth', 3); + plot(Time(startIdx:endIdx), JTrqAct(targetJointIdx(i), startIdx:endIdx),'b', 'linewidth', 1); + title('jtrq') grid on hold off + subplot(4,2,6) + plot(Time(startIdx:endIdx), (JTrqAct(targetJointIdx(i), startIdx:endIdx).*JVelAct(targetJointIdx(i), startIdx:endIdx))./(BusVoltage(targetJointIdx(i), startIdx:endIdx).*BusCurrent(targetJointIdx(i), startIdx:endIdx)), 'linewidth', 3); + title('efficiency') + grid on + subplot(4,2,7) + plot(Time(startIdx:endIdx), Temperature(targetJointIdx(i), startIdx:endIdx), 'linewidth', 3); + title('temperature') + grid on + subplot(4,2,8) + plot(Time(startIdx:endIdx), MotorCurrent(targetJointIdx(i), startIdx:endIdx), 'linewidth', 3); + title('motor current') + grid on end \ No newline at end of file diff --git a/Addition/MatlabPlotter/RfXYZ.m b/Addition/MatlabPlotter/RfXYZ.m index fb3ec98c..6ece6c0d 100644 --- a/Addition/MatlabPlotter/RfXYZ.m +++ b/Addition/MatlabPlotter/RfXYZ.m @@ -6,7 +6,9 @@ fn_path = '/Users/junhyeokahn/Repository/PnC/Addition/MatlabPlotter/functions'; addpath(fn_path) data_path = '/Users/junhyeokahn/Repository/PnC/ExperimentDataCheck'; - +fn_path = '/home/apptronik/Repository/PnC/Addition/MatlabPlotter/functions'; +addpath(fn_path) +data_path = '/home/apptronik/Repository/PnC/ExperimentDataCheck'; %% fig = fn_open_figures(3); @@ -18,22 +20,24 @@ RfAccDes = fn_read_file(data_path, 'rf_acc_des_debug', 3); %% Plot +start_idx = 1; +end_idx = length(Time)-2; for i = 1:3 figure(fig(i)) subplot(3,1,1) hold on - plot(Time, RfPosDes(i, :), 'r', 'linewidth', 3); - plot(Time, RfPosAct(i, :), 'b', 'linewidth', 3); + plot(Time(start_idx:end_idx), RfPosDes(i, start_idx:end_idx), 'r', 'linewidth', 3); + plot(Time(start_idx:end_idx), RfPosAct(i, start_idx:end_idx), 'b', 'linewidth', 1); ylabel('rf pos'); grid on hold off subplot(3,1,2) hold on - plot(Time, RfVelDes(i, :), 'r', 'linewidth', 3); - plot(Time, RfVelDes(i, :), 'b', 'linewidth', 3); + plot(Time(start_idx:end_idx), RfVelDes(i, start_idx:end_idx), 'r', 'linewidth', 3); + plot(Time(start_idx:end_idx), RfVelAct(i, start_idx:end_idx), 'b', 'linewidth', 1); hold off grid on subplot(3,1,3) - plot(Time, RfAccDes(i, :), 'r', 'linewidth', 3); + plot(Time(start_idx:end_idx), RfAccDes(i, start_idx:end_idx), 'r', 'linewidth', 3); grid on end diff --git a/Config/Draco/LOW_LEVEL_CONFIG.yaml b/Config/Draco/LOW_LEVEL_CONFIG.yaml index b4ecb7e8..f04d05c4 100644 --- a/Config/Draco/LOW_LEVEL_CONFIG.yaml +++ b/Config/Draco/LOW_LEVEL_CONFIG.yaml @@ -6,24 +6,84 @@ #t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] #en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] #en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] -#current_limit : [10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10 , 10] +#current_limit : [5 , 10 , 10 , 10 , 3 , 5 , 10 , 10 , 10 , 3] ####### Weak Gain -#jp_kp : [40 , 50 , 50 , 50 , 9 , 40 , 50 , 50 , 50 , 9] -#jp_kd : [2 , 3 , 3 , 3 , 0.4 , 2 , 3 , 3 , 3 , 0.4] +#jp_kp : [40 , 70 , 70 , 70 , 9 , 40 , 70 , 70 , 70 , 9] +#jp_kd : [2 , 5 , 5 , 5 , 0.4 , 2 , 5 , 5 , 5 , 0.4] #t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] #t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] #en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] #en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] -#current_limit : [2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2 , 2] +#current_limit : [3 , 7 , 7 , 7 , 1 , 3 , 7 , 7 , 7 , 1] ####### Zero Gain -- gravity compensation +#jp_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#jp_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] +#en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#current_limit : [5 , 5 , 5 , 5 , 5 , 5 , 5 , 5 , 5 , 5] + +####### ROM Test Gain -- hip yaw + +#jp_kp : [40 , 0 , 0 , 0 , 0 , 40 , 0 , 0 , 0 , 0] +#jp_kd : [2 , 0 , 0 , 0 , 0 , 2 , 0 , 0 , 0 , 0] +#t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] +#en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#current_limit : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] + +####### ROM Test Gain -- hip roll + +#jp_kp : [0 , 50 , 0 , 0 , 0 , 0 , 50 , 0 , 0 , 0] +#jp_kd : [0 , 3 , 0 , 0 , 0 , 0 , 3 , 0 , 0 , 0] +#t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] +#en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#current_limit : [1 , 3, 1 , 1 , 1 , 1 , 3 , 1 , 1 , 1] + +####### ROM Test Gain -- hip pitch + +#jp_kp : [40 , 70 , 70 , 70 , 9 , 40 , 70 , 70 , 70 , 9] +#jp_kd : [2 , 5 , 5 , 5 , 0.4 , 2 , 5 , 5 , 5 , 0.4] +#t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] +#en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#current_limit : [3 , 7 , 7 , 7 , 1 , 3 , 7 , 7 , 7 , 1] + +######### ROM Test Gain -- knee + +#jp_kp : [40 , 70 , 70 , 50 , 9 , 40 , 70 , 70 , 70 , 9] +#jp_kd : [2 , 5 , 5 , 3 , 0.4 , 2 , 5 , 5 , 7 , 0.4] +#t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] +#en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#current_limit : [3 , 7 , 7 , 7 , 1 , 3 , 7 , 7 , 7 , 1] + +######## ROM Test Gain -- Ankle + +#jp_kp : [40 , 70 , 70 , 50 , 9 , 40 , 70 , 70 , 70 , 9] +#jp_kd : [2 , 5 , 5 , 3 , 0.4 , 2 , 5 , 5 , 7 , 0.4] +#t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] +#en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] +#current_limit : [3 , 7 , 7 , 7 , 2 , 3 , 7 , 7 , 7 , 2] + +######## OSC Test + jp_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] jp_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] t_kp : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] t_kd : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] en_auto_kd : [1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1] en_dob : [0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0] -current_limit : [3 , 3 , 3 , 3 , 3 , 3 , 3 , 3 , 3 , 3] +current_limit : [1 , 1 , 1 , 1 , 1 , 5 , 5 , 5 , 5 , 3] diff --git a/Config/FixedDraco/INTERFACE.yaml b/Config/FixedDraco/INTERFACE.yaml index 5ee180b2..d4434d9f 100644 --- a/Config/FixedDraco/INTERFACE.yaml +++ b/Config/FixedDraco/INTERFACE.yaml @@ -1,3 +1,3 @@ #TestName: InvKinTest -#TestName: OSCTest -TestName: JointTest +TestName: OSCTest +#TestName: JointTest diff --git a/Config/FixedDraco/TEST/JOINT_TEST.yaml b/Config/FixedDraco/TEST/JOINT_TEST.yaml index 4050276a..d46cdfc3 100644 --- a/Config/FixedDraco/TEST/JOINT_TEST.yaml +++ b/Config/FixedDraco/TEST/JOINT_TEST.yaml @@ -1,9 +1,37 @@ sinusoidal_configuration: transition_time: 1.0 - mid: [0.0, 0.0, -0.8, 0.82, 1.57, 0.0, 0.0, -0.8, 0.82, 1.57] - amp: [0.0, 0.0, 0.3, 0.3, 0.2, 0.0, 0.0, -0.3, -0.3, 0.2] - freq: [0.0, 0.0, 0.3, 0.3, 0.2, 0.0, 0.0, 0.3, 0.3, 0.2] + + ## normal sinusoid + mid: [0.0, 0.0, -0.8, 0.9, 1.57, 0.0, 0.0, -0.8, 0.9, 1.57] + amp: [0.0, 0.0, 0.3, 0.2, 0.0, 0.0, 0.0, -0.3, -0.2, 0.0] + freq: [0.0, 0.0, 0.7, 0.7, 0.0, 0.0, 0.0, 0.7, 0.7, 0.0] + + ## ROM hip yaw + #mid: [0.0 , 0.0 , 0.0 , 0.0 , 1.57 , 0.0 , 0.0 , 0.0 , 0.0 , 1.57] + #amp: [0.65 , 0.0 , 0.0 , 0.0 , 0.0 , -0.65 , 0.0 , 0.0 , 0.0 , 0.0] + #freq: [0.2 , 0.0 , 0.0 , 0.0 , 0.0 , 0.2 , 0.0 , 0.0 , 0.0 , 0.0] + + ## ROM hip roll + #mid: [0.0 , 0.325 , 0.0 , 0.0 , 1.57 , 0.0 , -0.325 , 0.0 , 0.0 , 1.57] + #amp: [0.0 , 0.475 , 0.0 , 0.0 , 0.0 , 0.0 , 0.475 , 0.0 , 0.0 , 0.0] + #freq: [0.0 , 0.2 , 0.0 , 0.0 , 0.0 , 0.0 , 0.2 , 0.0 , 0.0 , 0.0] + + ## ROM hip pitch + #mid: [-0.7355 , 0.0 , -0.125 , 1.2689 , 1.57 , 0.3290 , 0.0 , -0.125 , 1.2689 , 1.57] + #amp: [0.0 , 0.0 , 0.925 , 0.0 , 0.0 , 0.0 , 0.0 , -0.925 , 0.0 , 0.0] + #freq: [0.0 , 0.0 , 0.2 , 0.0 , 0.0 , 0.0 , 0.0 , 0.2 , 0.0 , 0.0] + + ## ROM knee + #mid: [0 , 0 , -0.7 , 1.1 , 1.57 , 0 , 0 , -0.7 , 1.1 , 1.57] + #amp: [0 , 0 , 0 , -0.8 , 0.0 , 0.0 , 0.0 , 0.0 , 0.8 , 0.0] + #freq: [0 , 0 , 0 , 0.2 , 0.0 , 0.0 , 0.0 , 0.0 , 0.2 , 0.0] + + ## ROM Ankle + #mid: [0 , 0 , -0.7 , 1.1 , 1.29 , 0 , 0 , -0.7 , 1.1 , 1.29] + #amp: [0 , 0 , 0 , 0.0 , 0.76 , 0.0 , 0.0 , 0.0 , 0.0 , 0.76] + #freq: [0 , 0 , 0 , 0.0 , 0.2 , 0.0 , 0.0 , 0.0 , 0.0 , -0.2] + control_configuration: diff --git a/Config/FixedDraco/TEST/OSC_TEST.yaml b/Config/FixedDraco/TEST/OSC_TEST.yaml index 22e88d3b..02ac394d 100644 --- a/Config/FixedDraco/TEST/OSC_TEST.yaml +++ b/Config/FixedDraco/TEST/OSC_TEST.yaml @@ -1,13 +1,12 @@ -sinusoidal_configuration: - - transition_time: 1.0 - mid: [-0.0211 , -0.167055 , 0.332529] - amp: [0.0 , 0.0 , 0.05] - freq: [0.0 , 0.0 , 1.0] +sinusoidal_configuration: + transition_time: 0.5 + mid : [-0.0211 , 0.167055 , 0.332529] + amp : [0.15 , 0.0 , 0.05] + freq : [ 0.5 , 0.0 , 0.5] control_configuration: - kp_rf: [300., 300., 300.] - kd_rf: [150., 150., 150.] - kp_q: [100. , 100. , 100. , 100. , 80. , 100. , 100. , 100. , 100. , 80.] - kd_q: [15. , 15. , 15. , 15. , 15. , 15. , 15. , 15. , 15. , 15.] + kp_rf : [400 , 550 , 550] + kd_rf : [3 , 3 , 3] + kp_q : [30. , 30. , 30. , 30. , 10. , 30. , 30. , 30. , 30. , 10.] + kd_q : [2. , 2. , 2. , 2. , 1. , 2. , 2. , 2. , 2. , 1.] diff --git a/PnC/FixedDracoPnC/FixedDracoInterface.cpp b/PnC/FixedDracoPnC/FixedDracoInterface.cpp index 6fab886d..7ecf976a 100644 --- a/PnC/FixedDracoPnC/FixedDracoInterface.cpp +++ b/PnC/FixedDracoPnC/FixedDracoInterface.cpp @@ -15,13 +15,20 @@ FixedDracoInterface::FixedDracoInterface(): Interface() { mJPosDes = Eigen::VectorXd::Zero(10); mJVelDes = Eigen::VectorXd::Zero(10); mJTrqDes = Eigen::VectorXd::Zero(10); mJPosAct = Eigen::VectorXd::Zero(10); mJVelAct = Eigen::VectorXd::Zero(10); mJTrqAct = Eigen::VectorXd::Zero(10); + mMotorCurrent = Eigen::VectorXd::Zero(10); mBusVoltage = Eigen::VectorXd::Zero(10); + mBusCurrent = Eigen::VectorXd::Zero(10); mTemperature = Eigen::VectorXd::Zero(10); + dataManager->RegisterData(&mTime, DOUBLE, "Time"); dataManager->RegisterData(&mJPosDes, VECT, "JPosDes", 10); dataManager->RegisterData(&mJVelDes, VECT, "JVelDes", 10); dataManager->RegisterData(&mJTrqDes, VECT, "JTrqDes", 10); dataManager->RegisterData(&mJPosAct, VECT, "JPosAct", 10); dataManager->RegisterData(&mJVelAct, VECT, "JVelAct", 10); - dataManager->RegisterData(&mJTrqAct, VECT, "JTrqAct", 10); + dataManager->RegisterData(&mJTrqAct, VECT, "JTrqAct", 10); + dataManager->RegisterData(&mMotorCurrent, VECT, "motorCurrent", 10); + dataManager->RegisterData(&mBusVoltage, VECT, "BusVoltage", 10); + dataManager->RegisterData(&mBusCurrent, VECT, "BusCurrent", 10); + dataManager->RegisterData(&mTemperature, VECT, "Temperature", 10); printf("[Fixed Draco Interface] Constructed\n"); } @@ -53,6 +60,10 @@ void FixedDracoInterface::getCommand(void* sensorData_, void* commandData_) { mJPosAct = data->q; mJVelAct = data->qdot; mJTrqAct = data->jtrq; + mMotorCurrent = data->motorCurrent; + mBusVoltage = data->busVoltage; + mBusCurrent = data->busCurrent; + mTemperature = data->temperature; } void FixedDracoInterface::_constructTest() { diff --git a/PnC/FixedDracoPnC/FixedDracoInterface.hpp b/PnC/FixedDracoPnC/FixedDracoInterface.hpp index 1a43c962..8bb60f67 100644 --- a/PnC/FixedDracoPnC/FixedDracoInterface.hpp +++ b/PnC/FixedDracoPnC/FixedDracoInterface.hpp @@ -8,6 +8,10 @@ class FixedDracoSensorData Eigen::VectorXd q; Eigen::VectorXd qdot; Eigen::VectorXd jtrq; + Eigen::VectorXd motorCurrent; + Eigen::VectorXd busVoltage; + Eigen::VectorXd busCurrent; + Eigen::VectorXd temperature; }; class FixedDracoCommand @@ -30,6 +34,10 @@ class FixedDracoInterface: public Interface Eigen::VectorXd mJPosAct; Eigen::VectorXd mJVelAct; Eigen::VectorXd mJTrqAct; + Eigen::VectorXd mMotorCurrent; + Eigen::VectorXd mBusVoltage; + Eigen::VectorXd mBusCurrent; + Eigen::VectorXd mTemperature; public: FixedDracoInterface(); diff --git a/PnC/FixedDracoPnC/TestSet/OSCTest.cpp b/PnC/FixedDracoPnC/TestSet/OSCTest.cpp index d4f7d0ed..84cd9dd4 100644 --- a/PnC/FixedDracoPnC/TestSet/OSCTest.cpp +++ b/PnC/FixedDracoPnC/TestSet/OSCTest.cpp @@ -110,9 +110,14 @@ void OSCTest::getTorqueInput(void * commandData_) { // Option 1 : integrate, integrate //cmd->qdot = myUtils::eulerIntegration( mRobot->getQdot(), qddot_des, SERVO_RATE ); //cmd->q = myUtils::eulerIntegration( mRobot->getQ(), cmd->qdot, SERVO_RATE ); + // TODO + cmd->q = mTestInitQ; + cmd->qdot.setZero(); + //cmd->jtrq = mRobot->getMassMatrix() * qddot_des + mRobot->getGravity(); + cmd->jtrq = mRobot->getMassMatrix() * qddot_des + mRobot->getGravity(); // Option 2 : integrate - cmd->qdot = qdot_des; - cmd->q = myUtils::doubleIntegration(mRobot->getQ(), cmd->qdot, qddot_des, SERVO_RATE); + //cmd->qdot = qdot_des; + //cmd->q = myUtils::doubleIntegration(mRobot->getQ(), cmd->qdot, qddot_des, SERVO_RATE); rf_pos_des_debug = rf_pos_des; rf_vel_des_debug = rf_vel_des; @@ -132,6 +137,9 @@ void OSCTest::initialize() { YAML::Node sinusoidal_cfg = test_cfg["sinusoidal_configuration"]; myUtils::readParameter(sinusoidal_cfg, "transition_time", mInterpolationDuration); myUtils::readParameter(sinusoidal_cfg, "mid", mMid); + mMid = mTestInitRFPos; + mMid[0] += 0.02; //TODO + mMid[2] += 0.05; myUtils::readParameter(sinusoidal_cfg, "amp", mAmp); myUtils::readParameter(sinusoidal_cfg, "freq", mFreq); YAML::Node control_cfg= test_cfg["control_configuration"]; diff --git a/Simulator/FixedDraco/FixedDracoWorldNode.cpp b/Simulator/FixedDraco/FixedDracoWorldNode.cpp index f2134358..6c9fb40d 100644 --- a/Simulator/FixedDraco/FixedDracoWorldNode.cpp +++ b/Simulator/FixedDraco/FixedDracoWorldNode.cpp @@ -37,6 +37,10 @@ void FixedDracoWorldNode::customPreStep() { mSensorData->q = mSkel->getPositions(); mSensorData->qdot = mSkel->getVelocities(); mSensorData->jtrq = mSkel->getForces(); + mSensorData->motorCurrent = Eigen::VectorXd::Zero(10); + mSensorData->busVoltage = Eigen::VectorXd::Zero(10); + mSensorData->busCurrent = Eigen::VectorXd::Zero(10); + mSensorData->temperature = Eigen::VectorXd::Zero(10); mInterface->getCommand(mSensorData, mCommand); mTorqueCommand = mCommand->jtrq; diff --git a/Simulator/FixedDraco/Main.cpp b/Simulator/FixedDraco/Main.cpp index 15bd5a3a..8b77a579 100644 --- a/Simulator/FixedDraco/Main.cpp +++ b/Simulator/FixedDraco/Main.cpp @@ -142,7 +142,7 @@ int main() { world->addSkeleton(robot); Eigen::Vector3d gravity(0.0, 0.0, -9.81); world->setGravity(gravity); - world->setTimeStep(1.0/1500); + world->setTimeStep(SERVO_RATE); // ==================== // Display Joints Frame