From 80fdcbb87fe37ae365f3ac29d99c849cb6a405ef Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Wed, 13 Oct 2021 18:15:58 +0200
Subject: [PATCH] Fix library issue, import of yaml and prepare deployment on
the robot
---
CMakeLists.txt | 6 ++++-
scripts/config_solo12.yaml | 2 +-
src/Joystick.cpp | 6 ++---
src/Params.cpp | 2 +-
src/control_solo12.cpp | 22 +++++++++----------
...onfig_solo12.yaml => walk_parameters.yaml} | 4 ++--
6 files changed, 22 insertions(+), 20 deletions(-)
rename src/{config_solo12.yaml => walk_parameters.yaml} (97%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2ca869c9..761b80c2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -101,6 +101,9 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC pinocchio::pinocchio)
find_package(odri_control_interface REQUIRED)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC odri_control_interface::odri_control_interface)
+#Â Link master board library
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC master_board_sdk::master_board_sdk)
+
# Find parametric curves library and headers
# add_project_dependency(curves REQUIRED)
@@ -133,7 +136,8 @@ endif()
target_compile_options(${PROJECT_NAME} PUBLIC -DNDEBUG -O3)
-target_compile_definitions(${PROJECT_NAME} PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/src/config_solo12.yaml")
+target_compile_definitions(${PROJECT_NAME} PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/scripts/config_solo12.yaml")
+target_compile_definitions(${PROJECT_NAME} PUBLIC WALK_PARAMETERS_YAML="${PROJECT_SOURCE_DIR}/src/walk_parameters.yaml")
# Main Executable
# add_executable(${PROJECT_NAMESPACE}-${PROJECT_NAME} src/main.cpp)
diff --git a/scripts/config_solo12.yaml b/scripts/config_solo12.yaml
index aadd48be..f2c2c681 100644
--- a/scripts/config_solo12.yaml
+++ b/scripts/config_solo12.yaml
@@ -44,4 +44,4 @@ joint_calibrator:
Kp: 1.
Kd: 0.05
T: 1.
- dt: 0.001
+ dt: 0.002
diff --git a/src/Joystick.cpp b/src/Joystick.cpp
index d1732f4f..7d9b5a89 100644
--- a/src/Joystick.cpp
+++ b/src/Joystick.cpp
@@ -63,9 +63,9 @@ void Joystick::update_v_ref_gamepad()
}
else if (event.type == JS_EVENT_AXIS)
{
- if (event.number == 0) gamepad.v_y = + event.value / 32767.0;
- else if(event.number == 1) gamepad.v_x = - event.value / 32767.0;
- else if(event.number == 3) gamepad.w_yaw = + event.value / 32767.0;
+ if (event.number == 0) gamepad.v_x = - event.value / 32767.0;
+ else if(event.number == 1) gamepad.v_y = - event.value / 32767.0;
+ else if(event.number == 3) gamepad.w_yaw = - event.value / 32767.0;
}
}
// printf("Start:%d Stop:%d Vx:%f \tVy:%f \tWyaw:%f\n",gamepad.start,gamepad.select,gamepad.v_x,gamepad.v_y,gamepad.w_yaw);
diff --git a/src/Params.cpp b/src/Params.cpp
index 16e7f324..62055b2b 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -62,7 +62,7 @@ Params::Params()
footsteps_init(12, 0.0), // Fill with zeros, will be filled with values later
footsteps_under_shoulders(12, 0.0) // Fill with zeros, will be filled with values later
{
- initialize(CONFIG_SOLO12_YAML);
+ initialize(WALK_PARAMETERS_YAML);
}
void Params::initialize(const std::string& file_path) {
diff --git a/src/control_solo12.cpp b/src/control_solo12.cpp
index a783cf30..ca39ab53 100644
--- a/src/control_solo12.cpp
+++ b/src/control_solo12.cpp
@@ -57,9 +57,7 @@ int main()
//FakeRobot* robot = new FakeRobot();
// Store initial position data.
- Vector12 des_pos;
- des_pos << 0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, -0.7, +1.4, -0.0, -0.7,
- +1.4;
+ Vector12 q_init = Vector12(params.q_init.data());
// Initialization of variables
Controller controller; // Main controller
@@ -69,12 +67,12 @@ int main()
// Initialize the communication, session, joints, wait for motors to be ready
// and run the joint calibration.
- robot->Initialize(des_pos);
+ robot->Initialize(q_init);
robot->joints->SetZeroCommands();
robot->ParseSensorData();
// Wait for Enter input before starting the control loop
- put_on_the_floor(robot, des_pos, params, controller);
+ put_on_the_floor(robot, q_init, params, controller);
std::chrono::time_point<std::chrono::steady_clock> t_log [params.N_SIMULATION-2];
// Main loop
@@ -103,11 +101,11 @@ int main()
}
// Send commands to the robot
- robot->joints->SetPositionGains(Vector12::Zero());
- robot->joints->SetVelocityGains(Vector12::Zero());
- robot->joints->SetDesiredPositions(Vector12::Zero());
- robot->joints->SetDesiredVelocities(Vector12::Zero());
- robot->joints->SetTorques(Vector12::Zero());
+ robot->joints->SetPositionGains(controller.P);
+ robot->joints->SetVelocityGains(controller.D);
+ robot->joints->SetDesiredPositions(controller.q_des);
+ robot->joints->SetDesiredVelocities(controller.v_des);
+ robot->joints->SetTorques((controller.FF).cwiseProduct(controller.tau_ff));
// Checks if the robot is in error state (that is, if any component
// returns an error). If there is an error, the commands to send
@@ -162,7 +160,7 @@ int main()
parallel_thread.join();
std::cout << "Parallel thread closed" << std::endl;
- int duration_log [params.N_SIMULATION-2];
+ /*int duration_log [params.N_SIMULATION-2];
for (int i = 0; i < params.N_SIMULATION-3; i++)
{
duration_log[i] = static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(t_log[i+1] - t_log[i]).count());
@@ -171,7 +169,7 @@ int main()
{
std::cout << std::chrono::duration_cast<std::chrono::microseconds>(t_log[i+1] - t_log[i]).count() << ", ";
}
- std::cout << std::endl;
+ std::cout << std::endl;*/
return 0;
}
diff --git a/src/config_solo12.yaml b/src/walk_parameters.yaml
similarity index 97%
rename from src/config_solo12.yaml
rename to src/walk_parameters.yaml
index c822c574..63fe1909 100644
--- a/src/config_solo12.yaml
+++ b/src/walk_parameters.yaml
@@ -9,7 +9,7 @@ robot:
use_flat_plane: true # If True the ground is flat, otherwise it has bumps
predefined_vel: false # If we are using a predefined reference velocity (True) or a joystick (False)
velID: 10 # Identifier of the reference velocity profile to choose which one will be sent to the robot
- N_SIMULATION: 20 # Number of simulated wbc time steps
+ N_SIMULATION: 100000 # Number of simulated wbc time steps
enable_pyb_GUI: true # Enable/disable PyBullet GUI
enable_corba_viewer: true # Enable/disable Corba Viewer
enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
@@ -19,7 +19,7 @@ robot:
# [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583] # h_com = 0.2
q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407] #Â h_com = 0.218
# q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4] # Initial articular positions
- dt_wbc: 0.001 # Time step of the whole body control
+ dt_wbc: 0.002 # Time step of the whole body control
dt_mpc: 0.02 # Time step of the model predictive control
type_MPC: 0 # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
Kp_main: [3.0, 3.0, 3.0] # Proportional gains for the PD+
--
GitLab