cleanup locomotion and add standalone simulation project (mjsim_quad)
This commit is contained in:
Nimesh Khandelwal
parent
5428bffe53
commit
99115550e5
|
|
@ -0,0 +1,3 @@
|
|||
[submodule "mjsim_quad"]
|
||||
path = mjsim_quad
|
||||
url = https://github.com/nimesh00/mjsim_quad.git
|
||||
|
|
@ -22,4 +22,4 @@ add_subdirectory(src/utils)
|
|||
add_subdirectory(src/dynamics)
|
||||
add_subdirectory(src/communication)
|
||||
add_subdirectory(src/execution)
|
||||
add_subdirectory(src/robot_simulation)
|
||||
# add_subdirectory(src/robot_simulation)
|
||||
|
|
@ -1,49 +0,0 @@
|
|||
find_package(rclcpp REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
/opt/ros/foxy/include
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION ${COMMON_INCLUDE_DIR}
|
||||
FILES_MATCHING PATTERN "*.h*"
|
||||
)
|
||||
|
||||
add_library(quad_communication
|
||||
src/CommunicationManager.cpp
|
||||
src/SHM.cpp
|
||||
)
|
||||
install(TARGETS quad_communication
|
||||
ARCHIVE DESTINATION ${COMMON_LIBRARY_DIR}
|
||||
LIBRARY DESTINATION ${COMMON_LIBRARY_DIR}
|
||||
)
|
||||
|
||||
add_library(ros_comm
|
||||
src/QuadROSComm.cpp
|
||||
)
|
||||
target_link_libraries(ros_comm
|
||||
${rclcpp_LIBRARIES}
|
||||
${sensor_msgs_LIBRARIES}
|
||||
${nav_msgs_LIBRARIES}
|
||||
quad_communication
|
||||
)
|
||||
install(TARGETS
|
||||
ros_comm
|
||||
ARCHIVE DESTINATION ${COMMON_LIBRARY_DIR}
|
||||
LIBRARY DESTINATION ${COMMON_LIBRARY_DIR}
|
||||
)
|
||||
|
||||
add_executable(test_ros_comm
|
||||
tests/test_ros_comm.cpp
|
||||
)
|
||||
target_link_libraries(test_ros_comm
|
||||
ros_comm
|
||||
)
|
||||
install(TARGETS
|
||||
test_ros_comm
|
||||
ARCHIVE DESTINATION ${COMMON_INSTALL_DIR}
|
||||
RUNTIME DESTINATION ${COMMON_INSTALL_DIR}
|
||||
)
|
||||
|
|
@ -1,53 +0,0 @@
|
|||
#include "CommunicationManager.hpp"
|
||||
|
||||
CommunicationManager::CommunicationManager(const DATA_ACCESS_MODE& mode) : m_name("svan"), m_mode(mode) {
|
||||
}
|
||||
|
||||
CommunicationManager::CommunicationManager(const std::string& name, const DATA_ACCESS_MODE& mode) : m_name(name), m_mode(mode) {
|
||||
}
|
||||
|
||||
void CommunicationManager::writeSensorData(const QuadrupedSensorData& sensor_data) {
|
||||
if (m_mode == DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) {
|
||||
std::cout << "Access mode set to only read sensor data. Cannot Write. Operation failed.\n";
|
||||
return;
|
||||
}
|
||||
if (m_sensor_data_ptr == NULL) {
|
||||
std::cout << "Could not get the address to write sensor data...\n";
|
||||
return;
|
||||
}
|
||||
m_sensor_data_ptr->copy(sensor_data);
|
||||
}
|
||||
|
||||
void CommunicationManager::writeCommandData(const QuadrupedCommandData& cmd_data) {
|
||||
if (m_mode == DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) {
|
||||
std::cout << "Access mode set to only read command data. Cannot Write. Operation failed.\n";
|
||||
return;
|
||||
}
|
||||
if (m_joint_command_data_ptr == NULL) {
|
||||
std::cout << "Could not get the address to write command data...\n";
|
||||
return;
|
||||
}
|
||||
m_joint_command_data_ptr->copy(cmd_data);
|
||||
}
|
||||
|
||||
void CommunicationManager::getSensorData(QuadrupedSensorData& sensor_data) {
|
||||
if (m_mode == DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) {
|
||||
std::cout << "Access mode set to only write sensor data. Cannot read. Operation failed.\n";
|
||||
return;
|
||||
}
|
||||
if (m_sensor_data_ptr == NULL) {
|
||||
std::cout << "Memory not initialized. Cannot read.\n";
|
||||
}
|
||||
sensor_data.copy(*m_sensor_data_ptr);
|
||||
}
|
||||
|
||||
void CommunicationManager::getCommandData(QuadrupedCommandData& cmd_data) {
|
||||
if (m_mode == DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) {
|
||||
std::cout << "Access mode set to only write command data. Cannot read. Operation failed.\n";
|
||||
return;
|
||||
}
|
||||
if (m_joint_command_data_ptr == NULL) {
|
||||
std::cout << "Memory not initialized. Cannot read.\n";
|
||||
}
|
||||
cmd_data.copy(*m_joint_command_data_ptr);
|
||||
}
|
||||
|
|
@ -1,172 +0,0 @@
|
|||
#include "QuadROSComm.hpp"
|
||||
|
||||
QuadROSComm::QuadROSComm() : m_name("svan"), rclcpp::Node("svan"), CommunicationManager(DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) {
|
||||
InitClass();
|
||||
}
|
||||
|
||||
QuadROSComm::QuadROSComm(const std::string& name) : m_name(name), rclcpp::Node(name), CommunicationManager(name, DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) {
|
||||
InitClass();
|
||||
}
|
||||
|
||||
QuadROSComm::~QuadROSComm() {
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
void QuadROSComm::InitClass() {
|
||||
m_dt = 1./m_loop_rate;
|
||||
|
||||
m_joint_state_pub = this->create_publisher<sensor_msgs::msg::JointState>("/" + m_name + "/joint_cmd", 1);
|
||||
auto pub_timer = std::chrono::duration<double>(m_dt);
|
||||
m_timer = this->create_wall_timer(pub_timer, std::bind(&QuadROSComm::timer_cb, this));
|
||||
|
||||
m_joint_state_sub = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/" + m_name + "/joint_state",
|
||||
1,
|
||||
std::bind(&QuadROSComm::get_joint_state_cb, this, std::placeholders::_1)
|
||||
);
|
||||
m_imu_sub = this->create_subscription<sensor_msgs::msg::Imu>(
|
||||
"/" + m_name + "/imu",
|
||||
1,
|
||||
std::bind(&QuadROSComm::get_imu_cb, this, std::placeholders::_1)
|
||||
);
|
||||
m_odom_sub = this->create_subscription<nav_msgs::msg::Odometry>(
|
||||
"/" + m_name + "/odom",
|
||||
1,
|
||||
std::bind(&QuadROSComm::get_odom_cb, this, std::placeholders::_1)
|
||||
);
|
||||
|
||||
js_data = sensor_msgs::msg::JointState();
|
||||
js_data.position.clear();
|
||||
js_data.velocity.clear();
|
||||
js_data.effort.clear();
|
||||
for (int i = 0; i < 12; ++i) {
|
||||
js_data.position.push_back(0);
|
||||
js_data.velocity.push_back(0);
|
||||
js_data.effort.push_back(0);
|
||||
}
|
||||
}
|
||||
|
||||
void QuadROSComm::get_joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const {
|
||||
for (int i = 0; i < 12; ++i) {
|
||||
m_sensor_data_ptr -> q(i) = msg -> position[i];
|
||||
m_sensor_data_ptr -> qd(i) = msg -> velocity[i];
|
||||
m_sensor_data_ptr -> tau(i) = msg -> effort[i];
|
||||
}
|
||||
}
|
||||
|
||||
void QuadROSComm::get_imu_cb(const sensor_msgs::msg::Imu::SharedPtr msg) const {
|
||||
m_sensor_data_ptr -> quat(0) = msg->orientation.x;
|
||||
m_sensor_data_ptr -> quat(1) = msg->orientation.y;
|
||||
m_sensor_data_ptr -> quat(2) = msg->orientation.z;
|
||||
m_sensor_data_ptr -> quat(3) = msg->orientation.w;
|
||||
|
||||
m_sensor_data_ptr -> w_B(0) = msg->angular_velocity.x;
|
||||
m_sensor_data_ptr -> w_B(1) = msg->angular_velocity.y;
|
||||
m_sensor_data_ptr -> w_B(2) = msg->angular_velocity.z;
|
||||
|
||||
m_sensor_data_ptr -> a_B(0) = msg->linear_acceleration.x;
|
||||
m_sensor_data_ptr -> a_B(1) = msg->linear_acceleration.y;
|
||||
m_sensor_data_ptr -> a_B(2) = msg->linear_acceleration.z;
|
||||
|
||||
// TODO: populate a_W and w_W as well
|
||||
}
|
||||
|
||||
void QuadROSComm::get_odom_cb(const nav_msgs::msg::Odometry::SharedPtr msg) const {
|
||||
// TODO: Use Odom data as required. Maybe extend sensor_data to have odom data?
|
||||
}
|
||||
|
||||
void QuadROSComm::timer_cb() {
|
||||
for (int i = 0; i < 12; ++i) {
|
||||
js_data.position[i] = m_joint_command_data_ptr -> q(i);
|
||||
js_data.velocity[i] = m_joint_command_data_ptr -> qd(i);
|
||||
js_data.effort[i] = m_joint_command_data_ptr -> tau(i);
|
||||
}
|
||||
m_joint_state_pub->publish(js_data);
|
||||
}
|
||||
|
||||
void QuadROSComm::Run() {
|
||||
// rclcpp::init(0, nullptr);
|
||||
rclcpp::spin(shared_from_this());
|
||||
}
|
||||
|
||||
/***********************************************************/
|
||||
|
||||
CommROSP2E::CommROSP2E() : m_name("svan"), rclcpp::Node("svan"), CommunicationManager(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) {
|
||||
InitClass();
|
||||
}
|
||||
|
||||
CommROSP2E::CommROSP2E(const std::string& name) : m_name(name), rclcpp::Node(name), CommunicationManager(name, DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) {
|
||||
InitClass();
|
||||
}
|
||||
|
||||
CommROSP2E::~CommROSP2E() {
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
void CommROSP2E::InitClass() {
|
||||
m_dt = 1./m_loop_rate;
|
||||
|
||||
m_joint_state_pub = this->create_publisher<sensor_msgs::msg::JointState>("/" + m_name + "/joint_state", 1);
|
||||
m_imu_pub = this->create_publisher<sensor_msgs::msg::Imu>("/" + m_name + "/imu", 1);
|
||||
m_odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("/" + m_name + "/odom", 1);
|
||||
|
||||
auto pub_timer = std::chrono::duration<double>(m_dt);
|
||||
m_timer = this->create_wall_timer(pub_timer, std::bind(&CommROSP2E::timer_cb, this));
|
||||
|
||||
m_joint_cmd_sub = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/" + m_name + "/joint_cmd",
|
||||
1,
|
||||
std::bind(&CommROSP2E::get_joint_cmd_cb, this, std::placeholders::_1)
|
||||
);
|
||||
|
||||
js_data = sensor_msgs::msg::JointState();
|
||||
imu_data = sensor_msgs::msg::Imu();
|
||||
odom_data = nav_msgs::msg::Odometry();
|
||||
|
||||
js_data.position.clear();
|
||||
js_data.velocity.clear();
|
||||
js_data.effort.clear();
|
||||
for (int i = 0; i < 12; ++i) {
|
||||
js_data.position.push_back(0);
|
||||
js_data.velocity.push_back(0);
|
||||
js_data.effort.push_back(0);
|
||||
}
|
||||
}
|
||||
|
||||
void CommROSP2E::get_joint_cmd_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const {
|
||||
for (int i = 0; i < 12; ++i) {
|
||||
m_joint_command_data_ptr -> q(i) = msg -> position[i];
|
||||
m_joint_command_data_ptr -> qd(i) = msg -> velocity[i];
|
||||
m_joint_command_data_ptr -> tau(i) = msg -> effort[i];
|
||||
}
|
||||
}
|
||||
|
||||
void CommROSP2E::timer_cb() {
|
||||
for (int i = 0; i < 12; ++i) {
|
||||
js_data.position[i] = m_sensor_data_ptr -> q(i);
|
||||
js_data.velocity[i] = m_sensor_data_ptr -> qd(i);
|
||||
js_data.effort[i] = m_sensor_data_ptr -> tau(i);
|
||||
}
|
||||
// Populate imu_data
|
||||
imu_data.orientation.w = m_sensor_data_ptr -> quat(3);
|
||||
imu_data.orientation.x = m_sensor_data_ptr -> quat(0);
|
||||
imu_data.orientation.y = m_sensor_data_ptr -> quat(1);
|
||||
imu_data.orientation.z = m_sensor_data_ptr -> quat(2);
|
||||
imu_data.linear_acceleration.x = m_sensor_data_ptr -> a_B(0);
|
||||
imu_data.linear_acceleration.y = m_sensor_data_ptr -> a_B(1);
|
||||
imu_data.linear_acceleration.z = m_sensor_data_ptr -> a_B(2);
|
||||
imu_data.angular_velocity.x = m_sensor_data_ptr -> w_B(0);
|
||||
imu_data.angular_velocity.y = m_sensor_data_ptr -> w_B(1);
|
||||
imu_data.angular_velocity.z = m_sensor_data_ptr -> w_B(2);
|
||||
// Populdate odometry data
|
||||
|
||||
// Publish stuff
|
||||
m_joint_state_pub->publish(js_data);
|
||||
m_imu_pub->publish(imu_data);
|
||||
m_odom_pub->publish(odom_data);
|
||||
}
|
||||
|
||||
void CommROSP2E::Run() {
|
||||
// rclcpp::init(0, nullptr);
|
||||
rclcpp::spin(shared_from_this());
|
||||
}
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
#include "QuadROSComm.hpp"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
std::string robot_name("go2");
|
||||
|
||||
QuadrupedCommandData cmd_data;
|
||||
QuadrupedSensorData sensor_data;
|
||||
auto comm_obj = std::make_shared<QuadROSComm>(robot_name);
|
||||
comm_obj->setSensorDataPtr(&sensor_data);
|
||||
comm_obj->setCommandDataPtr(&cmd_data);
|
||||
// QuadROSComm comm_obj(robot_name);
|
||||
|
||||
comm_obj->Run();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,3 +1,10 @@
|
|||
cmake_minimum_required(VERSION 3.5.1)
|
||||
project(QuadrupedControl)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
|
||||
# INTERPROCEDURAL_OPTIMIZATION is enforced when enabled.
|
||||
set(CMAKE_POLICY_DEFAULT_CMP0069 NEW)
|
||||
# Default to GLVND if available.
|
||||
|
|
@ -9,7 +16,7 @@ set(OpenGL_GL_PREFERENCE GLVND)
|
|||
# set(THREADS_PREFER_PTHREAD_FLAG ON)
|
||||
|
||||
# find_package(mujoco CONFIG REQUIRED)
|
||||
# find_package(glfw3 REQUIRED)
|
||||
find_package(glfw3 REQUIRED)
|
||||
find_package(Threads REQUIRED)
|
||||
|
||||
include_directories(
|
||||
|
|
@ -23,32 +30,25 @@ install(DIRECTORY include/
|
|||
DESTINATION ${COMMON_INCLUDE_DIR}
|
||||
FILES_MATCHING PATTERN "*.h*"
|
||||
)
|
||||
|
||||
add_executable(quad_sim src/main.cpp)
|
||||
add_dependencies(quad_sim quad_communication)
|
||||
target_link_libraries(quad_sim
|
||||
add_executable(simulate src/sim_main.cpp)
|
||||
if (DEFINED ENV{USE_ROS_COMM})
|
||||
add_definitions(-DUSE_ROS_COMM)
|
||||
endif()
|
||||
add_dependencies(simulate quad_communication)
|
||||
target_link_libraries(simulate
|
||||
# mujoco::mujoco
|
||||
mujoco
|
||||
glfw
|
||||
Threads::Threads
|
||||
quad_communication
|
||||
# $<$<BOOL:$ENV{USE_ROS_COMM}>:ros_comm>
|
||||
)
|
||||
install(TARGETS quad_sim
|
||||
ARCHIVE DESTINATION ${COMMON_INSTALL_DIR}
|
||||
RUNTIME DESTINATION ${COMMON_INSTALL_DIR}
|
||||
)
|
||||
|
||||
add_executable(sim_ros src/main_ros.cpp)
|
||||
add_dependencies(sim_ros ros_comm)
|
||||
target_link_libraries(sim_ros
|
||||
# mujoco::mujoco
|
||||
mujoco
|
||||
glfw
|
||||
Threads::Threads
|
||||
# quad_communication
|
||||
ros_comm
|
||||
)
|
||||
install(TARGETS sim_ros
|
||||
if (DEFINED ENV{USE_ROS_COMM})
|
||||
target_link_libraries(simulate
|
||||
ros_comm
|
||||
)
|
||||
endif()
|
||||
install(TARGETS simulate
|
||||
ARCHIVE DESTINATION ${COMMON_INSTALL_DIR}
|
||||
RUNTIME DESTINATION ${COMMON_INSTALL_DIR}
|
||||
)
|
||||
|
|
@ -1,77 +0,0 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
|
||||
#include <GLFW/glfw3.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
// Libraries for sleep
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include <sys/ipc.h>
|
||||
#include <sys/shm.h>
|
||||
#include "memory_types.hpp"
|
||||
|
||||
class MJSim {
|
||||
public:
|
||||
MJSim();
|
||||
~MJSim();
|
||||
|
||||
virtual void CustomController(const mjModel* model, mjData* data);
|
||||
|
||||
void Run();
|
||||
private:
|
||||
void init_sim();
|
||||
|
||||
// keyboard callback
|
||||
static void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods);
|
||||
|
||||
// mouse button callback
|
||||
static void mouse_button(GLFWwindow* window, int button, int act, int mods);
|
||||
|
||||
// mouse move callback
|
||||
static void mouse_move(GLFWwindow* window, double xpos, double ypos);
|
||||
|
||||
// scroll callback
|
||||
static void scroll(GLFWwindow* window, double xoffset, double yoffset);
|
||||
|
||||
void UpdateSensorData();
|
||||
|
||||
std::string m_name;
|
||||
|
||||
// MuJoCo data structures
|
||||
mjModel* m = NULL; // MuJoCo model
|
||||
mjData* d = NULL; // MuJoCo data
|
||||
mjvCamera cam; // abstract camera
|
||||
mjvOption opt; // visualization options
|
||||
mjvScene scn; // abstract scene
|
||||
mjrContext con; // custom GPU context
|
||||
GLFWwindow* window;
|
||||
|
||||
// mouse interaction
|
||||
bool button_left = false;
|
||||
bool button_middle = false;
|
||||
bool button_right = false;
|
||||
double lastx = 0;
|
||||
double lasty = 0;
|
||||
|
||||
// holders of one step history of time and position to calculate dertivatives
|
||||
mjtNum position_history = 0;
|
||||
mjtNum previous_time = 0;
|
||||
|
||||
// controller related variables
|
||||
float_t ctrl_update_freq = 100;
|
||||
mjtNum last_update = 0.0;
|
||||
mjtNum ctrl;
|
||||
|
||||
// Variables for SHM read (Control commands: joint position, velocity, torque)
|
||||
key_t read_key;
|
||||
QuadrupedCommandData joint_command_data;
|
||||
// Variables for SHM write (Sensor data: joint position, velocity, torque, imu data and probably base pose and contact states if required)
|
||||
key_t write_key;
|
||||
QuadrupedSensorData sensor_data;
|
||||
};
|
||||
|
|
@ -5,29 +5,40 @@
|
|||
#include <iostream>
|
||||
|
||||
enum DATA_ACCESS_MODE {
|
||||
PLANT_TO_EXECUTOR,
|
||||
EXECUTOR_TO_PLANT
|
||||
PLANT,
|
||||
EXECUTOR
|
||||
};
|
||||
|
||||
// Single communication manager for quadruped
|
||||
class CommunicationManager {
|
||||
public:
|
||||
CommunicationManager();
|
||||
CommunicationManager(const DATA_ACCESS_MODE& mode);
|
||||
CommunicationManager(const std::string&, const DATA_ACCESS_MODE& mode);
|
||||
~CommunicationManager() {}
|
||||
|
||||
void writeSensorData(const QuadrupedSensorData& sensor_data);
|
||||
void writeCommandData(const QuadrupedCommandData& cmd_data);
|
||||
|
||||
void setSensorDataPtr(QuadrupedSensorData* sensor_data_ptr) {
|
||||
m_sensor_data_ptr = sensor_data_ptr;
|
||||
}
|
||||
void setCommandDataPtr(QuadrupedCommandData* command_data_ptr) {
|
||||
m_joint_command_data_ptr = command_data_ptr;
|
||||
}
|
||||
void setMeasurementDataPtr(QuadrupedMeasurementData* measurement_data_ptr) {
|
||||
m_measurement_data_ptr = measurement_data_ptr;
|
||||
}
|
||||
void setEstimationDataPtr(QuadrupedEstimationData* estimation_data_ptr) {
|
||||
m_estimation_data_ptr = estimation_data_ptr;
|
||||
}
|
||||
|
||||
void writeSensorData(const QuadrupedSensorData& sensor_data);
|
||||
void writeCommandData(const QuadrupedCommandData& cmd_data);
|
||||
void writeMeasurementData(const QuadrupedMeasurementData& measure_data);
|
||||
void writeEstimationData(const QuadrupedEstimationData& est_data);
|
||||
|
||||
void getSensorData(QuadrupedSensorData& sensor_data);
|
||||
void getCommandData(QuadrupedCommandData& cmd_data);
|
||||
void getMeasurememtData(QuadrupedMeasurementData& measure_data);
|
||||
void getEstimationData(QuadrupedEstimationData& est_data);
|
||||
|
||||
void setAccessMode(const DATA_ACCESS_MODE& mode) {
|
||||
m_mode = mode;
|
||||
|
|
@ -36,10 +47,10 @@ public:
|
|||
protected:
|
||||
QuadrupedSensorData* m_sensor_data_ptr = NULL;
|
||||
QuadrupedCommandData* m_joint_command_data_ptr = NULL;
|
||||
QuadrupedEstimationData* m_estimation_data_ptr = NULL;
|
||||
QuadrupedMeasurementData* m_measurement_data_ptr = NULL;
|
||||
|
||||
int m_mode = DATA_ACCESS_MODE::PLANT_TO_EXECUTOR;
|
||||
int m_mode = DATA_ACCESS_MODE::PLANT;
|
||||
private:
|
||||
std::string m_name;
|
||||
// QuadrupedEstimatoinData m_estimation_data;
|
||||
// QuadrupedPlannerData m_planner_data;
|
||||
};
|
||||
|
|
@ -10,19 +10,28 @@
|
|||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/point.hpp"
|
||||
#include "std_msgs/msg/float32.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
// using std::placeholders::_1;
|
||||
|
||||
class QuadROSComm : public rclcpp::Node, public CommunicationManager {
|
||||
public:
|
||||
QuadROSComm();
|
||||
QuadROSComm(const std::string& name);
|
||||
QuadROSComm(const DATA_ACCESS_MODE& mode);
|
||||
QuadROSComm(const std::string& name, const DATA_ACCESS_MODE& mode);
|
||||
~QuadROSComm();
|
||||
|
||||
void setUpdateRate(const float& rate) {
|
||||
m_loop_rate = rate;
|
||||
m_dt = 1./rate;
|
||||
}
|
||||
|
||||
void Run();
|
||||
private:
|
||||
|
||||
void InitClass();
|
||||
private:
|
||||
std::string m_name;
|
||||
float m_loop_rate = 1000;
|
||||
float m_dt = 0.001;
|
||||
|
|
@ -32,6 +41,23 @@ private:
|
|||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_odom_sub;
|
||||
// Publisher(s)
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_joint_state_pub;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_est_cs_pub[4];
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_est_pc_pub[4];
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_est_base_position_pub[3];
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_est_base_velocity_pub[3];
|
||||
|
||||
// Subscriber(s)
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr m_joint_cmd_sub;
|
||||
// Publisher(s)
|
||||
// rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_joint_state_pub;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr m_imu_pub;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr m_odom_pub;
|
||||
// Measurement publishers
|
||||
rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr m_base_position_pub;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr m_base_vel_pub;
|
||||
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr m_base_acc_pub;
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_contact_forces_pub[12];
|
||||
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr m_estimated_fc_pub[12];
|
||||
|
||||
// Timer
|
||||
rclcpp::TimerBase::SharedPtr m_timer;
|
||||
|
|
@ -40,40 +66,23 @@ private:
|
|||
void get_joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const;
|
||||
void get_imu_cb(const sensor_msgs::msg::Imu::SharedPtr msg) const;
|
||||
void get_odom_cb(const nav_msgs::msg::Odometry::SharedPtr msg) const;
|
||||
// Timer callback
|
||||
void timer_cb();
|
||||
|
||||
sensor_msgs::msg::JointState js_data;
|
||||
};
|
||||
|
||||
class CommROSP2E : public rclcpp::Node, public CommunicationManager {
|
||||
public:
|
||||
CommROSP2E();
|
||||
CommROSP2E(const std::string& name);
|
||||
~CommROSP2E();
|
||||
|
||||
void Run();
|
||||
private:
|
||||
void InitClass();
|
||||
std::string m_name;
|
||||
float m_loop_rate = 1000;
|
||||
float m_dt = 0.001;
|
||||
// Subscriber(s)
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr m_joint_cmd_sub;
|
||||
// Publisher(s)
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_joint_state_pub;
|
||||
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr m_imu_pub;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr m_odom_pub;
|
||||
|
||||
// Timer
|
||||
rclcpp::TimerBase::SharedPtr m_timer;
|
||||
|
||||
// Subscription callbacks
|
||||
void get_joint_cmd_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const;
|
||||
// Timer callback
|
||||
void timer_cb();
|
||||
|
||||
// sensor_msgs::msg::JointState js_data;
|
||||
std_msgs::msg::Float32 est_cs_data[4];
|
||||
std_msgs::msg::Float32 est_pc_data[4];
|
||||
std_msgs::msg::Float32 est_base_pos_data[3];
|
||||
std_msgs::msg::Float32 est_base_vel_data[3];
|
||||
|
||||
sensor_msgs::msg::JointState js_data;
|
||||
sensor_msgs::msg::Imu imu_data;
|
||||
nav_msgs::msg::Odometry odom_data;
|
||||
geometry_msgs::msg::Point base_position_data;
|
||||
geometry_msgs::msg::Twist base_velocity_data;
|
||||
geometry_msgs::msg::Twist base_acceleration_data;
|
||||
std_msgs::msg::Float32 contact_forces_data[12];
|
||||
std_msgs::msg::Float32 estimated_fc_data[12];
|
||||
};
|
||||
|
|
@ -7,6 +7,7 @@
|
|||
// Randomy chosen key enums
|
||||
enum SHM_KEYS {
|
||||
SENSOR_DATA = 123,
|
||||
MEASUREMENT_DATA = 496,
|
||||
COMMAND_DATA = 549
|
||||
};
|
||||
|
||||
|
|
@ -1,248 +0,0 @@
|
|||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
|
||||
#include <GLFW/glfw3.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
// Libraries for sleep
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "memory_types.hpp"
|
||||
#include "SHM.hpp"
|
||||
|
||||
#define COMPILE_WITH_VISUALIZATION
|
||||
|
||||
// MuJoCo data structures
|
||||
mjModel* m = NULL; // MuJoCo model
|
||||
mjData* d = NULL; // MuJoCo data
|
||||
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
mjvCamera cam; // abstract camera
|
||||
mjvOption opt; // visualization options
|
||||
mjvScene scn; // abstract scene
|
||||
mjrContext con; // custom GPU context
|
||||
GLFWwindow* window;
|
||||
|
||||
// mouse interaction
|
||||
bool button_left = false;
|
||||
bool button_middle = false;
|
||||
bool button_right = false;
|
||||
double lastx = 0;
|
||||
double lasty = 0;
|
||||
#endif
|
||||
|
||||
// holders of one step history of time and position to calculate dertivatives
|
||||
mjtNum position_history = 0;
|
||||
mjtNum previous_time = 0;
|
||||
mjtNum timezero = 0;
|
||||
|
||||
// controller related variables
|
||||
float_t ctrl_update_freq = 500;
|
||||
mjtNum last_update = 0.0;
|
||||
mjtNum ctrl;
|
||||
|
||||
// // Variables for SHM read (Control commands: joint position, velocity, torque)
|
||||
// key_t read_key;
|
||||
// QuadrupedCommandData joint_command_data;
|
||||
// // Variables for SHM write (Sensor data: joint position, velocity, torque, imu data and probably base pose and contact states if required)
|
||||
// key_t write_key;
|
||||
// QuadrupedSensorData sensor_data;
|
||||
|
||||
QuadrupedSensorData sensor_data;
|
||||
QuadrupedCommandData joint_command_data;
|
||||
SHM comm_data(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR);
|
||||
|
||||
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
// keyboard callback
|
||||
void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods) {
|
||||
// backspace: reset simulation
|
||||
if (act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE) {
|
||||
mj_resetData(m, d);
|
||||
mj_forward(m, d);
|
||||
}
|
||||
}
|
||||
|
||||
// mouse button callback
|
||||
void mouse_button(GLFWwindow* window, int button, int act, int mods) {
|
||||
// update button state
|
||||
button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS);
|
||||
button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS);
|
||||
button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS);
|
||||
|
||||
// update mouse position
|
||||
glfwGetCursorPos(window, &lastx, &lasty);
|
||||
}
|
||||
|
||||
// mouse move callback
|
||||
void mouse_move(GLFWwindow* window, double xpos, double ypos) {
|
||||
// no buttons down: nothing to do
|
||||
if (!button_left && !button_middle && !button_right) {
|
||||
return;
|
||||
}
|
||||
|
||||
// compute mouse displacement, save
|
||||
double dx = xpos - lastx;
|
||||
double dy = ypos - lasty;
|
||||
lastx = xpos;
|
||||
lasty = ypos;
|
||||
|
||||
// get current window size
|
||||
int width, height;
|
||||
glfwGetWindowSize(window, &width, &height);
|
||||
|
||||
// get shift key state
|
||||
bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||
|
||||
glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);
|
||||
|
||||
// determine action based on mouse button
|
||||
mjtMouse action;
|
||||
if (button_right) {
|
||||
action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
|
||||
} else if (button_left) {
|
||||
action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
|
||||
} else {
|
||||
action = mjMOUSE_ZOOM;
|
||||
}
|
||||
|
||||
// move camera
|
||||
mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam);
|
||||
}
|
||||
|
||||
// scroll callback
|
||||
void scroll(GLFWwindow* window, double xoffset, double yoffset) {
|
||||
// emulate vertical mouse motion = 5% of window height
|
||||
mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam);
|
||||
}
|
||||
#endif
|
||||
|
||||
void UpdateSensorData();
|
||||
void CustomController(const mjModel*, mjData*);
|
||||
|
||||
void init_model(std::string model_file) {
|
||||
// load and compile model
|
||||
char error[1000] = "Could not load binary model";
|
||||
if (std::strlen(model_file.c_str())>4 && !std::strcmp(model_file.c_str() + std::strlen(model_file.c_str())-4, ".mjb")) {
|
||||
m = mj_loadModel(model_file.c_str(), 0);
|
||||
} else {
|
||||
m = mj_loadXML(model_file.c_str(), 0, error, 1000);
|
||||
}
|
||||
if (!m) {
|
||||
mju_error("Load model error: %s", error);
|
||||
}
|
||||
// make data
|
||||
d = mj_makeData(m);
|
||||
}
|
||||
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
void init_visualizer() {
|
||||
// init GLFW
|
||||
if (!glfwInit()) {
|
||||
mju_error("Could not initialize GLFW");
|
||||
}
|
||||
|
||||
// create window, make OpenGL context current, request v-sync
|
||||
window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL);
|
||||
glfwMakeContextCurrent(window);
|
||||
glfwSwapInterval(1);
|
||||
|
||||
// initialize visualization data structures
|
||||
mjv_defaultCamera(&cam);
|
||||
mjv_defaultOption(&opt);
|
||||
mjv_defaultScene(&scn);
|
||||
mjr_defaultContext(&con);
|
||||
|
||||
// create scene and context
|
||||
mjv_makeScene(m, &scn, 2000);
|
||||
mjr_makeContext(m, &con, mjFONTSCALE_150);
|
||||
|
||||
// install GLFW mouse and keyboard callbacks
|
||||
glfwSetKeyCallback(window, keyboard);
|
||||
glfwSetCursorPosCallback(window, mouse_move);
|
||||
glfwSetMouseButtonCallback(window, mouse_button);
|
||||
glfwSetScrollCallback(window, scroll);
|
||||
}
|
||||
#endif
|
||||
|
||||
void run_simulation(float sim_time = 1, bool show_visualization=false, float fps=60.) {
|
||||
#ifndef COMPILE_WITH_VISUALIZATION
|
||||
if (show_visualization) {
|
||||
std::cerr << "Exec compiled with Visuzlization disabled. Re-compile with COMPILE_WITH_VISUALIZATION flag enabled to start visualization!\n";
|
||||
}
|
||||
#endif
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
if (show_visualization) {
|
||||
init_visualizer();
|
||||
// use the first while condition if you want to simulate for a period.
|
||||
while( !glfwWindowShouldClose(window) && d->time-timezero < sim_time) {
|
||||
// while( !glfwWindowShouldClose(window)) {
|
||||
// advance interactive simulation for 1/60 sec
|
||||
// Assuming MuJoCo can simulate faster than real-time, which it usually can,
|
||||
// this loop will finish on time for the next frame to be rendered at 60 fps.
|
||||
// Otherwise add a cpu timer and exit this loop when it is time to render.
|
||||
#endif
|
||||
mjtNum simstart = d->time;
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
while( d->time - simstart < 1.0/fps ) {
|
||||
#else
|
||||
while (d->time - simstart < sim_time) {
|
||||
#endif
|
||||
UpdateSensorData();
|
||||
mj_step(m, d);
|
||||
}
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
// 15 ms is a little smaller than 60 Hz.
|
||||
int delay_ms = int(double(1e3)/fps);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
||||
// get framebuffer viewport
|
||||
mjrRect viewport = {0, 0, 0, 0};
|
||||
glfwGetFramebufferSize(window, &viewport.width, &viewport.height);
|
||||
|
||||
// update scene and render
|
||||
mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
|
||||
mjr_render(viewport, &scn, &con);
|
||||
|
||||
// swap OpenGL buffers (blocking call due to v-sync)
|
||||
glfwSwapBuffers(window);
|
||||
|
||||
// process pending GUI events, call GLFW callbacks
|
||||
glfwPollEvents();
|
||||
|
||||
}
|
||||
} else {
|
||||
mjtNum simstart = d->time;
|
||||
float dt = 0;
|
||||
if (fps == 0) {
|
||||
dt = sim_time;
|
||||
} else {
|
||||
dt = 1./fps;
|
||||
}
|
||||
while (d->time - simstart < dt) {
|
||||
UpdateSensorData();
|
||||
mj_step(m, d);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void shutdown_simulation() {
|
||||
// free visualization storage
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
mjv_freeScene(&scn);
|
||||
mjr_freeContext(&con);
|
||||
#endif
|
||||
|
||||
// free MuJoCo model and data, deactivate
|
||||
mj_deleteData(d);
|
||||
mj_deleteModel(m);
|
||||
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
// terminate GLFW (crashes with Linux NVidia drivers)
|
||||
#if defined(__APPLE__) || defined(_WIN32)
|
||||
glfwTerminate();
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1,325 +0,0 @@
|
|||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SIMULATE_SIMULATE_H_
|
||||
#define MUJOCO_SIMULATE_SIMULATE_H_
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <condition_variable>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <ratio>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjui.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "platform_ui_adapter.h"
|
||||
|
||||
namespace mujoco {
|
||||
|
||||
// The viewer itself doesn't require a reentrant mutex, however we use it in
|
||||
// order to provide a Python sync API that doesn't require separate locking
|
||||
// (since sync is by far the most common operation), but that also won't
|
||||
// deadlock if called when a lock is already held by the user script on the
|
||||
// same thread.
|
||||
class SimulateMutex : public std::recursive_mutex {};
|
||||
using MutexLock = std::unique_lock<std::recursive_mutex>;
|
||||
|
||||
// Simulate states not contained in MuJoCo structures
|
||||
class Simulate {
|
||||
public:
|
||||
using Clock = std::chrono::steady_clock;
|
||||
static_assert(std::ratio_less_equal_v<Clock::period, std::milli>);
|
||||
|
||||
static constexpr int kMaxGeom = 20000;
|
||||
|
||||
// create object and initialize the simulate ui
|
||||
Simulate(
|
||||
std::unique_ptr<PlatformUIAdapter> platform_ui_adapter,
|
||||
mjvCamera* cam, mjvOption* opt, mjvPerturb* pert, bool is_passive);
|
||||
|
||||
// Synchronize mjModel and mjData state with UI inputs, and update
|
||||
// visualization.
|
||||
void Sync();
|
||||
|
||||
void UpdateHField(int hfieldid);
|
||||
void UpdateMesh(int meshid);
|
||||
void UpdateTexture(int texid);
|
||||
|
||||
// Request that the Simulate UI display a "loading" message
|
||||
// Called prior to Load or LoadMessageClear
|
||||
void LoadMessage(const char* displayed_filename);
|
||||
|
||||
// Request that the Simulate UI thread render a new model
|
||||
void Load(mjModel* m, mjData* d, const char* displayed_filename);
|
||||
|
||||
// Clear the loading message
|
||||
// Can be called instead of Load to clear the message without
|
||||
// requesting the UI load a model
|
||||
void LoadMessageClear(void);
|
||||
|
||||
// functions below are used by the renderthread
|
||||
// load mjb or xml model that has been requested by load()
|
||||
void LoadOnRenderThread();
|
||||
|
||||
// render the ui to the window
|
||||
void Render();
|
||||
|
||||
// loop to render the UI (must be called from main thread because of MacOS)
|
||||
void RenderLoop();
|
||||
|
||||
// add state to history buffer
|
||||
void AddToHistory();
|
||||
|
||||
// constants
|
||||
static constexpr int kMaxFilenameLength = 1000;
|
||||
|
||||
// whether the viewer is operating in passive mode, where it cannot assume
|
||||
// that it has exclusive access to mjModel, mjData, and various mjv objects
|
||||
bool is_passive_ = false;
|
||||
|
||||
// model and data to be visualized
|
||||
mjModel* mnew_ = nullptr;
|
||||
mjData* dnew_ = nullptr;
|
||||
|
||||
mjModel* m_ = nullptr;
|
||||
mjData* d_ = nullptr;
|
||||
|
||||
int ncam_ = 0;
|
||||
int nkey_ = 0;
|
||||
int state_size_ = 0; // number of mjtNums in a history buffer state
|
||||
int nhistory_ = 0; // number of states saved in history buffer
|
||||
int history_cursor_ = 0; // cursor pointing at last saved state
|
||||
|
||||
std::vector<int> body_parentid_;
|
||||
|
||||
std::vector<int> jnt_type_;
|
||||
std::vector<int> jnt_group_;
|
||||
std::vector<int> jnt_qposadr_;
|
||||
std::vector<std::optional<std::pair<mjtNum, mjtNum>>> jnt_range_;
|
||||
std::vector<std::string> jnt_names_;
|
||||
|
||||
std::vector<int> actuator_group_;
|
||||
std::vector<std::optional<std::pair<mjtNum, mjtNum>>> actuator_ctrlrange_;
|
||||
std::vector<std::string> actuator_names_;
|
||||
|
||||
std::vector<mjtNum> history_; // history buffer (nhistory x state_size)
|
||||
|
||||
// mjModel and mjData fields that can be modified by the user through the GUI
|
||||
std::vector<mjtNum> qpos_;
|
||||
std::vector<mjtNum> qpos_prev_;
|
||||
std::vector<mjtNum> ctrl_;
|
||||
std::vector<mjtNum> ctrl_prev_;
|
||||
|
||||
mjvSceneState scnstate_;
|
||||
mjOption mjopt_prev_;
|
||||
mjvOption opt_prev_;
|
||||
mjvCamera cam_prev_;
|
||||
|
||||
int warn_vgeomfull_prev_;
|
||||
|
||||
// pending GUI-driven actions, to be applied at the next call to Sync
|
||||
struct {
|
||||
std::optional<std::string> save_xml;
|
||||
std::optional<std::string> save_mjb;
|
||||
std::optional<std::string> print_model;
|
||||
std::optional<std::string> print_data;
|
||||
bool reset;
|
||||
bool align;
|
||||
bool copy_pose;
|
||||
bool load_from_history;
|
||||
bool load_key;
|
||||
bool save_key;
|
||||
bool zero_ctrl;
|
||||
int newperturb;
|
||||
bool select;
|
||||
mjuiState select_state;
|
||||
bool ui_update_simulation;
|
||||
bool ui_update_physics;
|
||||
bool ui_update_rendering;
|
||||
bool ui_update_joint;
|
||||
bool ui_update_ctrl;
|
||||
bool ui_remake_ctrl;
|
||||
} pending_ = {};
|
||||
|
||||
SimulateMutex mtx;
|
||||
std::condition_variable_any cond_loadrequest;
|
||||
|
||||
int frames_ = 0;
|
||||
std::chrono::time_point<Clock> last_fps_update_;
|
||||
double fps_ = 0;
|
||||
|
||||
// options
|
||||
int spacing = 0;
|
||||
int color = 0;
|
||||
int font = 0;
|
||||
int ui0_enable = 1;
|
||||
int ui1_enable = 1;
|
||||
int help = 0;
|
||||
int info = 0;
|
||||
int profiler = 0;
|
||||
int sensor = 0;
|
||||
int pause_update = 1;
|
||||
int fullscreen = 0;
|
||||
int vsync = 1;
|
||||
int busywait = 0;
|
||||
|
||||
// keyframe index
|
||||
int key = 0;
|
||||
|
||||
// index of history-scrubber slider
|
||||
int scrub_index = 0;
|
||||
|
||||
// simulation
|
||||
int run = 1;
|
||||
|
||||
// atomics for cross-thread messages
|
||||
std::atomic_int exitrequest = 0;
|
||||
std::atomic_int droploadrequest = 0;
|
||||
std::atomic_int screenshotrequest = 0;
|
||||
std::atomic_int uiloadrequest = 0;
|
||||
|
||||
// loadrequest
|
||||
// 3: display a loading message
|
||||
// 2: render thread asked to update its model
|
||||
// 1: showing "loading" label, about to load
|
||||
// 0: model loaded or no load requested.
|
||||
int loadrequest = 0;
|
||||
|
||||
// strings
|
||||
char load_error[kMaxFilenameLength] = "";
|
||||
char dropfilename[kMaxFilenameLength] = "";
|
||||
char filename[kMaxFilenameLength] = "";
|
||||
char previous_filename[kMaxFilenameLength] = "";
|
||||
|
||||
// time synchronization
|
||||
int real_time_index = 0;
|
||||
bool speed_changed = true;
|
||||
float measured_slowdown = 1.0;
|
||||
// logarithmically spaced real-time slow-down coefficients (percent)
|
||||
static constexpr float percentRealTime[] = {
|
||||
100, 80, 66, 50, 40, 33, 25, 20, 16, 13,
|
||||
10, 8, 6.6, 5.0, 4, 3.3, 2.5, 2, 1.6, 1.3,
|
||||
1, .8, .66, .5, .4, .33, .25, .2, .16, .13,
|
||||
.1
|
||||
};
|
||||
|
||||
// control noise
|
||||
double ctrl_noise_std = 0.0;
|
||||
double ctrl_noise_rate = 0.0;
|
||||
|
||||
// watch
|
||||
char field[mjMAXUITEXT] = "qpos";
|
||||
int index = 0;
|
||||
|
||||
// physics: need sync
|
||||
int disable[mjNDISABLE] = {0};
|
||||
int enable[mjNENABLE] = {0};
|
||||
int enableactuator[mjNGROUP] = {0};
|
||||
|
||||
// rendering: need sync
|
||||
int camera = 0;
|
||||
|
||||
// abstract visualization
|
||||
mjvScene scn;
|
||||
mjvCamera& cam;
|
||||
mjvOption& opt;
|
||||
mjvPerturb& pert;
|
||||
mjvFigure figconstraint = {};
|
||||
mjvFigure figcost = {};
|
||||
mjvFigure figtimer = {};
|
||||
mjvFigure figsize = {};
|
||||
mjvFigure figsensor = {};
|
||||
|
||||
// additional user-defined visualization geoms (used in passive mode)
|
||||
mjvScene* user_scn = nullptr;
|
||||
mjtByte user_scn_flags_prev_[mjNRNDFLAG];
|
||||
|
||||
// OpenGL rendering and UI
|
||||
int refresh_rate = 60;
|
||||
int window_pos[2] = {0};
|
||||
int window_size[2] = {0};
|
||||
std::unique_ptr<PlatformUIAdapter> platform_ui;
|
||||
mjuiState& uistate;
|
||||
mjUI ui0 = {};
|
||||
mjUI ui1 = {};
|
||||
|
||||
// Constant arrays needed for the option section of UI and the UI interface
|
||||
// TODO setting the size here is not ideal
|
||||
const mjuiDef def_option[13] = {
|
||||
{mjITEM_SECTION, "Option", 1, nullptr, "AO"},
|
||||
{mjITEM_CHECKINT, "Help", 2, &this->help, " #290"},
|
||||
{mjITEM_CHECKINT, "Info", 2, &this->info, " #291"},
|
||||
{mjITEM_CHECKINT, "Profiler", 2, &this->profiler, " #292"},
|
||||
{mjITEM_CHECKINT, "Sensor", 2, &this->sensor, " #293"},
|
||||
{mjITEM_CHECKINT, "Pause update", 2, &this->pause_update, ""},
|
||||
#ifdef __APPLE__
|
||||
{mjITEM_CHECKINT, "Fullscreen", 0, &this->fullscreen, " #294"},
|
||||
#else
|
||||
{mjITEM_CHECKINT, "Fullscreen", 1, &this->fullscreen, " #294"},
|
||||
#endif
|
||||
{mjITEM_CHECKINT, "Vertical Sync", 1, &this->vsync, ""},
|
||||
{mjITEM_CHECKINT, "Busy Wait", 1, &this->busywait, ""},
|
||||
{mjITEM_SELECT, "Spacing", 1, &this->spacing, "Tight\nWide"},
|
||||
{mjITEM_SELECT, "Color", 1, &this->color, "Default\nOrange\nWhite\nBlack"},
|
||||
{mjITEM_SELECT, "Font", 1, &this->font, "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"},
|
||||
{mjITEM_END}
|
||||
};
|
||||
|
||||
|
||||
// simulation section of UI
|
||||
const mjuiDef def_simulation[14] = {
|
||||
{mjITEM_SECTION, "Simulation", 1, nullptr, "AS"},
|
||||
{mjITEM_RADIO, "", 5, &this->run, "Pause\nRun"},
|
||||
{mjITEM_BUTTON, "Reset", 2, nullptr, " #259"},
|
||||
{mjITEM_BUTTON, "Reload", 5, nullptr, "CL"},
|
||||
{mjITEM_BUTTON, "Align", 2, nullptr, "CA"},
|
||||
{mjITEM_BUTTON, "Copy pose", 2, nullptr, "CC"},
|
||||
{mjITEM_SLIDERINT, "Key", 3, &this->key, "0 0"},
|
||||
{mjITEM_BUTTON, "Load key", 3},
|
||||
{mjITEM_BUTTON, "Save key", 3},
|
||||
{mjITEM_SLIDERNUM, "Noise scale", 5, &this->ctrl_noise_std, "0 2"},
|
||||
{mjITEM_SLIDERNUM, "Noise rate", 5, &this->ctrl_noise_rate, "0 2"},
|
||||
{mjITEM_SEPARATOR, "History", 1},
|
||||
{mjITEM_SLIDERINT, "", 5, &this->scrub_index, "0 0"},
|
||||
{mjITEM_END}
|
||||
};
|
||||
|
||||
|
||||
// watch section of UI
|
||||
const mjuiDef def_watch[5] = {
|
||||
{mjITEM_SECTION, "Watch", 0, nullptr, "AW"},
|
||||
{mjITEM_EDITTXT, "Field", 2, this->field, "qpos"},
|
||||
{mjITEM_EDITINT, "Index", 2, &this->index, "1"},
|
||||
{mjITEM_STATIC, "Value", 2, nullptr, " "},
|
||||
{mjITEM_END}
|
||||
};
|
||||
|
||||
// info strings
|
||||
char info_title[Simulate::kMaxFilenameLength] = {0};
|
||||
char info_content[Simulate::kMaxFilenameLength] = {0};
|
||||
|
||||
// pending uploads
|
||||
std::condition_variable_any cond_upload_;
|
||||
int texture_upload_ = -1;
|
||||
int mesh_upload_ = -1;
|
||||
int hfield_upload_ = -1;
|
||||
};
|
||||
} // namespace mujoco
|
||||
|
||||
#endif
|
||||
|
|
@ -11,7 +11,11 @@
|
|||
#include <thread>
|
||||
|
||||
#include "memory_types.hpp"
|
||||
#ifdef USE_ROS_COMM
|
||||
#include "QuadROSComm.hpp"
|
||||
#else
|
||||
#include "SHM.hpp"
|
||||
#endif
|
||||
|
||||
#define COMPILE_WITH_VISUALIZATION
|
||||
|
||||
|
|
@ -53,8 +57,13 @@ mjtNum ctrl;
|
|||
|
||||
QuadrupedSensorData sensor_data;
|
||||
QuadrupedCommandData joint_command_data;
|
||||
QuadrupedMeasurementData measurement_data;
|
||||
// SHM comm_data(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR);
|
||||
std::shared_ptr<CommROSP2E> comm_data_ptr;
|
||||
#ifdef USE_ROS_COMM
|
||||
std::shared_ptr<QuadROSComm> comm_data_ptr;
|
||||
#else
|
||||
std::shared_ptr<SHM> comm_data_ptr;
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef COMPILE_WITH_VISUALIZATION
|
||||
|
|
@ -61,7 +61,6 @@ typedef struct QuadrupedEstimationData {
|
|||
vec12 rP;
|
||||
int4 cs;
|
||||
vec4 pc;
|
||||
int num_contact;
|
||||
vec19 js;
|
||||
vec18 jv;
|
||||
QuadrupedEstimationData() {
|
||||
|
|
@ -80,6 +79,8 @@ typedef struct QuadrupedEstimationData {
|
|||
this->rP = est.rP;
|
||||
this->cs = est.cs;
|
||||
this->pc = est.pc;
|
||||
this->js = est.js;
|
||||
this->jv = est.jv;
|
||||
}
|
||||
} QuadrupedEstimationData;
|
||||
|
||||
|
|
@ -113,4 +114,37 @@ typedef struct QuadrupedPlannerData {
|
|||
cs_ref = int4::Zero();
|
||||
pc_ref = vec4::Zero();
|
||||
}
|
||||
} QuadrupedPlannerData;
|
||||
} QuadrupedPlannerData;
|
||||
|
||||
typedef struct QuadrupedMeasurementData {
|
||||
vec3 base_position;
|
||||
// A better name might be PluckerVector?
|
||||
struct TotalVelocity {
|
||||
vec3 linear;
|
||||
vec3 angular;
|
||||
TotalVelocity() {
|
||||
linear = vec3::Zero();
|
||||
angular = vec3::Zero();
|
||||
}
|
||||
};
|
||||
TotalVelocity base_velocity;
|
||||
TotalVelocity base_acceleration;
|
||||
vec12 contact_force;
|
||||
vec12 estimated_contact_force;
|
||||
QuadrupedMeasurementData() {
|
||||
base_position = vec3::Zero();
|
||||
base_velocity = TotalVelocity();
|
||||
base_acceleration = TotalVelocity();
|
||||
contact_force = vec12::Zero();
|
||||
estimated_contact_force = vec12::Zero();
|
||||
}
|
||||
void copy(const QuadrupedMeasurementData& m) {
|
||||
this->base_position = m.base_position;
|
||||
this->base_velocity.linear = m.base_velocity.linear;
|
||||
this->base_velocity.angular = m.base_velocity.angular;
|
||||
this->base_acceleration.linear = m.base_acceleration.linear;
|
||||
this->base_acceleration.angular = m.base_acceleration.angular;
|
||||
this->contact_force = m.contact_force;
|
||||
this->estimated_contact_force = m.estimated_contact_force;
|
||||
}
|
||||
} QuadrupedMeasurementData;
|
||||
|
|
@ -0,0 +1,150 @@
|
|||
#pragma once
|
||||
|
||||
#include <memory_types.hpp>
|
||||
#include <iostream>
|
||||
|
||||
template <typename T> int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
|
||||
namespace pinocchio {
|
||||
inline mat3x3 skew_symm(const vec3 &v) {
|
||||
mat3x3 vx;
|
||||
vx << 0, -v(2), v(1),
|
||||
v(2), 0, -v(0),
|
||||
-v(1), v(0), 0;
|
||||
|
||||
return vx;
|
||||
}
|
||||
|
||||
inline mat3x3 Rx(float x)
|
||||
{
|
||||
mat3x3 R = mat3x3::Zero();
|
||||
|
||||
R << 1, 0, 0,
|
||||
0, cos(x), -sin(x),
|
||||
0, sin(x), cos(x);
|
||||
|
||||
return R;
|
||||
}
|
||||
inline mat3x3 Ry(float x)
|
||||
{
|
||||
mat3x3 R = mat3x3::Zero();
|
||||
|
||||
R << cos(x), 0, sin(x),
|
||||
0, 1, 0,
|
||||
-sin(x), 0, cos(x);
|
||||
|
||||
return R;
|
||||
}
|
||||
inline mat3x3 Rz(float x)
|
||||
{
|
||||
mat3x3 R = mat3x3::Zero();
|
||||
|
||||
R << cos(x), -sin(x), 0,
|
||||
sin(x), cos(x), 0,
|
||||
0, 0, 1;
|
||||
|
||||
return R;
|
||||
}
|
||||
|
||||
// Rewrite directly the analytical expression
|
||||
inline mat3x3 EulXYZ2Rot(const vec3 &eul)
|
||||
{
|
||||
// XYZ intrinsic euler angles, converts from base to global
|
||||
return Rx(eul(0)) * Ry(eul(1)) * Rz(eul(2));
|
||||
}
|
||||
|
||||
inline mat3x3 quat2rot(const vec4 &q) {
|
||||
// pinocchio convention of {x, y, z, w}
|
||||
float q0 = q(3);
|
||||
vec3 qv = q.block<3, 1>(0, 0);
|
||||
mat3x3 Rot = (2 * q0 * q0 - 1) * mat3x3::Identity() + 2 * q0 * skew_symm(qv) + 2 * qv * qv.transpose();
|
||||
|
||||
return Rot;
|
||||
}
|
||||
|
||||
inline vec4 zeta(const vec3 &v) {
|
||||
float v_norm = v.norm();
|
||||
vec4 q = vec4(0, 0, 0, 1);
|
||||
q(3) = cos(v_norm / 2);
|
||||
if (v_norm == 0) {
|
||||
q.block<3, 1>(0, 0) = vec3::Zero(3);
|
||||
} else {
|
||||
q.block<3, 1>(0, 0) = v * sin(v_norm / 2) / v_norm;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
|
||||
inline vec4 Rot2AxisAngle(const mat3x3 &R) {
|
||||
vec4 axang = vec4::Zero();
|
||||
|
||||
double tmp = (R.trace() - 1) / 2;
|
||||
axang.block<3, 1>(1, 0) = vec3(0, 0, 0);
|
||||
if (tmp >= 1.) {
|
||||
axang(0) = 0;
|
||||
} else if (tmp <= -1.) {
|
||||
axang(0) = M_PI;
|
||||
} else {
|
||||
axang(0) = std::acos(tmp);
|
||||
axang.block<3, 1>(1, 0) = (0.5 / sin(axang(0))) * vec3(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
|
||||
}
|
||||
|
||||
return axang;
|
||||
}
|
||||
|
||||
inline vec3 Rot2Phi(const mat3x3 &R) {
|
||||
vec4 axang = Rot2AxisAngle(R);
|
||||
|
||||
return axang(0) * axang.block<3, 1>(1, 0);
|
||||
}
|
||||
|
||||
inline vec4 EulXYZ2quat(const vec3& eul) {
|
||||
return zeta(Rot2Phi(EulXYZ2Rot(eul)));
|
||||
}
|
||||
|
||||
inline vec3 Rot2EulXYZ(const mat3x3& R) {
|
||||
// reference: https://www.geometrictools.com/Documentation/EulerAngles.pdf : Page 4-5
|
||||
vec3 eul(0, 0, 0);
|
||||
if (R(0, 2) < 1) {
|
||||
if (R(0, 2) > -1) {
|
||||
eul(0) = atan2(-R(1, 2), R(2, 2));
|
||||
eul(1) = asin(R(0, 2));
|
||||
eul(2) = atan2(-R(0, 1), R(0, 0));
|
||||
} else {
|
||||
eul(0) = -atan2(R(1, 0), R(1, 1));
|
||||
eul(1) = -M_PI / 2;
|
||||
eul(2) = 0;
|
||||
}
|
||||
} else {
|
||||
eul(0) = atan2(R(1, 0), R(1, 1));
|
||||
eul(1) = M_PI / 2;
|
||||
eul(2) = 0;
|
||||
}
|
||||
|
||||
return eul;
|
||||
}
|
||||
inline vec3 matrixLogRot(const mat3x3 &R) {
|
||||
double theta;
|
||||
|
||||
double tmp = (R.trace() - 1) / 2;
|
||||
if (tmp >= 1.) {
|
||||
theta = 0;
|
||||
} else if (tmp <= -1.) {
|
||||
theta = M_PI;
|
||||
} else {
|
||||
theta = std::acos(tmp);
|
||||
}
|
||||
|
||||
vec3 omega = vec3(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
|
||||
|
||||
if (theta > 1e-4) {
|
||||
omega *= theta / (2 * sin(theta));
|
||||
}
|
||||
else {
|
||||
omega /= 2;
|
||||
}
|
||||
|
||||
return omega;
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue