EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added simulation and locomotion files

This commit is contained in:
Nimesh Khandelwal 2024-02-15 18:43:43 -05:00
parent 3de7ba8490
commit 06575e4d9a
218 changed files with 1135099 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
locomotion/src/communication/CMakeLists.txt Normal file
View File

@ -0,0 +1,49 @@
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}
)

45
locomotion/src/communication/include/CommunicationManager.hpp Normal file
View File

@ -0,0 +1,45 @@
#pragma once
#include "memory_types.hpp"
#include <string>
#include <iostream>
enum DATA_ACCESS_MODE {
PLANT_TO_EXECUTOR,
EXECUTOR_TO_PLANT
};
// Single communication manager for quadruped
class CommunicationManager {
public:
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 getSensorData(QuadrupedSensorData& sensor_data);
void getCommandData(QuadrupedCommandData& cmd_data);
void setAccessMode(const DATA_ACCESS_MODE& mode) {
m_mode = mode;
}
protected:
QuadrupedSensorData* m_sensor_data_ptr = NULL;
QuadrupedCommandData* m_joint_command_data_ptr = NULL;
int m_mode = DATA_ACCESS_MODE::PLANT_TO_EXECUTOR;
private:
std::string m_name;
// QuadrupedEstimatoinData m_estimation_data;
// QuadrupedPlannerData m_planner_data;
};

79
locomotion/src/communication/include/QuadROSComm.hpp Normal file
View File

@ -0,0 +1,79 @@
#pragma once
#include "CommunicationManager.hpp"
#include <functional>
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.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();
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_state_sub;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr m_imu_sub;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr m_odom_sub;
// Publisher(s)
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr m_joint_state_pub;
// Timer
rclcpp::TimerBase::SharedPtr m_timer;
// Subscription callbacks
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;
sensor_msgs::msg::Imu imu_data;
nav_msgs::msg::Odometry odom_data;
};

29
locomotion/src/communication/include/SHM.hpp Normal file
View File

@ -0,0 +1,29 @@
#pragma once
#include "CommunicationManager.hpp"
#include <sys/ipc.h>
#include <sys/shm.h>
// Randomy chosen key enums
enum SHM_KEYS {
SENSOR_DATA = 123,
COMMAND_DATA = 549
};
class SHM : public CommunicationManager {
public:
SHM();
SHM(const DATA_ACCESS_MODE& mode);
SHM(const std::string& name, const DATA_ACCESS_MODE& mode);
~SHM();
// void writeSensorData(const QuadrupedSensorData& sensor_data) override;
// void writeCommandData(const QuadrupedCommandData& cmd_data) override;
private:
std::string m_name;
void InitClass();
void SetupMemory();
void* getSHMPointer(const key_t&, const size_t&);
};

53
locomotion/src/communication/src/CommunicationManager.cpp Normal file
View File

@ -0,0 +1,53 @@
#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);
}

172
locomotion/src/communication/src/QuadROSComm.cpp Normal file
View File

@ -0,0 +1,172 @@
#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());
}

64
locomotion/src/communication/src/SHM.cpp Normal file
View File

@ -0,0 +1,64 @@
#include "SHM.hpp"
SHM::SHM() : m_name("svan"), CommunicationManager(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) {
InitClass();
}
SHM::SHM(const DATA_ACCESS_MODE& mode) : m_name("svan"), CommunicationManager(mode) {
InitClass();
}
SHM::SHM(const std::string& name, const DATA_ACCESS_MODE& mode) : m_name(name), CommunicationManager(name, mode) {
InitClass();
}
SHM::~SHM() {
shmdt(m_joint_command_data_ptr);
shmdt(m_sensor_data_ptr);
}
void* SHM::getSHMPointer(const key_t& key, const size_t& totalSize) {
// get the id of the shared memory block with the given key
int shmid = shmget(key, totalSize, 0666|IPC_CREAT);
// std::cout << "shmid: " << shmid << "\n";
// Raise error if the shared memory could not be created/assigned
if (shmid == -1) {
perror("shmget");
return (void*)(01);
}
// get pointer to the shared memory block
void *sharedMemory = shmat(shmid, nullptr, 0);
// Raise error if the returned pointer is not valid
if (sharedMemory == reinterpret_cast<void*>(01)) {
perror("shmat");
return (void*)(01);
}
return sharedMemory;
}
void SHM::SetupMemory() {
key_t key;
size_t totalSize;
void* sharedMemory;
key = SHM_KEYS::COMMAND_DATA;
totalSize = sizeof(QuadrupedCommandData);
// std::cout << "Command data size: " << totalSize << "\n";
sharedMemory = getSHMPointer(key, totalSize);
m_joint_command_data_ptr = static_cast<QuadrupedCommandData*>(sharedMemory);
key = SHM_KEYS::SENSOR_DATA;
totalSize = sizeof(QuadrupedSensorData);
// std::cout << "Sensor data size: " << totalSize << "\n";
sharedMemory = getSHMPointer(key, totalSize);
m_sensor_data_ptr = static_cast<QuadrupedSensorData*>(sharedMemory);
// std::cout << "Memory setup completed...\n";
}
void SHM::InitClass() {
SetupMemory();
}

19
locomotion/src/communication/tests/test_ros_comm.cpp Normal file
View File

@ -0,0 +1,19 @@
#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;
}

77
locomotion/src/robot_simulation/include/MJSim.hpp Normal file
View File

@ -0,0 +1,77 @@
#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;
};

248
locomotion/src/robot_simulation/include/mjsim_helper.hpp Normal file
View File

@ -0,0 +1,248 @@
#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
}

249
locomotion/src/robot_simulation/include/mjsim_helper_ros.hpp Normal file
View File

@ -0,0 +1,249 @@
#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 "QuadROSComm.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);
std::shared_ptr<CommROSP2E> comm_data_ptr;
#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
}

325
locomotion/src/robot_simulation/include/quad_simulator.hpp Normal file
View File

@ -0,0 +1,325 @@
// 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

325
locomotion/src/robot_simulation/include/simulate.h Normal file
View File

@ -0,0 +1,325 @@
// 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

200
locomotion/src/robot_simulation/src/MJSim.cpp Normal file
View File

@ -0,0 +1,200 @@
#include "MJSim.hpp"
MJSim::MJSim() : m_name("go2") {
// Load and compile model
init_sim();
}
MJSim::~MJSim() {
// free visualization storage
mjv_freeScene(&scn);
mjr_freeContext(&con);
// free MuJoCo model and data, deactivate
mj_deleteData(d);
mj_deleteModel(m);
// terminate GLFW (crashes with Linux NVidia drivers)
#if defined(__APPLE__) || defined(_WIN32)
glfwTerminate();
#endif
}
void MJSim::CustomController(const mjModel* model, mjData* data) {
// controller with sensor readings
if (previous_time == 0)
{
previous_time = data->time;
return;
}
if (data->time - last_update > 1.0/ctrl_update_freq)
{
// Get the velocity of joint directly from MuJoCo?
mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time);
for (int i = 0; i < 12; ++i) {
float dtheta = joint_command_data.q(i) - sensor_data.qd(i);
float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i);
data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d;
}
last_update = data->time;
position_history = data->sensordata[0];
previous_time = data->time;
}
}
void MJSim::Run() {
// use the first while condition if you want to simulate for a period.
// while( !glfwWindowShouldClose(window) and d->time-timezero < 1.5)
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.
mjtNum simstart = d->time;
while( d->time - simstart < 1.0/60.0 ) {
UpdateSensorData();
mj_step(m, d);
}
// 15 ms is a little smaller than 60 Hz.
std::this_thread::sleep_for(std::chrono::milliseconds(15));
// 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();
}
}
void MJSim::init_sim() {
std::string model_file = "../../robots/" + m_name + "_description/mujoco/" + m_name + ".xml";
// 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);
// 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);
// install control callback
mjcb_control = this->CustomController;
// Initialize DOFs
// d->qpos[0] = 1.57;
// run main loop, target real-time simulation and 60 fps rendering
mjtNum timezero = d->time;
double_t update_rate = 0.01;
// making sure the first time step updates the ctrl previous_time
last_update = timezero-1.0/ctrl_update_freq;
}
// keyboard callback
void MJSim::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 MJSim::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 MJSim::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 MJSim::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);
}
void MJSim::UpdateSensorData() {
for (int i = 0; i < 12; ++i) {
sensor_data.q(i) = d -> sensordata[i];
}
}

88
locomotion/src/robot_simulation/src/main.cpp Normal file
View File

@ -0,0 +1,88 @@
#include "mjsim_helper.hpp"
#include "utils.hpp"
void UpdateSensorData() {
for (int i = 0; i < 12; ++i) {
sensor_data.q(i) = d -> sensordata[i];
sensor_data.qd(i) = d -> sensordata[12 + i];
sensor_data.tau(i) = d -> sensordata[24 + i];
}
for (int i = 0; i < 3; ++i) {
sensor_data.a_B(i) = d -> sensordata[36 + i];
sensor_data.w_B(i) = d -> sensordata[39 + i];
}
for (int i = 0; i < 4; ++i) {
sensor_data.quat(i) = d -> sensordata[(42 + ((i + 1) % 4))];
}
mat3x3 Rot = pinocchio::quat2rot(sensor_data.quat);
sensor_data.a_W = Rot * sensor_data.a_B;
sensor_data.w_W = Rot * sensor_data.w_B;
// std::cout << "Quat: " << sensor_data.quat.transpose() << "\n";
comm_data.writeSensorData(sensor_data);
}
void CustomController(const mjModel* model, mjData* data) {
// get joint command
comm_data.getCommandData(joint_command_data);
// controller with sensor readings
if (previous_time == 0) {
previous_time = data->time;
return;
}
// std::cout << "cmd js: " << joint_command_data.q.transpose() << "\n";
float Kp = 300;
float Kd = 8;
if (data->time - last_update > 1.0/ctrl_update_freq) {
// Get the velocity of joint directly from MuJoCo?
// mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time);
for (int i = 0; i < 12; ++i) {
float dtheta = joint_command_data.q(i) - sensor_data.q(i);
float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i);
// data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d;
data->ctrl[i] = joint_command_data.tau(i) + Kp * dtheta + Kd * dtheta_d;
}
last_update = data->time;
position_history = data->sensordata[0];
previous_time = data->time;
}
}
int main(int argc, char** argv) {
if (argc != 2) {
std::cerr << "Usage: ./quad_sim [robot_name]\n";
return 1;
}
std::string m_name = std::string(argv[1]);
// std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/" + m_name + ".xml";
std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/scene.xml";
init_model(model_file);
joint_command_data.q << 0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 0.0, 0.67, -1.3, -0.0, 0.67, -1.3;
// install control callback
mjcb_control = CustomController;
timezero = d->time;
double_t update_rate = 0.001;
// making sure the first time step updates the ctrl previous_time
last_update = timezero-1.0/ctrl_update_freq;
// comm_data.setSensorDataPtr(&sensor_data);
// comm_data.setCommandDataPtr(&joint_command_data);
// Run the simulation for 10 seconds with visualization enabled with 60 fps
run_simulation(10000, true, 60);
shutdown_simulation();
return 0;
}

107
locomotion/src/robot_simulation/src/main_ros.cpp Normal file
View File

