1 Webots下的仿真环境
Tinymal-B通过URDF可以导出Webots下的仿真模型，通过修改各关节的旋转坐标系定义，保证与机器人坐标系设置一样，基本参数为：
机体质量为：4.6kg
机体转动惯量：Ix=0.4,Iy=0.5,Iz=0.6
电机：共12个单质量0.3kg
机体长度H：0.306m
机体宽度W：0.09m
大腿长度L1：0.12m
小腿长度L2：0.14m
侧展偏距L3：0.075m
膝关节虚拟连杆角度偏角Sita:13°
调整个Joint的Axis可以完成与MIT或右手坐标系的匹配
Webots下Tinymal-B桌面版仿真模型
在仿真中机器人模型包含了雷达、相机和IMU，足端增加了用于力矢量绘制的Node，同时World中还有相应的落足点指示物，模型下载地址为：
链接：https://pan.baidu.com/s/12BaedTkp2wsBDbr0Kt0MGw
提取码：ziiu
--来自百度网盘超级会员V3的分享机器人关节腿号定义为：
const char* motor_c[12]
{
"RF_2_Link_joint","RF_3_Link_joint","RF_1_Link_joint",//FR 0 1 2
"RH_2_Link_joint","RH_3_Link_joint","RH_1_Link_joint",//HR 3 4 5
"LF_2_Link_joint","LF_3_Link_joint","LF_1_Link_joint",//FL 6 7 8
"LH_2_Link_joint","LH_3_Link_joint","LH_1_Link_joint",//HL 9 10 11
};
其中侧展为1，大腿为2，小腿为3，Webots下设备初始化方式为：
//放在主函数循环之前 初始化设备
void webots_device_init(void)
{
/* 陀螺仪设备初始化 */
IMU = wb_robot_get_device("dogInertialUnit");
wb_inertial_unit_enable(IMU, TIME_STEP);
/* GPS设备初始化 */
//GPS = wb_robot_get_device("gps");
//wb_gps_enable(GPS, TIME_STEP);
RGB_F = wb_robot_get_device("RGB_F");
wb_camera_enable(RGB_F, TIME_STEP );
LIDAR_F = wb_robot_get_device("lidar");
wb_lidar_enable(LIDAR_F, TIME_STEP );
wb_lidar_enable_point_cloud(LIDAR_F);
RANGE_F = wb_robot_get_device("range-finder");
wb_range_finder_enable(RANGE_F, TIME_STEP);
// init lms291
display_lidar = wb_robot_get_device("lidar_display");
display_human1 = wb_robot_get_device("human_display1");
lidar_width = wb_lidar_get_horizontal_resolution(LIDAR_F);
max_range = wb_lidar_get_max_range(LIDAR_F);
// init the display
background = init_display(display_lidar);
background_human1 = init_display_human(display_human1);
/* 电机及位置传感器设备初始化 */
for (uint8_t i = 0; i < 12; i++) {
motor1[i] = wb_robot_get_device(motor_c[i]);
posensor1[i] = wb_motor_get_position_sensor(motor1[i]);
wb_position_sensor_enable(posensor1[i], TIME_STEP);
wb_motor_enable_torque_feedback(motor1[i], TIME_STEP);
}
/* 足端触地传感器设备初始化 */
for (uint8_t i = 0; i < 4; i++) {
//foot[i] = wb_robot_get_device(foot_c[i]);
//wb_touch_sensor_enable(foot[i], TIME_STEP);
}
/* 使能键盘读取 */
wb_keyboard_enable(TIME_STEP);
wb_joystick_enable(TIME_STEP);
}
2 Tinymal的基本坐标系定义
下面给出了后续仿真中Tinymal的坐标系定义，其与MIT和传统的右手定义可能不同，主要是为了保证硬件标定是更简单的流程，因此严格约定了每条腿都具有一样的定义，该定义也与实际机器人软件、ROS下的URDF可视化一样。
首先看下小腿角度定义，如下图所示，小腿角度0°为与大腿上臂重合，则此时应该为170°左右，各腿坐标系均为前方X上方Z，小腿ID为sita2，数组顺序为[1]：
注：由于Tinymal小腿为异形腿，实际足端仍然是末端位置，但连杆角度为膝关节轴为足端的虚拟连杆
下图为大腿角度，大腿0度为跨关节下方垂线，则下图中的角度为45°，大腿ID为sita1，数组顺序为[0]：
注：为了方便在实物中标定，坐标系定义中虽然前后两侧腿均X轴向前但，关节角度的定义确是对称的，如下图中两个大腿均为90°，小腿则只需要小腿各自大腿上臂来定义均为170°左右
最后我们看下侧展，侧展不同于大腿并没有采用对称处理，另外坐标系Y轴定义也与MIT或右手准则不一样，朝向机体右方，如下图（机体后视图）所示左右两侧腿角度均为45°，侧展ID为sita3，数组顺序为[2]：
最后看下机体坐标系定义，机体前方为X，右方为Y：
注：上述描述的坐标系角度如果需要调整为MIT或右手坐标系，只需要在代码中修正即可，另外所有关节角度旋转正方向与扭矩正负方向一致！
2 Webots下站立仿真Demo
开源了Webots下的站立仿真，程序完整地读取了机器人所有传感器参数，并与开源的样机站立程序一致，运行后机器人将在仿真环境中实现姿态调节！由于Webots更新IMU坐标系定义不一样，我们默认采用：
本例程在Ubuntu环境下开发，这样相比Windows很多依赖安装更加简单，默认采用QT和Cmake进行项目管理，下载后需要修改Cmakelist中对应Webots的路径：
修改为自己的安装路径
如果无法打开World后无法找到相应控制器，则需要新建一个控制，将代码复制进去修改路径：
项目主体文件目录：
如果没有采用源码安装Webots则需要修改项目Cmakelist如下：
cmake_minimum_required(VERSION 2.6)
SET(PROJECT_NAME my_controller12)
PROJECT(${PROJECT_NAME})
get_filename_component(PROJECT ${CMAKE_SOURCE_DIR} NAME)
find_package(Eigen3)
add_compile_definitions(__USE_SINGLE_PRECISION__)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3 -pthread" )
# set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/../../build)
include_directories(
"./build"
"./include_vmc"
"./include_wb"
"./vmc_inc"
"./mpc"
"./vision_location"
"./mpc"
"./common_utils"
"./urdf"
"./rbdl"
"./model"
"./cube_vision"
"/usr/local/webots/include/controller/c"
"/usr/local/webots/include/controller/cpp/"
"./qpOASES/include"
)
link_directories("/usr/local/lib")
include_directories("/usr/local/include")
LINK_LIBRARIES("/usr/local/webots/lib/controller/libController.so")
LINK_LIBRARIES("/usr/local/webots/lib/controller/libCppController.so")
AUX_SOURCE_DIRECTORY(gait_src DIR_SRCS)
AUX_SOURCE_DIRECTORY(locomotion_src DIR_SRCS1)
AUX_SOURCE_DIRECTORY(math_src DIR_SRCS2)
AUX_SOURCE_DIRECTORY(mpc DIR_SRCS3)
AUX_SOURCE_DIRECTORY(source DIR_SRCS4)
AUX_SOURCE_DIRECTORY(source_vmc DIR_SRCS5)
AUX_SOURCE_DIRECTORY(vision_location DIR_SRCS6)
AUX_SOURCE_DIRECTORY(rbdl DIR_SRCS7)
AUX_SOURCE_DIRECTORY(urdf DIR_SRCS8)
AUX_SOURCE_DIRECTORY(common_utils DIR_SRCS9)
AUX_SOURCE_DIRECTORY(model DIR_SRCS10)
AUX_SOURCE_DIRECTORY(cube_vision DIR_SRCS11)
ADD_EXECUTABLE(${PROJECT_NAME} ${DIR_SRCS} ${DIR_SRCS1} ${DIR_SRCS2} ${DIR_SRCS3} ${DIR_SRCS4} ${DIR_SRCS5} ${DIR_SRCS6} ${DIR_SRCS7} ${DIR_SRCS8} ${DIR_SRCS9} ${DIR_SRCS10} ${DIR_SRCS11})
add_subdirectory(qpOASES 1lib/ EXCLUDE_FROM_ALL)
target_link_libraries(${PROJECT_NAME} qpOASES)
add_subdirectory(yaml-cpp lib/ EXCLUDE_FROM_ALL)
target_link_libraries(${PROJECT_NAME} yaml-cpp)
link_directories($ENV{WEBOTS_HOME}/lib/controller)
link_directories($ENV{WEBOTS_HOME}/lib/webots)
file(GLOB C_SOURCES *.c)
file(GLOB CPP_SOURCES *.cpp)
set(SOURCES ${C_SOURCES} ${CPP_SOURCES})
if (NOT CPP_SOURCES STREQUAL "") # Sources contain C++ files
set (LIBRARIES ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX})
include_directories($ENV{WEBOTS_HOME}/include/controller/c $ENV{WEBOTS_HOME}/include/controller/cpp)
else() # C
set (LIBRARIES ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX})
include_directories($ENV{WEBOTS_HOME}/include/controller/c)
endif()
add_custom_command(TARGET ${PROJECT} POST_BUILD COMMAND ${CMAKE_COMMAND} -E
copy ${CMAKE_BINARY_DIR}/${PROJECT} ${CMAKE_SOURCE_DIR}
)
站立Demo项目下载地址：
链接：https://pan.baidu.com/s/1v7EnAam_x8PIuiIClILVNg
提取码：40o6
--来自百度网盘超级会员V3的分享注：后续随开源项目关注更新步态机动Demo，Tinymal项目订购加入超过50人！
