Commit 9eae65ae authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Delete

parent ed46b6e6
# bolt/demos
## What it is
How to use Bolt at LAAS-CNRS to do demonstrations (calibration, sensor reading and command writting), with ONTAKE computer
## Authors
- Paul Rouanet
## Goal of each code
- demo_bolt_calibration.cpp : Demo to test the calibration on real robot.
- demo_bolt_sensor_reading.cpp : Reading and printing of sensors datas, without control
- demo_bolt_write_commands.cpp : Demo of a sinusoidal control on each joint : swinging effect of Bolt's legs
## How to compile at LAAS ? (on ONTAKE computer)
Put the workspace at the root, in usr. Ex : `/usr/bolt_ws`. Then, put all source codes needed in a directory named `/src`.
```
mkdir /usr/bolt_ws/src
cp <@SourceCodes> /usr/bolt_ws/src
```
Before any compilation, go in bolt_ws :
```
cd /usr/bolt_ws
```
When it is done, you are ready to compile. Always in `/usr/bolt_ws` :
```
colcon build --packages-up-to ros2_control_bolt
```
After that, put the robot in position with legs stretched (more or less in initial position), and switch on the robot : turn on the alimentation (25V, ~1A), and the emergency stop button.
Then, you can send the executable to the robot. We will take the example of demo_bolt_sensor_reading.cpp to illustrate the command to send in the shell.
```
sudo -E "PYTHONPATH=$PYTHONPATH" "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" /usr/bolt_ws/build/ros2_control_bolt/ros2_control_bolt_demo_bolt_sensor_reading enp3s0
```
Here you are, the robot will realize the action of the code sent !
# Inspiré de config_solo12.yaml
robot:
interface: ens3 #enp3s0(?)
joint_modules:
motor_numbers: [0, 3, 2, 1, 5, 4]
motor_constants: 0.025
gear_ratios: 9.
max_currents: 12. #MODIF avec bolt_driver.yaml
reverse_polarities: [
true, true, true, true, false, false #MODIF avec bolt_driver.yaml
]
lower_joint_limits: [
-0.5, -1.7, -3.4, -0.5, -1.7, -3.4 #MODIF avec test sur systeme
]
upper_joint_limits: [
0.5, 1.7, +3.4, +0.5, +1.7, +3.4 #MODIF avec test sur systeme
]
max_joint_velocities: 80.
safety_damping: 0.3 #MODIF avec bolt_driver.yaml
imu:
rotate_vector: [1, 2, 3]
orientation_vector: [1, 2, 3, 4]
joint_calibrator:
# Can be either POS, NEG, ALT or AUTO
search_methods: [
AUTO, AUTO, AUTO, AUTO, AUTO, AUTO
#POS, NEG, POS, NEG, NEG, POS #MODIF avec bolt_driver.yaml
]
position_offsets: [
0.238, -0.308, 0.276, -0.115, -0.584, 0.432
] #Calibration cpp codes made
Kp: 3.
Kd: 0.05
T: 2. #MODIF avec bolt_driver.yaml
dt: 0.001
/* Test de la calibration */
/* Inspiré de demo_create_bolt_robot.cpp*/
#include <system_bolt.hpp>
#include <odri_control_interface/utils.hpp>
#include <odri_control_interface/imu.hpp>
using namespace odri_control_interface;
#include <iostream>
#include <stdexcept>
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<bool, 6, 1> Vector6b;
typedef Eigen::Matrix<long, 3, 1> Vector3l;
typedef Eigen::Matrix<long, 4, 1> Vector4l;
typedef Eigen::Matrix<long, 6, 1> Vector6l;
typedef Eigen::Matrix<int, 6, 1> Vector6i;
int main(int argc, char **argv)
{
if (argc != 2)
{
throw std::runtime_error(
"Please provide the interface name "
"(i.e. using 'ifconfig' on linux");
}
nice(-20); // Give the process a high priority.
auto main_board_ptr_ = std::make_shared<MasterBoardInterface>(argv[1]);
Vector6i motor_numbers;
motor_numbers << 0, 3, 2, 1, 5, 4;
Vector6b motor_reversed;
motor_reversed << true, false, true, true, false, false;
Vector6d joint_lower_limits;
joint_lower_limits << -0.5, -1.7, -3.4, -0.5, -1.7, -3.4; //Modif d'après lecture des capteurs (demo bolt)
Vector6d joint_upper_limits;
joint_upper_limits << 0.5, 1.7, +3.4, +0.5, +1.7, +3.4; //Modif d'après lecture des capteurs (demo bolt)
// Define the joint module.
auto joints = std::make_shared<JointModules>(main_board_ptr_,
motor_numbers,
0.025,
9.,
12., //MAX CURRENT = 12
motor_reversed,
joint_lower_limits,
joint_upper_limits,
80.,
0.5);
// Define the IMU.
Vector3l rotate_vector;
Vector4l orientation_vector;
rotate_vector << 1, 2, 3;
orientation_vector << 1, 2, 3, 4;
auto imu = std::make_shared<IMU>(
main_board_ptr_, rotate_vector, orientation_vector);
// Define the robot.
auto robot = std::make_shared<Robot>(main_board_ptr_, joints, imu);
// Start the robot.
robot->Start();
// Define controller to calibrate the joints.
std::vector<CalibrationMethod> directions = {
AUTO, AUTO, AUTO, AUTO, AUTO, AUTO}; // AUTO*12 --> AUTO*6
Vector6d position_offsets;
position_offsets << 0.238, -0.308, 0.276, -0.115, -0.584, 0.432; // Redefinition après calibrage
auto calib_ctrl = std::make_shared<JointCalibrator>(
joints, directions, position_offsets, 5., 0.05, 2., 0.001);
Vector6d torques;
/*double kp = 3.;
double kd = 0.05;*/
int c = 0;
std::chrono::time_point<std::chrono::system_clock> last =
std::chrono::system_clock::now();
bool is_calibrated = false;
while (!robot->IsTimeout())
{
if (((std::chrono::duration<double>)(std::chrono::system_clock::now()-last)).count() > 0.001)
{
last = std::chrono::system_clock::now(); // last+dt would be better
robot->ParseSensorData();
if (robot->IsReady())
{
if (!is_calibrated)
{
is_calibrated = calib_ctrl->Run();
if (is_calibrated)
{
std::cout << "Calibration is done." << std::endl;
}
}
else
{
// Run the main controller.
auto pos = robot->joints->GetPositions();
auto vel = robot->joints->GetVelocities();
// No torque commands.
for (int i = 0; i < 6; i++)
{
torques[i] = 0.0;
}
robot->joints->SetTorques(torques);
//robot->joints->SetZeroCommands() //A tester
}
}
// 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
// are changed to send the safety control.
robot->SendCommand();
c++;
if (c % 1000 == 0)
{
std::cout << "Count : " << c << "\n";
}
}
else
{
std::this_thread::yield();
}
}
std::cout << "Normal program shutdown." << std::endl;
return 0;
}
/*Inspiré de demo_solo12_from_yaml.cpp*/
/*Modifs :
Vector12d --> Vector6d
Boucles for pour les couples : i<12 --> i<6
CONFIG_SOLO12_YAML --> CONFIG_BOLT_YAML
*/
#include <odri_control_interface/calibration.hpp>
#include <odri_control_interface/robot.hpp>
#include <odri_control_interface/utils.hpp>
using namespace odri_control_interface;
#include <iostream>
#include <stdexcept>
typedef Eigen::Matrix<double, 6, 1> Vector6d;
int main()
{
nice(-20); // Give the process a high priority.
// Define the robot from a yaml file.
auto robot = RobotFromYamlFile(CONFIG_BOLT_YAML);
// Start the robot.
robot->Start();
// Define controller to calibrate the joints from yaml file.
auto calib_ctrl = JointCalibratorFromYamlFile(
CONFIG_BOLT_YAML, robot->joints);
// Initialize simple pd controller.
Vector6d torques;
double kp = 3.;
double kd = 0.05;
int c = 0;
std::chrono::time_point<std::chrono::system_clock> last =
std::chrono::system_clock::now();
bool is_calibrated = false;
while (!robot->IsTimeout())
{
if (((std::chrono::duration<double>)(std::chrono::system_clock::now() -
last))
.count() > 0.001)
{
last = std::chrono::system_clock::now(); // last+dt would be better
robot->ParseSensorData();
if (robot->IsReady())
{
if (!is_calibrated)
{
is_calibrated = calib_ctrl->Run();
if (is_calibrated)
{
std::cout << "Calibration is done." << std::endl;
}
}
else
{
// Run the main controller.
auto pos = robot->joints->GetPositions();
auto vel = robot->joints->GetVelocities();
// Reverse the positions;
for (int i = 0; i < 6; i++)
{
torques[i] = -kp * pos[i] - kd * vel[i];
}
robot->joints->SetTorques(torques);
}
}
// 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
// are changed to send the safety control.
robot->SendCommand();
c++;
if (c % 1000 == 0)
{
std::cout << "Joints: ";
robot->joints->PrintVector(robot->joints->GetPositions());
std::cout << std::endl;
}
}
else
{
std::this_thread::yield();
}
}
std::cout << "Normal program shutdown." << std::endl;
return 0;
}
/* Utile pour la lecture de capteur ---> read() de system_bolt.cpp*/
/* Inspiré de demo_create_bolt_robot.cpp*/
#include <system_bolt.hpp>
#include <odri_control_interface/utils.hpp>
#include <odri_control_interface/imu.hpp>
using namespace odri_control_interface;
#include <iostream>
#include <stdexcept>
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<bool, 6, 1> Vector6b;
typedef Eigen::Matrix<long, 3, 1> Vector3l;
typedef Eigen::Matrix<long, 4, 1> Vector4l;
typedef Eigen::Matrix<long, 6, 1> Vector6l;
typedef Eigen::Matrix<int, 6, 1> Vector6i;
int main(int argc, char **argv)
{
if (argc != 2)
{
throw std::runtime_error(
"Please provide the interface name "
"(i.e. using 'ifconfig' on linux");
}
nice(-20); // Give the process a high priority.
auto main_board_ptr_ = std::make_shared<MasterBoardInterface>(argv[1]);
Vector6i motor_numbers;
motor_numbers << 0, 3, 2, 1, 5, 4;
Vector6b motor_reversed;
motor_reversed << true, false, true, true, false, false;
Vector6d joint_lower_limits;
joint_lower_limits << -0.5, -1.7, -3.4, -0.5, -1.7, -3.4; //Modif d'après lecture des capteurs (demo bolt)
Vector6d joint_upper_limits;
joint_upper_limits << 0.5, 1.7, +3.4, +0.5, +1.7, +3.4; //Modif d'après lecture des capteurs (demo bolt)
// Define the joint module.
auto joints = std::make_shared<JointModules>(main_board_ptr_,
motor_numbers,
0.025,
9.,
12., //MAX CURRENT = 12
motor_reversed,
joint_lower_limits,
joint_upper_limits,
80.,
0.5);
// Define the IMU.
Vector3l rotate_vector;
Vector4l orientation_vector;
rotate_vector << 1, 2, 3;
orientation_vector << 1, 2, 3, 4;
auto imu = std::make_shared<IMU>(
main_board_ptr_, rotate_vector, orientation_vector);
// Define the robot.
auto robot = std::make_shared<Robot>(main_board_ptr_, joints, imu);
// Start the robot.
robot->Start();
int c = 0;
std::chrono::time_point<std::chrono::system_clock> last =
std::chrono::system_clock::now();
while (!robot->IsTimeout())
{
if (((std::chrono::duration<double>)(std::chrono::system_clock::now()-last)).count() > 0.001)
{
last = std::chrono::system_clock::now(); // last+dt would be better
robot->ParseSensorData();
c++;
if (c % 1000 == 0)
{
std::cout << "Count : " << c << "\n";
std::cout << "\n";
std::cout << "Joints : \n";
std::cout << "Position: ";
joints->PrintVector(joints->GetPositions());
std::cout << "\n";
std::cout << "Velocities: ";
joints->PrintVector(joints->GetVelocities());
std::cout << "\n";
std::cout << "Measured Torques: ";
joints->PrintVector(joints->GetMeasuredTorques());
std::cout << "\n";
std::cout << "\n";
std::cout << "IMU : \n";
std::cout << "Gyroscope ";
joints->PrintVector(imu->GetGyroscope());
std::cout << "\n";
std::cout << "Accelerometer ";
joints->PrintVector(imu->GetAccelerometer());
std::cout << "\n";
std::cout << "Linear Acceleration ";
joints->PrintVector(imu->GetLinearAcceleration());
std::cout << "\n";
std::cout << "Attitude Euler ";
joints->PrintVector(imu->GetAttitudeEuler());
std::cout << "\n";
std::cout << "Attitude Quaternion ";
joints->PrintVector(imu->GetAttitudeQuaternion());
std::cout << "\n";
std::cout << "\n";
std::cout << "\n";
std::cout << "\n";
std::cout << std::endl;
}
}
else
{
std::this_thread::yield();
}
}
std::cout << "Normal program shutdown." << std::endl;
return 0;
}
/* Utile pour l'écriture de commande ---> write() de system_bolt.cpp*/
/* Inspiré de demo_create_bolt_robot.cpp*/
#include <system_bolt.hpp>
#include <odri_control_interface/utils.hpp>
#include <odri_control_interface/imu.hpp>
using namespace odri_control_interface;
#include <iostream>
#include <stdexcept>
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<bool, 6, 1> Vector6b;
typedef Eigen::Matrix<long, 3, 1> Vector3l;
typedef Eigen::Matrix<long, 4, 1> Vector4l;
typedef Eigen::Matrix<long, 6, 1> Vector6l;
typedef Eigen::Matrix<int, 6, 1> Vector6i;
int main(int argc, char **argv)
{
if (argc != 2)
{
throw std::runtime_error(
"Please provide the interface name "
"(i.e. using 'ifconfig' on linux");
}
nice(-20); // Give the process a high priority.
auto main_board_ptr_ = std::make_shared<MasterBoardInterface>(argv[1]);
Vector6i motor_numbers;
motor_numbers << 0, 3, 2, 1, 5, 4;
Vector6b motor_reversed;
motor_reversed << true, false, true, true, false, false;
Vector6d joint_lower_limits;
joint_lower_limits << -0.5, -1.7, -3.4, -0.5, -1.7, -3.4; //Modif d'après lecture des capteurs (demo bolt)
Vector6d joint_upper_limits;
joint_upper_limits << 0.5, 1.7, +3.4, +0.5, +1.7, +3.4; //Modif d'après lecture des capteurs (demo bolt)
// Define the joint module.
auto joints = std::make_shared<JointModules>(main_board_ptr_,
motor_numbers,
0.025,
9.,
12., //MAX CURRENT = 12
motor_reversed,
joint_lower_limits,
joint_upper_limits,
80.,
0.5);
// Define the IMU.
Vector3l rotate_vector;
Vector4l orientation_vector;
rotate_vector << 1, 2, 3;
orientation_vector << 1, 2, 3, 4;
auto imu = std::make_shared<IMU>(
main_board_ptr_, rotate_vector, orientation_vector);
// Define the robot.
auto robot = std::make_shared<Robot>(main_board_ptr_, joints, imu);
// Start the robot.
robot->Start();
// Define controller to calibrate the joints.
std::vector<CalibrationMethod> directions = {
AUTO, AUTO, AUTO, AUTO, AUTO, AUTO}; // AUTO*12 --> AUTO*6
Vector6d position_offsets;
position_offsets << 0.238, -0.308, 0.276, -0.115, -0.584, 0.432; // Redefinition après calibrage
auto calib_ctrl = std::make_shared<JointCalibrator>(
joints, directions, position_offsets, 5., 0.05, 2., 0.001);
Vector6d torques;
Vector6d positions;
double kp = 3.;
double kd = 0.05;
double t = 0.0;
double dt = 0.001;
double freq = 0.3 * 4;
double amplitude = (M_PI / 8.0) *2;
int c = 0;
std::chrono::time_point<std::chrono::system_clock> last =
std::chrono::system_clock::now();
bool is_calibrated = false;
while (!robot->IsTimeout())
{
if (((std::chrono::duration<double>)(std::chrono::system_clock::now()-last)).count() > 0.001)
{
last = std::chrono::system_clock::now(); // last+dt would be better
robot->ParseSensorData();
if (robot->IsReady())
{
if (!is_calibrated)
{
is_calibrated = calib_ctrl->Run();
if (is_calibrated)
{
std::cout << "Calibration is done." << std::endl;
}
}
else
{
// Run the main controller.
auto pos = robot->joints->GetPositions();
auto vel = robot->joints->GetVelocities();
positions[0] = 0;
positions[1] = amplitude * sin(2 * M_PI * freq * t);
positions[2] = - amplitude * sin(2 * M_PI * freq * t);
positions[3] = 0;
positions[4] = - amplitude * sin(2 * M_PI * freq * t);
positions[5] = amplitude * sin(2 * M_PI * freq * t);
t += dt;
robot->joints->SetDesiredPositions(positions);