@ -0,0 +1,107 @@
#include "mjsim_helper_ros.hpp"
#include "utils.hpp"
#include <csignal>
// Signal handler to gracefully handle Ctrl+C
void signalHandler(int signum) {
std::cout << "Interrupt signal (" << signum << ") received.\n";
// Add cleanup or exit logic as needed
exit(signum);
}
void UpdateSensorData() {
for (int i = 0; i < 12; ++i) {
sensor_data.q(i) = d -> sensordata[i];
sensor_data.qd(i) = d -> sensordata[12 + i];
sensor_data.tau(i) = d -> sensordata[24 + i];
}
for (int i = 0; i < 3; ++i) {
sensor_data.a_B(i) = d -> sensordata[36 + i];
sensor_data.w_B(i) = d -> sensordata[39 + i];
}
for (int i = 0; i < 4; ++i) {
sensor_data.quat(i) = d -> sensordata[(42 + ((i + 1) % 4))];
}
mat3x3 Rot = pinocchio::quat2rot(sensor_data.quat);
sensor_data.a_W = Rot * sensor_data.a_B;
sensor_data.w_W = Rot * sensor_data.w_B;
// std::cout << "Quat: " << sensor_data.quat.transpose() << "\n";
comm_data_ptr -> writeSensorData(sensor_data);
}
void CustomController(const mjModel* model, mjData* data) {
// get joint command
comm_data_ptr->getCommandData(joint_command_data);
// controller with sensor readings
if (previous_time == 0) {
previous_time = data->time;
return;
}
// std::cout << "cmd js: " << joint_command_data.q.transpose() << "\n";
float Kp = 300;
float Kd = 8;
if (data->time - last_update > 1.0/ctrl_update_freq) {
// Get the velocity of joint directly from MuJoCo?
// mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time);
for (int i = 0; i < 12; ++i) {
float dtheta = joint_command_data.q(i) - sensor_data.q(i);
float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i);
// data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d;
data->ctrl[i] = joint_command_data.tau(i) + Kp * dtheta + Kd * dtheta_d;
}
last_update = data->time;
position_history = data->sensordata[0];
previous_time = data->time;
}
}
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
if (argc != 2) {
std::cerr << "Usage: ./quad_sim [robot_name]\n";
return 1;
}
std::string m_name = std::string(argv[1]);
// std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/" + m_name + ".xml";
std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/scene.xml";
init_model(model_file);
joint_command_data.q << 0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 0.0, 0.67, -1.3, -0.0, 0.67, -1.3;
// install control callback
mjcb_control = CustomController;
timezero = d->time;
double_t update_rate = 0.001;
// making sure the first time step updates the ctrl previous_time
last_update = timezero-1.0/ctrl_update_freq;
// Register signal handler for Ctrl+C
signal(SIGINT, signalHandler);
comm_data_ptr = std::make_shared<CommROSP2E>(m_name);
comm_data_ptr->setSensorDataPtr(&sensor_data);
comm_data_ptr->setCommandDataPtr(&joint_command_data);
std::thread comm_thread;
comm_thread = std::thread(&CommROSP2E::Run, comm_data_ptr);
comm_thread.detach();
// Run the simulation for 10 seconds with visualization enabled with 60 fps
run_simulation(10000, true, 60);
shutdown_simulation();
return 0;
}

572
locomotion/src/robot_simulation/src/quad_simulate.cc Normal file
View File

@ -0,0 +1,572 @@
// 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.
#include <chrono>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <memory>
#include <mutex>
#include <new>
#include <string>
#include <thread>
#include <mujoco/mujoco.h>
#include "glfw_adapter.h"
#include "simulate.h"
#include "array_safety.h"
#define MUJOCO_PLUGIN_DIR "mujoco_plugin"
extern "C" {
#if defined(_WIN32) || defined(__CYGWIN__)
#include <windows.h>
#else
#if defined(__APPLE__)
#include <mach-o/dyld.h>
#endif
#include <sys/errno.h>
#include <unistd.h>
#endif
}
namespace {
namespace mj = ::mujoco;
namespace mju = ::mujoco::sample_util;
// constants
const double syncMisalign = 0.1; // maximum mis-alignment before re-sync (simulation seconds)
const double simRefreshFraction = 0.7; // fraction of refresh available for simulation
const int kErrorLength = 1024; // load error string length
// model and data
mjModel* m = nullptr;
mjData* d = nullptr;
// control noise variables
mjtNum* ctrlnoise = nullptr;
using Seconds = std::chrono::duration<double>;
//---------------------------------------- plugin handling -----------------------------------------
// return the path to the directory containing the current executable
// used to determine the location of auto-loaded plugin libraries
std::string getExecutableDir() {
#if defined(_WIN32) || defined(__CYGWIN__)
constexpr char kPathSep = '\\';
std::string realpath = [&]() -> std::string {
std::unique_ptr<char[]> realpath(nullptr);
DWORD buf_size = 128;
bool success = false;
while (!success) {
realpath.reset(new(std::nothrow) char[buf_size]);
if (!realpath) {
std::cerr << "cannot allocate memory to store executable path\n";
return "";
}
DWORD written = GetModuleFileNameA(nullptr, realpath.get(), buf_size);
if (written < buf_size) {
success = true;
} else if (written == buf_size) {
// realpath is too small, grow and retry
buf_size *=2;
} else {
std::cerr << "failed to retrieve executable path: " << GetLastError() << "\n";
return "";
}
}
return realpath.get();
}();
#else
constexpr char kPathSep = '/';
#if defined(__APPLE__)
std::unique_ptr<char[]> buf(nullptr);
{
std::uint32_t buf_size = 0;
_NSGetExecutablePath(nullptr, &buf_size);
buf.reset(new char[buf_size]);
if (!buf) {
std::cerr << "cannot allocate memory to store executable path\n";
return "";
}
if (_NSGetExecutablePath(buf.get(), &buf_size)) {
std::cerr << "unexpected error from _NSGetExecutablePath\n";
}
}
const char* path = buf.get();
#else
const char* path = "/proc/self/exe";
#endif
std::string realpath = [&]() -> std::string {
std::unique_ptr<char[]> realpath(nullptr);
std::uint32_t buf_size = 128;
bool success = false;
while (!success) {
realpath.reset(new(std::nothrow) char[buf_size]);
if (!realpath) {
std::cerr << "cannot allocate memory to store executable path\n";
return "";
}
std::size_t written = readlink(path, realpath.get(), buf_size);
if (written < buf_size) {
realpath.get()[written] = '\0';
success = true;
} else if (written == -1) {
if (errno == EINVAL) {
// path is already not a symlink, just use it
return path;
}
std::cerr << "error while resolving executable path: " << strerror(errno) << '\n';
return "";
} else {
// realpath is too small, grow and retry
buf_size *= 2;
}
}
return realpath.get();
}();
#endif
if (realpath.empty()) {
return "";
}
for (std::size_t i = realpath.size() - 1; i > 0; --i) {
if (realpath.c_str()[i] == kPathSep) {
return realpath.substr(0, i);
}
}
// don't scan through the entire file system's root
return "";
}
// scan for libraries in the plugin directory to load additional plugins
void scanPluginLibraries() {
// check and print plugins that are linked directly into the executable
int nplugin = mjp_pluginCount();
if (nplugin) {
std::printf("Built-in plugins:\n");
for (int i = 0; i < nplugin; ++i) {
std::printf(" %s\n", mjp_getPluginAtSlot(i)->name);
}
}
// define platform-specific strings
#if defined(_WIN32) || defined(__CYGWIN__)
const std::string sep = "\\";
#else
const std::string sep = "/";
#endif
// try to open the ${EXECDIR}/plugin directory
// ${EXECDIR} is the directory containing the simulate binary itself
const std::string executable_dir = getExecutableDir();
if (executable_dir.empty()) {
return;
}
const std::string plugin_dir = getExecutableDir() + sep + MUJOCO_PLUGIN_DIR;
mj_loadAllPluginLibraries(
plugin_dir.c_str(), +[](const char* filename, int first, int count) {
std::printf("Plugins registered by library '%s':\n", filename);
for (int i = first; i < first + count; ++i) {
std::printf(" %s\n", mjp_getPluginAtSlot(i)->name);
}
});
}
//------------------------------------------- simulation -------------------------------------------
mjModel* LoadModel(const char* file, mj::Simulate& sim) {
// this copy is needed so that the mju::strlen call below compiles
char filename[mj::Simulate::kMaxFilenameLength];
mju::strcpy_arr(filename, file);
// make sure filename is not empty
if (!filename[0]) {
return nullptr;
}
// load and compile
char loadError[kErrorLength] = "";
mjModel* mnew = 0;
if (mju::strlen_arr(filename)>4 &&
!std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb",
mju::sizeof_arr(filename) - mju::strlen_arr(filename)+4)) {
mnew = mj_loadModel(filename, nullptr);
if (!mnew) {
mju::strcpy_arr(loadError, "could not load binary model");
}
} else {
mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength);
// remove trailing newline character from loadError
if (loadError[0]) {
int error_length = mju::strlen_arr(loadError);
if (loadError[error_length-1] == '\n') {
loadError[error_length-1] = '\0';
}
}
}
mju::strcpy_arr(sim.load_error, loadError);
if (!mnew) {
std::printf("%s\n", loadError);
return nullptr;
}
// compiler warning: print and pause
if (loadError[0]) {
// mj_forward() below will print the warning message
std::printf("Model compiled, but simulation warning (paused):\n %s\n", loadError);
sim.run = 0;
}
return mnew;
}
// simulate in background thread (while rendering in main thread)
void PhysicsLoop(mj::Simulate& sim) {
// cpu-sim syncronization point
std::chrono::time_point<mj::Simulate::Clock> syncCPU;
mjtNum syncSim = 0;
// run until asked to exit
while (!sim.exitrequest.load()) {
if (sim.droploadrequest.load()) {
sim.LoadMessage(sim.dropfilename);
mjModel* mnew = LoadModel(sim.dropfilename, sim);
sim.droploadrequest.store(false);
mjData* dnew = nullptr;
if (mnew) dnew = mj_makeData(mnew);
if (dnew) {
sim.Load(mnew, dnew, sim.dropfilename);
// lock the sim mutex
const std::unique_lock<std::recursive_mutex> lock(sim.mtx);
mj_deleteData(d);
mj_deleteModel(m);
m = mnew;
d = dnew;
mj_forward(m, d);
// allocate ctrlnoise
free(ctrlnoise);
ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu);
mju_zero(ctrlnoise, m->nu);
} else {
sim.LoadMessageClear();
}
}
if (sim.uiloadrequest.load()) {
sim.uiloadrequest.fetch_sub(1);
sim.LoadMessage(sim.filename);
mjModel* mnew = LoadModel(sim.filename, sim);
mjData* dnew = nullptr;
if (mnew) dnew = mj_makeData(mnew);
if (dnew) {
sim.Load(mnew, dnew, sim.filename);
// lock the sim mutex
const std::unique_lock<std::recursive_mutex> lock(sim.mtx);
mj_deleteData(d);
mj_deleteModel(m);
m = mnew;
d = dnew;
mj_forward(m, d);
// allocate ctrlnoise
free(ctrlnoise);
ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
mju_zero(ctrlnoise, m->nu);
} else {
sim.LoadMessageClear();
}
}
// sleep for 1 ms or yield, to let main thread run
// yield results in busy wait - which has better timing but kills battery life
if (sim.run && sim.busywait) {
std::this_thread::yield();
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
{
// lock the sim mutex
const std::unique_lock<std::recursive_mutex> lock(sim.mtx);
// run only if model is present
if (m) {
// running
if (sim.run) {
bool stepped = false;
// record cpu time at start of iteration
const auto startCPU = mj::Simulate::Clock::now();
// elapsed CPU and simulation time since last sync
const auto elapsedCPU = startCPU - syncCPU;
double elapsedSim = d->time - syncSim;
// inject noise
if (sim.ctrl_noise_std) {
// convert rate and scale to discrete time (Ornstein–Uhlenbeck)
mjtNum rate = mju_exp(-m->opt.timestep / mju_max(sim.ctrl_noise_rate, mjMINVAL));
mjtNum scale = sim.ctrl_noise_std * mju_sqrt(1-rate*rate);
for (int i=0; i<m->nu; i++) {
// update noise
ctrlnoise[i] = rate * ctrlnoise[i] + scale * mju_standardNormal(nullptr);
// apply noise
d->ctrl[i] = ctrlnoise[i];
}
}
// requested slow-down factor
double slowdown = 100 / sim.percentRealTime[sim.real_time_index];
// misalignment condition: distance from target sim time is bigger than syncmisalign
bool misaligned =
mju_abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign;
// out-of-sync (for any reason): reset sync times, step
if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 ||
misaligned || sim.speed_changed) {
// re-sync
syncCPU = startCPU;
syncSim = d->time;
sim.speed_changed = false;
// run single step, let next iteration deal with timing
mj_step(m, d);
stepped = true;
}
// in-sync: step until ahead of cpu
else {
bool measured = false;
mjtNum prevSim = d->time;
double refreshTime = simRefreshFraction/sim.refresh_rate;
// step while sim lags behind cpu and within refreshTime
while (Seconds((d->time - syncSim)*slowdown) < mj::Simulate::Clock::now() - syncCPU &&
mj::Simulate::Clock::now() - startCPU < Seconds(refreshTime)) {
// measure slowdown before first step
if (!measured && elapsedSim) {
sim.measured_slowdown =
std::chrono::duration<double>(elapsedCPU).count() / elapsedSim;
measured = true;
}
// call mj_step
mj_step(m, d);
stepped = true;
// break if reset
if (d->time < prevSim) {
break;
}
}
}
// save current state to history buffer
if (stepped) {
sim.AddToHistory();
}
}
// paused
else {
// run mj_forward, to update rendering and joint sliders
mj_forward(m, d);
sim.speed_changed = true;
}
}
} // release std::lock_guard<std::mutex>
}
}
} // namespace
//-------------------------------------- physics_thread --------------------------------------------
void PhysicsThread(mj::Simulate* sim, const char* filename) {
// request loadmodel if file given (otherwise drag-and-drop)
if (filename != nullptr) {
sim->LoadMessage(filename);
m = LoadModel(filename, *sim);
if (m) {
// lock the sim mutex
const std::unique_lock<std::recursive_mutex> lock(sim->mtx);
d = mj_makeData(m);
}
if (d) {
sim->Load(m, d, filename);
// lock the sim mutex
const std::unique_lock<std::recursive_mutex> lock(sim->mtx);
mj_forward(m, d);
// allocate ctrlnoise
free(ctrlnoise);
ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
mju_zero(ctrlnoise, m->nu);
} else {
sim->LoadMessageClear();
}
}
PhysicsLoop(*sim);
// delete everything we allocated
free(ctrlnoise);
mj_deleteData(d);
mj_deleteModel(m);
}
//------------------------------------------ main --------------------------------------------------
// machinery for replacing command line error by a macOS dialog box when running under Rosetta
#if defined(__APPLE__) && defined(__AVX__)
extern void DisplayErrorDialogBox(const char* title, const char* msg);
static const char* rosetta_error_msg = nullptr;
__attribute__((used, visibility("default"))) extern "C" void _mj_rosettaError(const char* msg) {
rosetta_error_msg = msg;
}
#endif
void UpdateSensorData() {
for (int i = 0; i < 12; ++i) {
sensor_data.q(i) = d -> sensordata[i];
sensor_data.qd(i) = d -> sensordata[12 + i];
sensor_data.tau(i) = d -> sensordata[24 + i];
}
for (int i = 0; i < 3; ++i) {
sensor_data.a_B(i) = d -> sensordata[36 + i];
sensor_data.w_B(i) = d -> sensordata[39 + i];
}
for (int i = 0; i < 4; ++i) {
sensor_data.quat(i) = d -> sensordata[(42 + ((i + 1) % 4))];
}
mat3x3 Rot = pinocchio::quat2rot(sensor_data.quat);
sensor_data.a_W = Rot * sensor_data.a_B;
sensor_data.w_W = Rot * sensor_data.w_B;
// std::cout << "Quat: " << sensor_data.quat.transpose() << "\n";
comm_data.writeSensorData(sensor_data);
}
void CustomController(const mjModel* model, mjData* data) {
// get joint command
comm_data.getCommandData(joint_command_data);
// controller with sensor readings
if (previous_time == 0) {
previous_time = data->time;
return;
}
// std::cout << "cmd js: " << joint_command_data.q.transpose() << "\n";
float Kp = 300;
float Kd = 8;
if (data->time - last_update > 1.0/ctrl_update_freq) {
// Get the velocity of joint directly from MuJoCo?
// mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time);
for (int i = 0; i < 12; ++i) {
float dtheta = joint_command_data.q(i) - sensor_data.q(i);
float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i);
// data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d;
data->ctrl[i] = joint_command_data.tau(i) + Kp * dtheta + Kd * dtheta_d;
}
last_update = data->time;
position_history = data->sensordata[0];
previous_time = data->time;
}
}
// run event loop
int main(int argc, char** argv) {
// display an error if running on macOS under Rosetta 2
#if defined(__APPLE__) && defined(__AVX__)
if (rosetta_error_msg) {
DisplayErrorDialogBox("Rosetta 2 is not supported", rosetta_error_msg);
std::exit(1);
}
#endif
// print version, check compatibility
std::printf("MuJoCo version %s\n", mj_versionString());
if (mjVERSION_HEADER!=mj_version()) {
mju_error("Headers and library have different versions");
}
// scan for libraries in the plugin directory to load additional plugins
scanPluginLibraries();
mjvCamera cam;
mjv_defaultCamera(&cam);
mjvOption opt;
mjv_defaultOption(&opt);
mjvPerturb pert;
mjv_defaultPerturb(&pert);
// simulate object encapsulates the UI
auto sim = std::make_unique<mj::Simulate>(
std::make_unique<mj::GlfwAdapter>(),
&cam, &opt, &pert, /* is_passive = */ false
);
const char* filename = nullptr;
if (argc > 1) {
filename = argv[1];
}
// start physics thread
std::thread physicsthreadhandle(&PhysicsThread, sim.get(), filename);
// start simulation UI loop (blocking call)
sim->RenderLoop();
physicsthreadhandle.join();
return 0;
}

