Skip to content

Commit

Permalink
before testing force control in fixed configuration & before pass it …
Browse files Browse the repository at this point in the history 
…to apptronik
  • Loading branch information
@junhyeokahn
junhyeokahn committed Oct 3, 2018
1 parent ad9c1b4 commit 17f63d9
Show file tree
Hide file tree
Showing 12 changed files with 191 additions and 40 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,3 +41,4 @@ Configuration.h
drake_binary
*_RESULTS.yaml
*.pyc
octave-workspace
48 changes: 38 additions & 10 deletions Addition/MatlabPlotter/Generic.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
16 changes: 10 additions & 6 deletions Addition/MatlabPlotter/RfXYZ.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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
70 changes: 65 additions & 5 deletions Config/Draco/LOW_LEVEL_CONFIG.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
4 changes: 2 additions & 2 deletions Config/FixedDraco/INTERFACE.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
#TestName: InvKinTest
#TestName: OSCTest
TestName: JointTest
TestName: OSCTest
#TestName: JointTest
34 changes: 31 additions & 3 deletions Config/FixedDraco/TEST/JOINT_TEST.yaml
View file
Original file line number Diff line number Diff line change
@@ -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:

Expand Down
19 changes: 9 additions & 10 deletions Config/FixedDraco/TEST/OSC_TEST.yaml
View file
Original file line number Diff line number Diff line change
@@ -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.]
13 changes: 12 additions & 1 deletion PnC/FixedDracoPnC/FixedDracoInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}

Expand Down Expand Up @@ -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() {
Expand Down
8 changes: 8 additions & 0 deletions PnC/FixedDracoPnC/FixedDracoInterface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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();
Expand Down
12 changes: 10 additions & 2 deletions PnC/FixedDracoPnC/TestSet/OSCTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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"];
Expand Down
4 changes: 4 additions & 0 deletions Simulator/FixedDraco/FixedDracoWorldNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion Simulator/FixedDraco/Main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 17f63d9

Please sign in to comment.