2783
locomotion/src/robot_simulation/src/simulate.cc Normal file
View File

File diff suppressed because it is too large Load Diff

15
locomotion/src/robot_simulation/src/test_shm.cpp Normal file
View File

@ -0,0 +1,15 @@
#include "Communication/SHM.hpp"
int main(int argc, char** argv) {
SHM shm_data(DATA_ACCESS_MODE::EXECUTOR_TO_PLANT);
QuadrupedSensorData sensor_data;
int i = 0;
while (i < 500000) {
shm_data.getSensorData(sensor_data);
std::cout << "Quat: " << sensor_data.quat.transpose() << "\n";
i++;
}
return 0;
}

11
locomotion/src/robots/.gitignore vendored Normal file
View File

@ -0,0 +1,11 @@
CMakeLists.txt.user
CMakeCache.txt
CMakeFiles
CMakeScripts
Testing
Makefile
cmake_install.cmake
install_manifest.txt
compile_commands.json
CTestTestfile.cmake
_deps

21
locomotion/src/robots/LICENSE Normal file
View File

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2024 Nimesh Khandelwal
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

BIN
locomotion/src/robots/b1_dae_to_stl_decimation.blend Normal file
View File

Binary file not shown.

BIN
locomotion/src/robots/b1_dae_to_stl_decimation.blend1 Normal file
View File

Binary file not shown.

19
locomotion/src/robots/b1_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,19 @@
cmake_minimum_required(VERSION 2.8.3)
project(b1_description)
find_package(catkin REQUIRED COMPONENTS
genmsg
roscpp
std_msgs
tf
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
# include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)

70
locomotion/src/robots/b1_description/config/robot_control.yaml Normal file
View File

@ -0,0 +1,70 @@
b1_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

25
locomotion/src/robots/b1_description/launch/b1_rviz.launch Normal file
View File

@ -0,0 +1,25 @@
<launch>
<arg name="user_debug" default="false"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find b1_description)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- for higher robot_state_publisher average rate-->
<!-- <param name="rate" value="1000"/> -->
<!-- send fake joint values -->
<!--<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node>-->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find b1_description)/launch/check_joint.rviz"/>
</launch>

315
locomotion/src/robots/b1_description/launch/check_joint.rviz Normal file
View File

@ -0,0 +1,315 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1
- /TF1/Frames1
Splitter Ratio: 0.5
Tree Height: 530
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.5
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh_rotor:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
trunk:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: false
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: false
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
Enabled: true
Global Options:
Background Color: 238; 238; 238
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.7572907209396362
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.18471483886241913
Y: -0.16858524084091187
Z: -0.1451287716627121
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.609795868396759
Target Frame: <Fixed Frame>
Yaw: 4.128592014312744
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 759
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001b90000029dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000042800fffffffb0000000800540069006d00650100000000000004500000000000000000000003ae0000029d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1389
X: 472
Y: 122

63
locomotion/src/robots/b1_description/meshes/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
locomotion/src/robots/b1_description/meshes/calf.stl Normal file
View File

Binary file not shown.

63
locomotion/src/robots/b1_description/meshes/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
locomotion/src/robots/b1_description/meshes/hip.stl Normal file
View File

Binary file not shown.

63
locomotion/src/robots/b1_description/meshes/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
locomotion/src/robots/b1_description/meshes/thigh.stl Normal file
View File

Binary file not shown.

63
locomotion/src/robots/b1_description/meshes/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
locomotion/src/robots/b1_description/meshes/thigh_mirror.stl Normal file
View File

Binary file not shown.

63
locomotion/src/robots/b1_description/meshes/trunk.dae Normal file
View File

File diff suppressed because one or more lines are too long

BIN
locomotion/src/robots/b1_description/meshes/trunk.stl Normal file
View File

Binary file not shown.

2
locomotion/src/robots/b1_description/mujoco/assets/calf.mtl Normal file
View File

@ -0,0 +1,2 @@
# Blender 4.0.2 MTL File: 'None'
# www.blender.org

8525
locomotion/src/robots/b1_description/mujoco/assets/calf.obj Normal file
View File

File diff suppressed because it is too large Load Diff

2
locomotion/src/robots/b1_description/mujoco/assets/hip.mtl Normal file
View File

@ -0,0 +1,2 @@
# Blender 4.0.2 MTL File: 'None'
# www.blender.org

10421
locomotion/src/robots/b1_description/mujoco/assets/hip.obj Normal file
View File

File diff suppressed because it is too large Load Diff

2
locomotion/src/robots/b1_description/mujoco/assets/thigh.mtl Normal file
View File

@ -0,0 +1,2 @@
# Blender 4.0.2 MTL File: 'None'
# www.blender.org

12646
locomotion/src/robots/b1_description/mujoco/assets/thigh.obj Normal file
View File

File diff suppressed because it is too large Load Diff

35146
locomotion/src/robots/b1_description/mujoco/assets/thigh/thigh.obj Normal file
View File

File diff suppressed because it is too large Load Diff

2
locomotion/src/robots/b1_description/mujoco/assets/thigh_mirror.mtl Normal file
View File

@ -0,0 +1,2 @@
# Blender 4.0.2 MTL File: 'None'
# www.blender.org

12728
locomotion/src/robots/b1_description/mujoco/assets/thigh_mirror.obj Normal file
View File

File diff suppressed because it is too large Load Diff

2
locomotion/src/robots/b1_description/mujoco/assets/trunk.mtl Normal file
View File

@ -0,0 +1,2 @@
# Blender 4.0.2 MTL File: 'None'
# www.blender.org

60814
locomotion/src/robots/b1_description/mujoco/assets/trunk.obj Normal file
View File

File diff suppressed because it is too large Load Diff

230
locomotion/src/robots/b1_description/mujoco/b1.xml Normal file
View File

@ -0,0 +1,230 @@
<mujoco model="b1_description">
<compiler angle="radian" meshdir="../meshes/" autolimits="true"/>
<option cone="elliptic" impratio="100"/>
<default>
<default class="b1">
<geom friction="0.6" margin="0.001" condim="1"/>
<joint axis="0 1 0" damping="2" armature="0.01" frictionloss="0.2"/>
<motor ctrlrange="-230.7 230.7"/>
<default class="abduction">
<joint axis="1 0 0" range="-3.0472 3.0472"/>
</default>
<default class="hip">
<default class="front_hip">
<joint range="-3.5708 6.4907"/>
</default>
<default class="back_hip">
<joint range="-1.5236 8.5379"/>
</default>
</default>
<default class="knee">
<joint range="-4.7227 -1.83776"/>
<motor ctrlrange="-450.43 450.43"/>
</default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2"/>
</default>
<default class="collision">
<geom group="3"/>
<default class="foot">
<geom size="0.022" pos="-0.002 0 -0.213" priority="1" solimp="0.015 1 0.031" condim="6"
friction="0.8 0.02 0.01"/>
</default>
</default>
</default>
</default>
<asset>
<material name="metal" rgba=".9 .95 .95 1"/>
<material name="black" rgba="0 0 0 1"/>
<material name="white" rgba="1 1 1 1"/>
<material name="gray" rgba="0.671705 0.692426 0.774270 1"/>
<mesh name="trunk" file="trunk.stl"/>
<mesh name="hip" file="hip.stl"/>
<mesh name="thigh_mirror" file="thigh_mirror.stl"/>
<mesh name="calf" file="calf.stl"/>
<mesh name="thigh" file="thigh.stl"/>
</asset>
<worldbody>
<body name="base" pos="0 0 0.85" childclass="b1">
<inertial pos="0.00870935 0.0021737 0.00291991" quat="0.00194753 0.691276 0.00346027 0.72258" mass="25.797" diaginertia="0.820612 0.7877 0.187707"/>
<joint name="floating_base" type="free"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="trunk"/>
<geom size="0.3235 0.15 0.075" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" density="0" rgba="0.8 0 0 1"/>
<geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 1"/>
<geom size="0.05 0.01" pos="0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<site name="imu" pos="-0.02557 0 0.04232"/>
<body name="FR_hip" pos="0.3455 -0.072 0">
<inertial pos="-0.018016 -0.00971213 9.67456e-05" quat="0.514022 0.512801 0.485949 0.486488" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 -0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FR_thigh" pos="0 -0.12675 0">
<inertial pos="-0.000220117 0.0301731 -0.0507383" quat="0.736596 -0.0378368 0.076521 0.670924" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh_mirror"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FR_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
<body name="FL_hip" pos="0.3455 0.072 0">
<inertial pos="-0.018016 0.00971213 9.67456e-05" quat="0.486488 0.485949 0.512801 0.514022" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FL_thigh" pos="0 0.12675 0">
<inertial pos="-0.000220117 -0.0301731 -0.0507383" quat="0.670924 0.076521 -0.0378368 0.736596" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FL_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
<body name="RR_hip" pos="-0.3455 -0.072 0">
<inertial pos="0.018016 -0.00971213 9.67456e-05" quat="0.485949 0.486488 0.514022 0.512801" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 -0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RR_thigh" pos="0 -0.12675 0">
<inertial pos="-0.000220117 0.0301731 -0.0507383" quat="0.736596 -0.0378368 0.076521 0.670924" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh_mirror"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RR_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
<body name="RL_hip" pos="-0.3455 0.072 0">
<inertial pos="0.018016 0.00971213 9.67456e-05" quat="0.512801 0.514022 0.486488 0.485949" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RL_thigh" pos="0 0.12675 0">
<inertial pos="-0.000220117 -0.0301731 -0.0507383" quat="0.670924 0.076521 -0.0378368 0.736596" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RL_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor class="abduction" name="FL_hip" joint="FL_hip_joint"/>
<motor class="hip" name="FL_thigh" joint="FL_thigh_joint"/>
<motor class="knee" name="FL_calf" joint="FL_calf_joint"/>
<motor class="abduction" name="FR_hip" joint="FR_hip_joint"/>
<motor class="hip" name="FR_thigh" joint="FR_thigh_joint"/>
<motor class="knee" name="FR_calf" joint="FR_calf_joint"/>
<motor class="abduction" name="RL_hip" joint="RL_hip_joint"/>
<motor class="hip" name="RL_thigh" joint="RL_thigh_joint"/>
<motor class="knee" name="RL_calf" joint="RL_calf_joint"/>
<motor class="abduction" name="RR_hip" joint="RR_hip_joint"/>
<motor class="hip" name="RR_thigh" joint="RR_thigh_joint"/>
<motor class="knee" name="RR_calf" joint="RR_calf_joint"/>
</actuator>
<sensor>
<jointpos name="FL_hip_position_sensor" joint="FL_hip_joint" noise="0.001"/>
<jointpos name="FL_thigh_position_sensor" joint="FL_thigh_joint" noise="0.001"/>
<jointpos name="FL_calf_position_sensor" joint="FL_calf_joint" noise="0.001"/>
<jointpos name="FR_hip_position_sensor" joint="FR_hip_joint" noise="0.001"/>
<jointpos name="FR_thigh_position_sensor" joint="FR_thigh_joint" noise="0.001"/>
<jointpos name="FR_calf_position_sensor" joint="FR_calf_joint" noise="0.001"/>
<jointpos name="RL_hip_position_sensor" joint="RL_hip_joint" noise="0.001"/>
<jointpos name="RL_thigh_position_sensor" joint="RL_thigh_joint" noise="0.001"/>
<jointpos name="RL_calf_position_sensor" joint="RL_calf_joint" noise="0.001"/>
<jointpos name="RR_hip_position_sensor" joint="RR_hip_joint" noise="0.001"/>
<jointpos name="RR_thigh_position_sensor" joint="RR_thigh_joint" noise="0.001"/>
<jointpos name="RR_calf_position_sensor" joint="RR_calf_joint" noise="0.001"/>
<jointvel name="FL_hip_velocity_sensor" joint="FL_hip_joint" noise="0.001"/>
<jointvel name="FL_thigh_velocity_sensor" joint="FL_thigh_joint" noise="0.001"/>
<jointvel name="FL_calf_velocity_sensor" joint="FL_calf_joint" noise="0.001"/>
<jointvel name="FR_hip_velocity_sensor" joint="FR_hip_joint" noise="0.001"/>
<jointvel name="FR_thigh_velocity_sensor" joint="FR_thigh_joint" noise="0.001"/>
<jointvel name="FR_calf_velocity_sensor" joint="FR_calf_joint" noise="0.001"/>
<jointvel name="RL_hip_velocity_sensor" joint="RL_hip_joint" noise="0.001"/>
<jointvel name="RL_thigh_velocity_sensor" joint="RL_thigh_joint" noise="0.001"/>
<jointvel name="RL_calf_velocity_sensor" joint="RL_calf_joint" noise="0.001"/>
<jointvel name="RR_hip_velocity_sensor" joint="RR_hip_joint" noise="0.001"/>
<jointvel name="RR_thigh_velocity_sensor" joint="RR_thigh_joint" noise="0.001"/>
<jointvel name="RR_calf_velocity_sensor" joint="RR_calf_joint" noise="0.001"/>
<jointactuatorfrc name="FL_hip_force_sensor" joint="FL_hip_joint" noise="0.001"/>
<jointactuatorfrc name="FL_thigh_force_sensor" joint="FL_thigh_joint" noise="0.001"/>
<jointactuatorfrc name="FL_calf_force_sensor" joint="FL_calf_joint" noise="0.001"/>
<jointactuatorfrc name="FR_hip_force_sensor" joint="FR_hip_joint" noise="0.001"/>
<jointactuatorfrc name="FR_thigh_force_sensor" joint="FR_thigh_joint" noise="0.001"/>
<jointactuatorfrc name="FR_calf_force_sensor" joint="FR_calf_joint" noise="0.001"/>
<jointactuatorfrc name="RL_hip_force_sensor" joint="RL_hip_joint" noise="0.001"/>
<jointactuatorfrc name="RL_thigh_force_sensor" joint="RL_thigh_joint" noise="0.001"/>
<jointactuatorfrc name="RL_calf_force_sensor" joint="RL_calf_joint" noise="0.001"/>
<jointactuatorfrc name="RR_hip_force_sensor" joint="RR_hip_joint" noise="0.001"/>
<jointactuatorfrc name="RR_thigh_force_sensor" joint="RR_thigh_joint" noise="0.001"/>
<jointactuatorfrc name="RR_calf_force_sensor" joint="RR_calf_joint" noise="0.001"/>
<accelerometer name="linear_acceleration" site="imu" noise="0.001"/>
<gyro name="angular_velocity" site="imu" noise="0.001"/>
<framequat name="imu_orientation" objtype="site" objname="imu" noise="0.001"/>
</sensor>
<keyframe>
<key name="home" qpos="0 0 0.45 1 0 0 0 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"
ctrl="0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"/>
</keyframe>
</mujoco>

120
locomotion/src/robots/b1_description/mujoco/b1_mjgen.xml Normal file
View File

@ -0,0 +1,120 @@
<mujoco model="b1_description">
<compiler angle="radian" meshdir="../meshes/"/>
<asset>
<mesh name="trunk" file="trunk.stl"/>
<mesh name="hip" file="hip.stl"/>
<mesh name="thigh_mirror" file="thigh_mirror.stl"/>
<mesh name="calf" file="calf.stl"/>
<mesh name="thigh" file="thigh.stl"/>
</asset>
<worldbody>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="trunk"/>
<geom size="0.3235 0.15 0.075" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.0005 0.0005 0.0005" type="box" contype="0" conaffinity="0" group="1" density="0" rgba="0.8 0 0 1"/>
<geom size="0.0005 0.0005 0.0005" type="box" rgba="0.8 0 0 1"/>
<geom size="0.05 0.01" pos="0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 -0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="-0.1955 0.072 0" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FR_hip" pos="0.3455 -0.072 0">
<inertial pos="-0.018016 -0.00971213 9.67456e-05" quat="0.514022 0.512801 0.485949 0.486488" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 -0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FR_thigh" pos="0 -0.12675 0">
<inertial pos="-0.000220117 0.0301731 -0.0507383" quat="0.736596 -0.0378368 0.076521 0.670924" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh_mirror"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FR_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
<body name="FL_hip" pos="0.3455 0.072 0">
<inertial pos="-0.018016 0.00971213 9.67456e-05" quat="0.486488 0.485949 0.512801 0.514022" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FL_thigh" pos="0 0.12675 0">
<inertial pos="-0.000220117 -0.0301731 -0.0507383" quat="0.670924 0.076521 -0.0378368 0.736596" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="FL_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
<body name="RR_hip" pos="-0.3455 -0.072 0">
<inertial pos="0.018016 -0.00971213 9.67456e-05" quat="0.485949 0.486488 0.514022 0.512801" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 -0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RR_thigh" pos="0 -0.12675 0">
<inertial pos="-0.000220117 0.0301731 -0.0507383" quat="0.736596 -0.0378368 0.076521 0.670924" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh_mirror"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RR_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
<body name="RL_hip" pos="-0.3455 0.072 0">
<inertial pos="0.018016 0.00971213 9.67456e-05" quat="0.512801 0.514022 0.486488 0.485949" mass="2.366" diaginertia="0.00980696 0.00664332 0.0045362"/>
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" range="-0.75 0.75"/>
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="hip"/>
<geom size="0.09 0.02" pos="0 0.12675 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 0.00935 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RL_thigh" pos="0 0.12675 0">
<inertial pos="-0.000220117 -0.0301731 -0.0507383" quat="0.670924 0.076521 -0.0378368 0.736596" mass="4.2" diaginertia="0.0487177 0.0459334 0.00814802"/>
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" range="-1 3.5"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="thigh"/>
<geom size="0.175 0.025 0.04" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.05 0.01" pos="0 -0.0519 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0 0.8 0 1"/>
<body name="RL_calf" pos="0 0 -0.35">
<inertial pos="0.0049483 0 -0.210919" quat="0.707231 -0.00481122 -0.00483372 0.70695" mass="0.907" diaginertia="0.0162164 0.0160695 0.000406131"/>
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" range="-2.6 -0.6"/>
<geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="1 0.423529 0.0392157 1" mesh="calf"/>
<geom size="0.175 0.02 0.025" pos="0 0 -0.175" quat="0.707107 0 0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
<geom size="0.03" pos="0 0 -0.35" contype="0" conaffinity="0" group="1" density="0" rgba="0 0.8 0 1"/>
<geom size="0.04" pos="0 0 -0.35" rgba="0 0.8 0 1"/>
</body>
</body>
</body>
</worldbody>
</mujoco>

23
locomotion/src/robots/b1_description/mujoco/scene.xml Normal file
View File

@ -0,0 +1,23 @@
<mujoco model="b1 scene">
<include file="b1.xml"/>
<statistic center="0 0 0.1" extent="0.8"/>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="-130" elevation="-20"/>
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
</worldbody>
</mujoco>

14
locomotion/src/robots/b1_description/package.xml Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<package format="2">
<name>b1_description</name>
<version>0.0.0</version>
<description>The b1_description package</description>
<maintainer email="b1@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
</package>

1211
locomotion/src/robots/b1_description/urdf/b1_description.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

964
locomotion/src/robots/b1_description/urdf/b1_description_mj.urdf Normal file
View File

@ -0,0 +1,964 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from robot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="b1_description">
<mujoco>
<compiler
meshdir="../meshes/"
discardvisual="false"
/>
</mujoco>
<link name="world"/>
<joint name="floating_base" type="floating">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.9137254901960784 0.9137254901960784 0.8470588235294118 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.4235294117647059 0.0392156862745098 1.0"/>
</material>
<material name="brown">
<color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<!-- <xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if> -->
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/trunk.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.647 0.3 0.15"/>
</geometry>
</collision>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/trunk.stl" scale="1 1 1"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="0.008987 0.002243 0.003013"/>
<mass value="25"/>
<inertia ixx="0.183142146" ixy="-0.001379002" ixz="-0.027956055" iyy="0.756327752" iyz="0.000193774" izz="0.783777558"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.3455 -0.072 0"/>
<parent link="base"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="91.0035" lower="-0.75" upper="0.75" velocity="19.69"/>
</joint>
<joint name="FR_hip_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.1955 -0.072 0"/>
<parent link="base"/>
<child link="FR_hip_rotor"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.12675 0"/>
<geometry>
<cylinder length="0.04" radius="0.09"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.020298 -0.009758 0.000109"/>
<mass value="2.1"/>
<inertia ixx="0.00406608" ixy="0.000288071" ixz="-4.371e-06" iyy="0.008775259" iyz="-1.811e-06" izz="0.006060348"/>
</inertial>
</link>
<link name="FR_hip_rotor">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.199"/>
<inertia ixx="0.00039249" ixy="0.0" ixz="0.0" iyy="0.000219397" iyz="0.0" izz="0.000219397"/>
</inertial>
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.12675 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="93.33" lower="-1.0" upper="3.5" velocity="23.32"/>
</joint>
<joint name="FR_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 -0.00935 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_rotor"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/thigh_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.05 0.08"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.000235 0.028704 -0.054169"/>
<mass value="3.934"/>
<inertia ixx="0.044459086" ixy="-0.000128738" ixz="-0.002343913" iyy="0.046023457" iyz="-0.006032996" izz="0.008696078"/>
</inertial>
</link>
<link name="FR_thigh_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="140" lower="-2.6" upper="-0.6" velocity="15.55"/>
</joint>
<joint name="FR_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0519 0"/>
<parent link="FR_thigh"/>
<child link="FR_calf_rotor"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.04 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.005237 0.0 -0.202805"/>
<mass value="0.857"/>
<inertia ixx="0.015011003" ixy="5.2e-08" ixz="0.000250042" iyy="0.015159462" iyz="4.61e-07" izz="0.000375749"/>
</inertial>
</link>
<link name="FR_calf_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="3.2000000000000005e-05" ixy="0.0" ixz="0.0" iyy="3.2000000000000005e-05" iyz="0.0" izz="3.2000000000000005e-05"/>
</inertial>
</link>
<transmission name="FR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.3455 0.072 0"/>
<parent link="base"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="91.0035" lower="-0.75" upper="0.75" velocity="19.69"/>
</joint>
<joint name="FL_hip_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.1955 0.072 0"/>
<parent link="base"/>
<child link="FL_hip_rotor"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0.12675 0"/>
<geometry>
<cylinder length="0.04" radius="0.09"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.020298 0.009758 0.000109"/>
<mass value="2.1"/>
<inertia ixx="0.00406608" ixy="-0.000288071" ixz="-4.371e-06" iyy="0.008775259" iyz="1.811e-06" izz="0.006060348"/>
</inertial>
</link>
<link name="FL_hip_rotor">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.199"/>
<inertia ixx="0.00039249" ixy="0.0" ixz="0.0" iyy="0.000219397" iyz="0.0" izz="0.000219397"/>
</inertial>
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.12675 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="93.33" lower="-1.0" upper="3.5" velocity="23.32"/>
</joint>
<joint name="FL_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.00935 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_rotor"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/thigh.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.05 0.08"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.000235 -0.028704 -0.054169"/>
<mass value="3.934"/>
<inertia ixx="0.044459086" ixy="0.000128738" ixz="-0.002343913" iyy="0.046023457" iyz="0.006032996" izz="0.008696078"/>
</inertial>
</link>
<link name="FL_thigh_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="140" lower="-2.6" upper="-0.6" velocity="15.55"/>
</joint>
<joint name="FL_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 -0.0519 0"/>
<parent link="FL_thigh"/>
<child link="FL_calf_rotor"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.04 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.005237 0.0 -0.202805"/>
<mass value="0.857"/>
<inertia ixx="0.015011003" ixy="5.2e-08" ixz="0.000250042" iyy="0.015159462" iyz="4.61e-07" izz="0.000375749"/>
</inertial>
</link>
<link name="FL_calf_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="3.2000000000000005e-05" ixy="0.0" ixz="0.0" iyy="3.2000000000000005e-05" iyz="0.0" izz="3.2000000000000005e-05"/>
</inertial>
</link>
<transmission name="FL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.3455 -0.072 0"/>
<parent link="base"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="91.0035" lower="-0.75" upper="0.75" velocity="19.69"/>
</joint>
<joint name="RR_hip_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="-0.1955 -0.072 0"/>
<parent link="base"/>
<child link="RR_hip_rotor"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.12675 0"/>
<geometry>
<cylinder length="0.04" radius="0.09"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.020298 -0.009758 0.000109"/>
<mass value="2.1"/>
<inertia ixx="0.00406608" ixy="-0.000288071" ixz="4.371e-06" iyy="0.008775259" iyz="-1.811e-06" izz="0.006060348"/>
</inertial>
</link>
<link name="RR_hip_rotor">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.199"/>
<inertia ixx="0.00039249" ixy="0.0" ixz="0.0" iyy="0.000219397" iyz="0.0" izz="0.000219397"/>
</inertial>
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.12675 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="93.33" lower="-1.0" upper="3.5" velocity="23.32"/>
</joint>
<joint name="RR_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 -0.00935 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_rotor"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/thigh_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.05 0.08"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.000235 0.028704 -0.054169"/>
<mass value="3.934"/>
<inertia ixx="0.044459086" ixy="-0.000128738" ixz="-0.002343913" iyy="0.046023457" iyz="-0.006032996" izz="0.008696078"/>
</inertial>
</link>
<link name="RR_thigh_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="140" lower="-2.6" upper="-0.6" velocity="15.55"/>
</joint>
<joint name="RR_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0519 0"/>
<parent link="RR_thigh"/>
<child link="RR_calf_rotor"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.04 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.005237 0.0 -0.202805"/>
<mass value="0.857"/>
<inertia ixx="0.015011003" ixy="5.2e-08" ixz="0.000250042" iyy="0.015159462" iyz="4.61e-07" izz="0.000375749"/>
</inertial>
</link>
<link name="RR_calf_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="3.2000000000000005e-05" ixy="0.0" ixz="0.0" iyy="3.2000000000000005e-05" iyz="0.0" izz="3.2000000000000005e-05"/>
</inertial>
</link>
<transmission name="RR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.3455 0.072 0"/>
<parent link="base"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="91.0035" lower="-0.75" upper="0.75" velocity="19.69"/>
</joint>
<joint name="RL_hip_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="-0.1955 0.072 0"/>
<parent link="base"/>
<child link="RL_hip_rotor"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0.12675 0"/>
<geometry>
<cylinder length="0.04" radius="0.09"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.020298 0.009758 0.000109"/>
<mass value="2.1"/>
<inertia ixx="0.00406608" ixy="0.000288071" ixz="4.371e-06" iyy="0.008775259" iyz="1.811e-06" izz="0.006060348"/>
</inertial>
</link>
<link name="RL_hip_rotor">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.199"/>
<inertia ixx="0.00039249" ixy="0.0" ixz="0.0" iyy="0.000219397" iyz="0.0" izz="0.000219397"/>
</inertial>
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.12675 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="93.33" lower="-1.0" upper="3.5" velocity="23.32"/>
</joint>
<joint name="RL_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.00935 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_rotor"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/thigh.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.05 0.08"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.000235 -0.028704 -0.054169"/>
<mass value="3.934"/>
<inertia ixx="0.044459086" ixy="0.000128738" ixz="-0.002343913" iyy="0.046023457" iyz="0.006032996" izz="0.008696078"/>
</inertial>
</link>
<link name="RL_thigh_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="140" lower="-2.6" upper="-0.6" velocity="15.55"/>
</joint>
<joint name="RL_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 -0.0519 0"/>
<parent link="RL_thigh"/>
<child link="RL_calf_rotor"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.175"/>
<geometry>
<box size="0.35 0.04 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.005237 0.0 -0.202805"/>
<mass value="0.857"/>
<inertia ixx="0.015011003" ixy="5.2e-08" ixz="0.000250042" iyy="0.015159462" iyz="4.61e-07" izz="0.000375749"/>
</inertial>
</link>
<link name="RL_calf_rotor">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<mass value="0.266"/>
<inertia ixx="0.000485657" ixy="0.0" ixz="0.0" iyy="0.00091885" iyz="0.0" izz="0.000485657"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.35"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.04"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<inertia ixx="3.2000000000000005e-05" ixy="0.0" ixz="0.0" iyy="3.2000000000000005e-05" iyz="0.0" izz="3.2000000000000005e-05"/>
</inertial>
</link>
<transmission name="RL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="RL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="RL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
</robot>

160
locomotion/src/robots/b1_description/xacro/const.xacro Normal file
View File

@ -0,0 +1,160 @@
<?xml version="1.0"?>
<robot name="b1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.30"/>
<xacro:property name="trunk_length" value="0.647"/>
<xacro:property name="trunk_height" value="0.15"/>
<xacro:property name="hip_radius" value="0.09"/>
<xacro:property name="hip_length" value="0.04"/>
<xacro:property name="thigh_shoulder_radius" value="0.044"/>
<xacro:property name="thigh_shoulder_length" value="0.08"/>
<xacro:property name="thigh_width" value="0.05"/>
<xacro:property name="thigh_height" value="0.08"/>
<xacro:property name="calf_width" value="0.04"/>
<xacro:property name="calf_height" value="0.05"/>
<xacro:property name="foot_radius" value="0.04"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.12675"/>
<xacro:property name="thigh_length" value="0.35"/>
<xacro:property name="calf_length" value="0.35"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.3455"/>
<xacro:property name="leg_offset_y" value="0.072"/>
<xacro:property name="trunk_offset_z" value="0.01675"/>
<xacro:property name="hip_offset" value="0.12675"/>
<!-- offset of link and rotor locations (left front) -->
<xacro:property name="hip_offset_x" value="0.3455"/>
<xacro:property name="hip_offset_y" value="0.072"/>
<xacro:property name="hip_offset_z" value="0.0"/>
<xacro:property name="hip_rotor_offset_x" value="0.1955"/>
<xacro:property name="hip_rotor_offset_y" value="0.072"/>
<xacro:property name="hip_rotor_offset_z" value="0.0"/>
<xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.12675"/>
<xacro:property name="thigh_offset_z" value="0.0"/>
<xacro:property name="thigh_rotor_offset_x" value="0.0"/>
<xacro:property name="thigh_rotor_offset_y" value="0.00935"/>
<xacro:property name="thigh_rotor_offset_z" value="0.0"/>
<xacro:property name="calf_offset_x" value="0.0"/>
<xacro:property name="calf_offset_y" value="0.0"/>
<xacro:property name="calf_offset_z" value="-0.35"/>
<xacro:property name="calf_rotor_offset_x" value="0.0"/>
<xacro:property name="calf_rotor_offset_y" value="-0.0519"/>
<xacro:property name="calf_rotor_offset_z" value="0.0"/>
<!-- joint limits -->
<xacro:property name="damping" value="0"/>
<xacro:property name="friction" value="0"/>
<xacro:property name="hip_position_max" value="0.75"/>
<xacro:property name="hip_position_min" value="-0.75"/>
<xacro:property name="hip_velocity_max" value="19.69"/>
<xacro:property name="hip_torque_max" value="91.0035"/>
<xacro:property name="thigh_position_max" value="3.5"/>
<xacro:property name="thigh_position_min" value="-1.0"/>
<xacro:property name="thigh_velocity_max" value="23.32"/>
<xacro:property name="thigh_torque_max" value="93.33"/>
<xacro:property name="calf_position_max" value="-0.6"/>
<xacro:property name="calf_position_min" value="-2.6"/>
<xacro:property name="calf_velocity_max" value="15.55"/>
<xacro:property name="calf_torque_max" value="140"/>
<!-- dynamics inertial value -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="25"/>
<xacro:property name="trunk_com_x" value="0.008987"/>
<xacro:property name="trunk_com_y" value="0.002243"/>
<xacro:property name="trunk_com_z" value="0.003013"/>
<xacro:property name="trunk_ixx" value="0.183142146"/>
<xacro:property name="trunk_ixy" value="-0.001379002"/>
<xacro:property name="trunk_ixz" value="-0.027956055"/>
<xacro:property name="trunk_iyy" value="0.756327752"/>
<xacro:property name="trunk_iyz" value="0.000193774"/>
<xacro:property name="trunk_izz" value="0.783777558"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="2.1"/>
<xacro:property name="hip_com_x" value="-0.020298"/>
<xacro:property name="hip_com_y" value="0.009758"/>
<xacro:property name="hip_com_z" value="0.000109"/>
<xacro:property name="hip_ixx" value="0.00406608"/>
<xacro:property name="hip_ixy" value="-0.000288071"/>
<xacro:property name="hip_ixz" value="-0.000004371"/>
<xacro:property name="hip_iyy" value="0.008775259"/>
<xacro:property name="hip_iyz" value="0.000001811"/>
<xacro:property name="hip_izz" value="0.006060348"/>
<xacro:property name="hip_rotor_mass" value="0.199"/>
<xacro:property name="hip_rotor_com_x" value="0.0"/>
<xacro:property name="hip_rotor_com_y" value="0.0"/>
<xacro:property name="hip_rotor_com_z" value="0.0"/>
<xacro:property name="hip_rotor_ixx" value="0.00039249"/>
<xacro:property name="hip_rotor_ixy" value="0.0"/>
<xacro:property name="hip_rotor_ixz" value="0.0"/>
<xacro:property name="hip_rotor_iyy" value="0.000219397"/>
<xacro:property name="hip_rotor_iyz" value="0.0"/>
<xacro:property name="hip_rotor_izz" value="0.000219397"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="3.934"/>
<xacro:property name="thigh_com_x" value="-0.000235"/>
<xacro:property name="thigh_com_y" value="-0.028704"/>
<xacro:property name="thigh_com_z" value="-0.054169"/>
<xacro:property name="thigh_ixx" value="0.044459086"/>
<xacro:property name="thigh_ixy" value="0.000128738"/>
<xacro:property name="thigh_ixz" value="-0.002343913"/>
<xacro:property name="thigh_iyy" value="0.046023457"/>
<xacro:property name="thigh_iyz" value="0.006032996"/>
<xacro:property name="thigh_izz" value="0.008696078"/>
<xacro:property name="thigh_rotor_mass" value="0.266"/>
<xacro:property name="thigh_rotor_com_x" value="0.0"/>
<xacro:property name="thigh_rotor_com_y" value="0.0"/>
<xacro:property name="thigh_rotor_com_z" value="0.0"/>
<xacro:property name="thigh_rotor_ixx" value="0.000485657"/>
<xacro:property name="thigh_rotor_ixy" value="0.0"/>
<xacro:property name="thigh_rotor_ixz" value="0.0"/>
<xacro:property name="thigh_rotor_iyy" value="0.00091885"/>
<xacro:property name="thigh_rotor_iyz" value="0.0"/>
<xacro:property name="thigh_rotor_izz" value="0.000485657"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.857"/>
<xacro:property name="calf_com_x" value="0.005237"/>
<xacro:property name="calf_com_y" value="0.0"/>
<xacro:property name="calf_com_z" value="-0.202805"/>
<xacro:property name="calf_ixx" value="0.015011003"/>
<xacro:property name="calf_ixy" value="0.000000052"/>
<xacro:property name="calf_ixz" value="0.000250042"/>
<xacro:property name="calf_iyy" value="0.015159462"/>
<xacro:property name="calf_iyz" value="0.000000461"/>
<xacro:property name="calf_izz" value="0.000375749"/>
<xacro:property name="calf_rotor_mass" value="0.266"/>
<xacro:property name="calf_rotor_com_x" value="0.0"/>
<xacro:property name="calf_rotor_com_y" value="0.0"/>
<xacro:property name="calf_rotor_com_z" value="0.0"/>
<xacro:property name="calf_rotor_ixx" value="0.000485657"/>
<xacro:property name="calf_rotor_ixy" value="0.0"/>
<xacro:property name="calf_rotor_ixz" value="0.0"/>
<xacro:property name="calf_rotor_iyy" value="0.00091885"/>
<xacro:property name="calf_rotor_iyz" value="0.0"/>
<xacro:property name="calf_rotor_izz" value="0.000485657"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.05"/>
</robot>

266
locomotion/src/robots/b1_description/xacro/gazebo.xacro Normal file
View File

@ -0,0 +1,266 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/b1_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<!-- <gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Green</material>
</plot>
</plugin>
</gazebo> -->
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<!-- Visualization of Foot contacts. -->
<gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot>

272
locomotion/src/robots/b1_description/xacro/leg.xacro Normal file
View File

@ -0,0 +1,272 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find b1_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<joint name="${name}_hip_rotor_joint" type="fixed">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip_rotor"/>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="package://b1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<link name="${name}_hip_rotor">
<visual>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<!-- <collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
<mass value="${hip_rotor_mass}"/>
<inertia
ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
izz="${hip_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
</joint>
<joint name="${name}_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh_rotor"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://b1_description/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://b1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<link name="${name}_thigh_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<!-- <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
<mass value="${thigh_rotor_mass}"/>
<inertia
ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
izz="${thigh_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
</joint>
<joint name="${name}_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf_rotor"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<link name="${name}_calf_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<!-- <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
<mass value="${calf_rotor_mass}"/>
<inertia
ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
izz="${calf_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

40
locomotion/src/robots/b1_description/xacro/materials.xacro Normal file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

102
locomotion/src/robots/b1_description/xacro/robot.xacro Normal file
View File

@ -0,0 +1,102 @@
<?xml version="1.0"?>
<robot name="b1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find b1_description)/xacro/const.xacro"/>
<xacro:include filename="$(find b1_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find b1_description)/xacro/leg.xacro"/>
<xacro:include filename="$(find b1_description)/xacro/gazebo.xacro"/>
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1" front_hind_dae="True" />
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False" />
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False" />
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
</robot>

46
locomotion/src/robots/b1_description/xacro/stairs.xacro Normal file
View File

@ -0,0 +1,46 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="stair_length" value="0.640" />
<xacro:property name="stair_width" value="0.310" />
<xacro:property name="stair_height" value="0.170" />
<xacro:macro name="stairs" params="stairs xpos ypos zpos">
<joint name="stair_joint_origin" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="stair_link_${stairs}"/>
</joint>
<link name="stair_link_${stairs}">
<visual>
<geometry>
<box size="${stair_length} ${stair_width} ${stair_height}"/>
</geometry>
<material name="grey"/>
<origin rpy="0 0 0" xyz="${xpos} ${ypos} ${zpos}"/>
</visual>
<collision>
<geometry>
<box size="${stair_length} ${stair_width} ${stair_height}"/>
</geometry>
</collision>
<inertial>
<mass value="0.80"/>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
</inertial>
</link>
<xacro:if value="${stairs}">
<xacro:stairs stairs="${stairs-1}" xpos="0" ypos="${ypos-stair_width/2}" zpos="${zpos+stair_height}"/>
<joint name="stair_joint_${stairs}" type="fixed">
<parent link="stair_link_${stairs}"/>
<child link="stair_link_${stairs-1}"/>
</joint>
</xacro:if>
</xacro:macro>
</robot>

42
locomotion/src/robots/b1_description/xacro/transmission.xacro Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

43
locomotion/src/robots/convert_dae_to_stl_decimate.py Normal file
View File

@ -0,0 +1,43 @@
# Set the directory containing DAE files
dae_directory = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/b1_description/meshes"
# Set the output directory for STL files
stl_directory = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/b1_description/meshes"
# Set the ratio for the decimation modifier (adjust as needed)
decimation_ratio = 0.1
# List all files in the directory
files = os.listdir(dae_directory)
# Filter out only DAE files
dae_files = [file for file in files if file.endswith(".dae")]
# Loop through each DAE file and convert to STL
for dae_file in dae_files:
# Load DAE file
bpy.ops.wm.collada_import(filepath=os.path.join(dae_directory, dae_file))
# Apply the decimation modifier
bpy.ops.object.select_all(action='SELECT')
bpy.ops.object.modifier_add(type='DECIMATE')
# Explicitly set the context for the modifier_apply operation
# bpy.context.view_layer.objects.active = bpy.context.selected_objects[0]
bpy.context.object.modifiers["Decimate"].ratio = decimation_ratio
bpy.ops.object.modifier_apply(modifier="Decimate")
# Set the output path for STL file
stl_file = os.path.join(stl_directory, dae_file.replace(".dae", ".stl"))
# Export as STL with use_mesh_modifiers set to False
bpy.ops.export_mesh.stl(filepath=stl_file, use_selection=True, use_mesh_modifiers=False, ascii=False)
# Remove the imported mesh to avoid overlap
bpy.ops.object.select_all(action='DESELECT')
bpy.ops.object.select_by_type(type='MESH')
bpy.ops.object.delete()
# Print a message indicating the conversion is complete
print("DAE to STL conversion completed.")

36
locomotion/src/robots/convert_stl_to_obj.py Normal file
View File

@ -0,0 +1,36 @@
import bpy
import os
# Set the directory containing STL files
directory = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/m2_description/meshes"
# Set the output directory for OBJ files
output_directory = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/m2_description/mujoco/assets"
# List all files in the directory
files = os.listdir(directory)
# Filter out only STL files
stl_files = [file for file in files if file.endswith(".stl") or file.endswith(".STL")]
# Loop through each STL file and convert to OBJ
for stl_file in stl_files:
# Load STL file
bpy.ops.import_mesh.stl(filepath=os.path.join(directory, stl_file))
# Set the output path for OBJ file
obj_file = os.path.join(output_directory, stl_file.replace(".STL", ".obj"))
# Export as OBJ
bpy.ops.wm.obj_export(
filepath=obj_file,
check_existing=True,
)
# Remove the imported mesh to avoid overlap
bpy.ops.object.select_all(action='DESELECT')
bpy.ops.object.select_by_type(type='MESH')
bpy.ops.object.delete()
# Print a message indicating the conversion is complete
print("STL to OBJ conversion completed.")

19
locomotion/src/robots/go1_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,19 @@
cmake_minimum_required(VERSION 2.8.3)
project(go1_description)
find_package(catkin REQUIRED COMPONENTS
genmsg
roscpp
std_msgs
tf
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
# include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)

70
locomotion/src/robots/go1_description/config/robot_control.yaml Normal file
View File

@ -0,0 +1,70 @@
go1_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

482
locomotion/src/robots/go1_description/launch/check_joint.rviz Normal file
View File

@ -0,0 +1,482 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 523
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.5
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_chin:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_face:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_laserscan_link_left:
Alpha: 1
Show Axes: false
Show Trail: false
camera_laserscan_link_right:
Alpha: 1
Show Axes: false
Show Trail: false
camera_left:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_optical_chin:
Alpha: 1
Show Axes: false
Show Trail: false
camera_optical_face:
Alpha: 1
Show Axes: false
Show Trail: false
camera_optical_left:
Alpha: 1
Show Axes: false
Show Trail: false
camera_optical_rearDown:
Alpha: 1
Show Axes: false
Show Trail: false
camera_optical_right:
Alpha: 1
Show Axes: false
Show Trail: false
camera_rearDown:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
camera_right:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
trunk:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ultraSound_face:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ultraSound_left:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ultraSound_right:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: false
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /cam1/point_cloud_face
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /cam2/point_cloud_chin
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /cam3/point_cloud_left
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /cam4/point_cloud_right
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /cam5/point_cloud_rearDown
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Global Options:
Background Color: 238; 238; 238
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.0180186033248901
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.10819146782159805
Y: 0.02224866859614849
Z: -0.08825916796922684
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.3647974133491516
Target Frame: <Fixed Frame>
Yaw: 4.123597621917725
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 752
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001c700000296fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000296000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000042800fffffffb0000000800540069006d006501000000000000045000000000000000000000039d0000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1386
X: 442
Y: 159

23
locomotion/src/robots/go1_description/launch/go1_rviz.launch Normal file
View File

@ -0,0 +1,23 @@
<launch>
<arg name="user_debug" default="false"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find go1_description)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- for higher robot_state_publisher average rate-->
<!-- <param name="rate" value="1000"/> -->
<!-- send fake joint values -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find go1_description)/launch/check_joint.rviz"/>
</launch>

339
locomotion/src/robots/go1_description/meshes/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

303
locomotion/src/robots/go1_description/meshes/depthCamera.dae Normal file
View File

File diff suppressed because one or more lines are too long

408
locomotion/src/robots/go1_description/meshes/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

783
locomotion/src/robots/go1_description/meshes/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

1266
locomotion/src/robots/go1_description/meshes/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

132
locomotion/src/robots/go1_description/meshes/trunk.dae Normal file
View File

File diff suppressed because one or more lines are too long

369
locomotion/src/robots/go1_description/meshes/ultraSound.dae Normal file
View File

File diff suppressed because one or more lines are too long

13
locomotion/src/robots/go1_description/package.xml Normal file
View File

@ -0,0 +1,13 @@
<?xml version="1.0"?>
<package format="2">
<name>go1_description</name>
<version>0.0.0</version>
<description>The go1_description package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
</package>

1777
locomotion/src/robots/go1_description/urdf/go1.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

159
locomotion/src/robots/go1_description/xacro/const.xacro Normal file
View File

@ -0,0 +1,159 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.0935"/>
<xacro:property name="trunk_length" value="0.3762"/>
<xacro:property name="trunk_height" value="0.114"/>
<xacro:property name="hip_radius" value="0.046"/>
<xacro:property name="hip_length" value="0.04"/>
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
<xacro:property name="thigh_shoulder_length" value="0.032"/>
<xacro:property name="thigh_width" value="0.0245"/>
<xacro:property name="thigh_height" value="0.034"/>
<xacro:property name="calf_width" value="0.016"/>
<xacro:property name="calf_height" value="0.016"/>
<xacro:property name="foot_radius" value="0.02"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.08"/>
<xacro:property name="thigh_length" value="0.213"/>
<xacro:property name="calf_length" value="0.213"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.1881"/>
<xacro:property name="leg_offset_y" value="0.04675"/>
<!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
<xacro:property name="hip_offset" value="0.08"/>
<!-- offset of link and rotor locations (left front) -->
<xacro:property name="hip_offset_x" value="0.1881"/>
<xacro:property name="hip_offset_y" value="0.04675"/>
<xacro:property name="hip_offset_z" value="0.0"/>
<xacro:property name="hip_rotor_offset_x" value="0.11215"/>
<xacro:property name="hip_rotor_offset_y" value="0.04675"/>
<xacro:property name="hip_rotor_offset_z" value="0.0"/>
<xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.08"/>
<xacro:property name="thigh_offset_z" value="0.0"/>
<xacro:property name="thigh_rotor_offset_x" value="0.0"/>
<xacro:property name="thigh_rotor_offset_y" value="-0.00015"/>
<xacro:property name="thigh_rotor_offset_z" value="0.0"/>
<xacro:property name="calf_offset_x" value="0.0"/>
<xacro:property name="calf_offset_y" value="0.0"/>
<xacro:property name="calf_offset_z" value="-0.213"/>
<xacro:property name="calf_rotor_offset_x" value="0.0"/>
<xacro:property name="calf_rotor_offset_y" value="-0.03235"/>
<xacro:property name="calf_rotor_offset_z" value="0.0"/>
<!-- joint limits -->
<xacro:property name="damping" value="0.01"/>
<xacro:property name="friction" value="0.2"/>
<xacro:property name="hip_position_max" value="0.863"/>
<xacro:property name="hip_position_min" value="-0.863"/>
<xacro:property name="hip_velocity_max" value="30.1"/>
<xacro:property name="hip_torque_max" value="23.7"/>
<xacro:property name="thigh_position_max" value="4.501"/>
<xacro:property name="thigh_position_min" value="-0.686"/>
<xacro:property name="thigh_velocity_max" value="30.1"/>
<xacro:property name="thigh_torque_max" value="23.7"/>
<xacro:property name="calf_position_max" value="-0.888"/>
<xacro:property name="calf_position_min" value="-2.818"/>
<xacro:property name="calf_velocity_max" value="20.06"/>
<xacro:property name="calf_torque_max" value="35.55"/>
<!-- dynamics inertial value total 12.84kg -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="5.204"/>
<xacro:property name="trunk_com_x" value="0.0223"/>
<xacro:property name="trunk_com_y" value="0.002"/>
<xacro:property name="trunk_com_z" value="-0.0005"/>
<xacro:property name="trunk_ixx" value="0.0168128557"/>
<xacro:property name="trunk_ixy" value="-0.0002296769"/>
<xacro:property name="trunk_ixz" value="-0.0002945293"/>
<xacro:property name="trunk_iyy" value="0.063009565"/>
<xacro:property name="trunk_iyz" value="-0.0000418731"/>
<xacro:property name="trunk_izz" value="0.0716547275"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="0.591"/>
<xacro:property name="hip_com_x" value="-0.005657"/>
<xacro:property name="hip_com_y" value="-0.008752"/>
<xacro:property name="hip_com_z" value="-0.000102"/>
<xacro:property name="hip_ixx" value="0.000334008405"/>
<xacro:property name="hip_ixy" value="-0.000010826066"/>
<xacro:property name="hip_ixz" value="0.000001290732"/>
<xacro:property name="hip_iyy" value="0.000619101213"/>
<xacro:property name="hip_iyz" value="0.000001643194"/>
<xacro:property name="hip_izz" value="0.00040057614"/>
<xacro:property name="hip_rotor_mass" value="0.089"/>
<xacro:property name="hip_rotor_com_x" value="0.0"/>
<xacro:property name="hip_rotor_com_y" value="0.0"/>
<xacro:property name="hip_rotor_com_z" value="0.0"/>
<xacro:property name="hip_rotor_ixx" value="0.000111842"/>
<xacro:property name="hip_rotor_ixy" value="0.0"/>
<xacro:property name="hip_rotor_ixz" value="0.0"/>
<xacro:property name="hip_rotor_iyy" value="0.000059647"/>
<xacro:property name="hip_rotor_iyz" value="0.0"/>
<xacro:property name="hip_rotor_izz" value="0.000059647"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="0.92"/>
<xacro:property name="thigh_com_x" value="-0.003342"/>
<xacro:property name="thigh_com_y" value="-0.018054"/>
<xacro:property name="thigh_com_z" value="-0.033451"/>
<xacro:property name="thigh_ixx" value="0.004431760472"/>
<xacro:property name="thigh_ixy" value="0.000057496807"/>
<xacro:property name="thigh_ixz" value="-0.000218457134"/>
<xacro:property name="thigh_iyy" value="0.004485671726"/>
<xacro:property name="thigh_iyz" value="0.000572001265"/>
<xacro:property name="thigh_izz" value="0.000740309489"/>
<xacro:property name="thigh_rotor_mass" value="0.089"/>
<xacro:property name="thigh_rotor_com_x" value="0.0"/>
<xacro:property name="thigh_rotor_com_y" value="0.0"/>
<xacro:property name="thigh_rotor_com_z" value="0.0"/>
<xacro:property name="thigh_rotor_ixx" value="0.000059647"/>
<xacro:property name="thigh_rotor_ixy" value="0.0"/>
<xacro:property name="thigh_rotor_ixz" value="0.0"/>
<xacro:property name="thigh_rotor_iyy" value="0.000111842"/>
<xacro:property name="thigh_rotor_iyz" value="0.0"/>
<xacro:property name="thigh_rotor_izz" value="0.000059647"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.135862"/>
<xacro:property name="calf_com_x" value="0.006197"/>
<xacro:property name="calf_com_y" value="0.001408"/>
<xacro:property name="calf_com_z" value="-0.116695"/>
<xacro:property name="calf_ixx" value="0.001088793059"/>
<xacro:property name="calf_ixy" value="-0.000000255679"/>
<xacro:property name="calf_ixz" value="0.000007117814"/>
<xacro:property name="calf_iyy" value="0.001100428748"/>
<xacro:property name="calf_iyz" value="0.000002077264"/>
<xacro:property name="calf_izz" value="0.000024787446"/>
<xacro:property name="calf_rotor_mass" value="0.089"/>
<xacro:property name="calf_rotor_com_x" value="0.0"/>
<xacro:property name="calf_rotor_com_y" value="0.0"/>
<xacro:property name="calf_rotor_com_z" value="0.0"/>
<xacro:property name="calf_rotor_ixx" value="0.000059647"/>
<xacro:property name="calf_rotor_ixy" value="0.0"/>
<xacro:property name="calf_rotor_ixz" value="0.0"/>
<xacro:property name="calf_rotor_iyy" value="0.000111842"/>
<xacro:property name="calf_rotor_iyz" value="0.0"/>
<xacro:property name="calf_rotor_izz" value="0.000059647"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.06"/>
</robot>

87
locomotion/src/robots/go1_description/xacro/depthCamera.xacro Normal file
View File

@ -0,0 +1,87 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="depthCamera" params="camID name *origin">
<joint name="camera_joint_${name}" type="fixed">
<xacro:insert_block name="origin"/>
<parent link="trunk"/>
<child link="camera_${name}"/>
</joint>
<link name="camera_${name}">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<joint name="camera_optical_joint_${name}" type="fixed">
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="camera_${name}"/>
<child link="camera_optical_${name}"/>
</joint>
<link name="camera_optical_${name}">
</link>
<gazebo reference="camera_${name}">
<!-- <material>Gazebo/Black</material> -->
<sensor name="camera_${name}_camera" type="depth">
<update_rate>16</update_rate>
<camera>
<horizontal_fov>2.094</horizontal_fov>
<image>
<width>928</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>5</far>
</clip>
</camera>
<plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.025</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>camera_${name}_ir</cameraName>
<imageTopicName>/camera_${name}/color/image_raw</imageTopicName>
<cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>
<depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>
<!-- <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> -->
<pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>
<frameName>camera_optical_${name}</frameName>
<pointCloudCutoff>0.1</pointCloudCutoff>
<pointCloudCutoffMax>1.5</pointCloudCutoffMax>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0.0045</Cx>
<Cy>0.0039</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>

385
locomotion/src/robots/go1_description/xacro/gazebo.xacro Normal file
View File

@ -0,0 +1,385 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/go1_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<!-- <gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Green</material>
</plot>
</plugin>
</gazebo> -->
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Depth camera -->
<!-- <gazebo>
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.025</baseline>
<always_on>true</always_on>
<updateRate>0.0</updateRate>
<cameraName>unitree_camera_left</cameraName>
<frameName>depthCamera_link_left</frameName>
<imageTopicName>rgb/imageRaw_left</imageTopicName>
<depthImageTopicName>depth/imageRaw_left</depthImageTopicName>
<pointCloudTopicName>depth/points_left</pointCloudTopicName>
<cameraInfoTopicName>rgb/cameraInfo_left</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/cameraInfo_left</depthImageCameraInfoTopicName>
<pointCloudCutoff>0.1</pointCloudCutoff>
<pointCloudCutoffMax>1.5</pointCloudCutoffMax>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0.0</CxPrime>
<Cx>0.0045</Cx>
<Cy>0.0039</Cy>
<focalLength>0.004</focalLength>
<hackBaseline>0.0</hackBaseline>
</plugin>
</gazebo> -->
<!-- <gazebo reference="depthCamera_link_left">
<sensor name="unitree_camera_left" type="depth_camera">
<update_rate>16</update_rate>
<always_on>true</always_on>
<visualize>true</visualize>
<camera>
<horizontal_fov>2.094</horizontal_fov>
<image>
<width>928</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<depth_camera></depth_camera>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.025</baseline>
<always_on>true</always_on>
<updateRate>0.0</updateRate>
<cameraName>unitree_camera_left</cameraName>
<frameName>depthCamera_link_left</frameName>
<imageTopicName>rgb/imageRaw_left</imageTopicName>
<depthImageTopicName>depth/imageRaw_left</depthImageTopicName>
<pointCloudTopicName>depth/points_left</pointCloudTopicName>
<cameraInfoTopicName>rgb/cameraInfo_left</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/cameraInfo_left</depthImageCameraInfoTopicName>
<pointCloudCutoff>0.1</pointCloudCutoff>
<pointCloudCutoffMax>1.5</pointCloudCutoffMax>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
<CxPrime>0.0</CxPrime>
<Cx>0.0045</Cx>
<Cy>0.0039</Cy>
<focalLength>0.004</focalLength>
<hackBaseline>0.0</hackBaseline>
</plugin>
</sensor>
</gazebo> -->
<gazebo reference="depthCamera_link_left">
<sensor name="camera_link_camera" type="depth">
<update_rate>20</update_rate>
<camera>
<horizontal_fov>1.047198</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.05</near>
<far>3</far>
</clip>
</camera>
<plugin name="camera_link_controller" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<cameraName>camera_link_ir</cameraName>
<imageTopicName>/camera_link/color/image_raw</imageTopicName>
<cameraInfoTopicName>/camera_link/color/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera_link/depth/image_raw</depthImageTopicName>
<depthImageInfoTopicName>/camera_link/depth/camera_info</depthImageInfoTopicName>
<pointCloudTopicName>/camera_link/depth/points</pointCloudTopicName>
<frameName>depthCamera_link_left</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
<distortionK1>0.00000001</distortionK1>
<distortionK2>0.00000001</distortionK2>
<distortionK3>0.00000001</distortionK3>
<distortionT1>0.00000001</distortionT1>
<distortionT2>0.00000001</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<!-- Visualization of Foot contacts. -->
<gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot>

272
locomotion/src/robots/go1_description/xacro/leg.xacro Normal file
View File

@ -0,0 +1,272 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<joint name="${name}_hip_rotor_joint" type="fixed">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip_rotor"/>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="package://go1_description/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<link name="${name}_hip_rotor">
<visual>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
<material name="green"/>
</visual>
<!-- <collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
<mass value="${hip_rotor_mass}"/>
<inertia
ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
izz="${hip_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
</joint>
<joint name="${name}_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh_rotor"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://go1_description/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<link name="${name}_thigh_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
<material name="green"/>
</visual>
<!-- <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
<mass value="${thigh_rotor_mass}"/>
<inertia
ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
izz="${thigh_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
</joint>
<joint name="${name}_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf_rotor"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://go1_description/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<link name="${name}_calf_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
<material name="green"/>
</visual>
<!-- <collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
<mass value="${calf_rotor_mass}"/>
<inertia
ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
izz="${calf_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

40
locomotion/src/robots/go1_description/xacro/materials.xacro Normal file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

151
locomotion/src/robots/go1_description/xacro/robot.xacro Normal file
View File

@ -0,0 +1,151 @@
<?xml version="1.0"?>
<robot name="go1" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:include filename="$(find go1_description)/xacro/const.xacro"/>
<xacro:include filename="$(find go1_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find go1_description)/xacro/leg.xacro"/>
<!-- <xacro:include filename="$(find go1_description)/xacro/stairs.xacro"/> -->
<xacro:include filename="$(find go1_description)/xacro/gazebo.xacro"/>
<xacro:include filename="$(find go1_description)/xacro/depthCamera.xacro"/>
<xacro:include filename="$(find go1_description)/xacro/ultraSound.xacro"/>
<!-- <xacro:include filename="$(find go1_gazebo)/launch/stairs.urdf.xacro"/> -->
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rotor related joint and link is only for demonstrate location. -->
<!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://go1_description/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<!-- <material name="orange"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1" front_hind_dae="True" />
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False" />
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False" />
<xacro:depthCamera camID="1" name="face">
<origin rpy="${PI} 0 0" xyz="0.2785 0.0125 0.0167"/>
</xacro:depthCamera>
<xacro:depthCamera camID="2" name="chin">
<origin rpy="${PI} ${PI/2} 0" xyz="0.2522 0.0125 -0.0436"/>
</xacro:depthCamera>
<xacro:depthCamera camID="3" name="left">
<origin rpy="${PI} 0.2618 ${PI/2}" xyz="-0.066 0.082 -0.0176"/>
</xacro:depthCamera>
<xacro:depthCamera camID="4" name="right">
<origin rpy="${PI} 0.2618 ${-PI/2}" xyz="-0.041 -0.082 -0.0176"/>
</xacro:depthCamera>
<xacro:depthCamera camID="5" name="rearDown">
<origin rpy="${PI} ${PI/2} 0" xyz="-0.0825 0.0125 -0.04365"/>
</xacro:depthCamera>
<joint name="camera_laserscan_joint_left" type="fixed">
<origin rpy="0 0.2618 0" xyz="0 0 0"/>
<parent link="camera_left"/>
<child link="camera_laserscan_link_left"/>
</joint>
<link name="camera_laserscan_link_left">
</link>
<joint name="camera_laserscan_joint_right" type="fixed">
<origin rpy="0 0.2618 0" xyz="0 0 0"/>
<parent link="camera_right"/>
<child link="camera_laserscan_link_right"/>
</joint>
<link name="camera_laserscan_link_right">
</link>
<xacro:ultraSound name="left">
<origin rpy="0 0.2618 ${PI/2}" xyz="-0.0535 0.0826 0.00868"/>
</xacro:ultraSound>
<xacro:ultraSound name="right">
<origin rpy="0 0.2618 ${-PI/2}" xyz="-0.0535 -0.0826 0.00868"/>
</xacro:ultraSound>
<xacro:ultraSound name="face">
<origin rpy="0 0 0" xyz="0.2747 0.0 -0.0088"/>
</xacro:ultraSound>
</robot>

42
locomotion/src/robots/go1_description/xacro/transmission.xacro Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

33
locomotion/src/robots/go1_description/xacro/ultraSound.xacro Normal file
View File

@ -0,0 +1,33 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ultraSound" params="name *origin">
<joint name="ultraSound_joint_${name}" type="fixed">
<xacro:insert_block name="origin"/>
<parent link="trunk"/>
<child link="ultraSound_${name}"/>
</joint>
<link name="ultraSound_${name}">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
</xacro:macro>
</robot>

14
locomotion/src/robots/go2_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(go2_description)
find_package(catkin REQUIRED)
catkin_package()
find_package(roslaunch)
foreach(dir config launch meshes urdf)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)

54
locomotion/src/robots/go2_description/README.md Normal file
View File

@ -0,0 +1,54 @@
## go2_urdf
This repository contains the urdf model of go2.
## Build the library
Create a new catkin workspace:
```
# Create the directories
# Do not forget to change <...> parts
mkdir -p <directory_to_ws>/<catkin_ws_name>/src
cd <directory_to_ws>/<catkin_ws_name>/
# Initialize the catkin workspace
catkin init
```
Clone this library:
```
# Navigate to the directory of src
# Do not forget to change <...> parts
cd <directory_to_ws>/<catkin_ws_name>/src
git clone git@github.com:unitreerobotics/go2_urdf.git
```
Build:
```
# Build it
catkin build
# Source it
source <directory_to_ws>/<catkin_ws_name>/devel/setup.bash
```
## Run the library
```
# Show urdf model of go2 in Rviz
roslaunch go2_description go2_rviz.launch
```
## When used for isaac gym or other similiar engine
Collision parameters in urdf can be amended to better train the robot:
Open "go2_description.urdf" in "./go2_description/urdf",
and amend the ` box size="0.213 0.0245 0.034" ` in links of "FL_thigh", "FR_thigh", "RL_thigh", "RR_thigh".
For example, change previous values to ` box size="0.11 0.0245 0.034" ` means the length of the thigh is shortened from 0.213 to 0.11, which can avoid unnecessary collision between the thigh link and the calf link.
The collision model before and after the above amendment are shown as "Normal_collision_model.png" and "Amended_collision_model.png" respectively.

1
locomotion/src/robots/go2_description/config/joint_names_go2_description.yaml Normal file
View File

@ -0,0 +1 @@
controller_joint_names: ['', 'FL_hip_joint', 'Fl_thigh_joint', 'FL_calf_joint', 'FR_hip_joint', 'FR_thigh_joint', 'FR_calf_joint', 'RL_hip_joint', 'RL_thigh_joint', 'RL_calf_joint', 'RR_hip_joint', 'RR_thigh_joint', 'RR_calf_joint', ]

70
locomotion/src/robots/go2_description/config/robot_control.yaml Executable file
View File

@ -0,0 +1,70 @@
go2_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: unitree_legged_control/UnitreeJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

217
locomotion/src/robots/go2_description/dae/base.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
locomotion/src/robots/go2_description/dae/calf.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
locomotion/src/robots/go2_description/dae/calf_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

89
locomotion/src/robots/go2_description/dae/foot.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
locomotion/src/robots/go2_description/dae/hip.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
locomotion/src/robots/go2_description/dae/thigh.dae Normal file
View File

File diff suppressed because one or more lines are too long

130
locomotion/src/robots/go2_description/dae/thigh_mirror.dae Normal file
View File

File diff suppressed because one or more lines are too long

250
locomotion/src/robots/go2_description/launch/check_joint.rviz Normal file
View File

@ -0,0 +1,250 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /TF1/Frames1
Splitter Ratio: 0.5
Tree Height: 746
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 85; 87; 83
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Fl_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RL_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_calf:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_foot:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_hip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_thigh:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu:
Alpha: 1
Show Axes: false
Show Trail: false
radar:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Alpha: 1
Marker Scale: 0.4000000059604645
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /link_inertias_viz
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 238; 238; 236
Default Light: true
Fixed Frame: base
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.7113326191902161
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.14559818804264069
Y: 0.02902654930949211
Z: -0.1431877315044403
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5047972798347473
Target Frame: <Fixed Frame>
Yaw: 0.7604149580001831
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1043
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 1920
Y: 0

20
locomotion/src/robots/go2_description/launch/gazebo.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<include
file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find go2_description)/urdf/go2_description.urdf -urdf -model go2_description"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

22
locomotion/src/robots/go2_description/launch/go2_rviz.launch Normal file
View File

@ -0,0 +1,22 @@
<launch>
<arg name="user_debug" default="false"/>
<param name="robot_description" textfile="$(find go2_description)/urdf/go2_description.urdf" />
<!-- for higher robot_state_publisher average rate-->
<!-- <param name="rate" value="1000"/> -->
<!-- send fake joint values -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui">
<param name="use_gui" value="TRUE"/>
</node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find go2_description)/launch/check_joint.rviz"/>
</launch>

BIN
locomotion/src/robots/go2_description/meshes/base.stl Normal file
View File

Binary file not shown.

130
locomotion/src/robots/go2_description/meshes/calf.dae Executable file
View File

File diff suppressed because one or more lines are too long

Some files were not shown because too many files have changed in this diff Show More