Webots Supervisor下的四足机器人可视化教程

1介绍

众所周知四足机器人的仿真对开发算法是非常重要的，由于四足机器人即包含支撑力伺服控制，规划控制，如何确认规划的结果是符合预期的是十分关键的，特别是当涉及到落足点规划或高程图显示时，如何没有合适的可视化接口，那是没法快速验证算法的正确性的。以MIT Cheetha的仿真为例其提供了OpenGL下良好的可视化接口，使用其能快速实现高程图显示：

MIT 仿真下的可视化与窗口

对于其他仿真环境如Pybullet或RaiSIM等，其可视化接口更加方便，总体来说对于一个四足机器人可视化除了方便于调试外，更重要的是可以让你的仿真看起来逼格很高，可以参考ETH论文中的截图，当然基于ROS和Rviz下做四足机器人的可视化或数字孪生我感觉才是正途，如果只考虑四足机器人的本体控制，需要完成的可视化主要包括如下几个部分：

（1）机器人本体可视化，大部分仿真环境直接支持；

（2）足端轨迹的可视化，用于摆动轨迹规划算法验证；

（3）落足点可视化，用于落足点选择算法验证；

（4）足端力可视化，用于力控算法验证；

（5）机器人质心轨迹可视化，用于MPC等预测轨迹控制效果验证；

（6）高程图可视化，用于落足算法、高程区域选择算法验证；

ETH的可视化

2 Webots下Supervisor的可视化方案

我本人目前主要基于Webots来开发机器人的运动控制算法，Webots最大的特点就是IDE傻瓜式，渲染效果好，附带很多现成的模型，这一点和Gazebo很像，在Windows下的开发则可以基于VisualStudio的IDE来实现，也十分的方便，但是在我实现可视化的时候发现相关的教程十分的少，这可能也是Webots为何不如其他几个仿真环境被论文引用得多的原因，如果要学会熟练使用除了CSDN或知乎上几个简单的入门例子外，只能去啃官方的文档和运行其中的示例，总而言之Webots是可以实现可视化和获取仿真数据的，其需要使用Supervisor控制器，下面贴出了一个知乎帖子，我在实现时也参考了其相关介绍以及晚月君的例程，下面仅针对四足机器人需要的可视化功能进行介绍，如果要学会使用还是需要参考我提供的例子来自己进行尝试：

Webots Supervisor控制器初探-力矢量显示12 赞同 · 0 评论文章

2.1 建立Supervisor控制器

在Webots中可以给Robot类的节点绑定控制器，对于我们的四足机器人往往绑定的是具有控制算法的可执行文件，而要实现从Webots环境下获取真实数据或改变某个节点的属性，就需要新建一个机器人并为其开发带有Supervisor类的控制器：

另外记得一定要把其supervisor属性勾上，然后新建C++的VS项目工程：

2.2 Webots中节点与Field

在Webots中我们操作特点的节点是通过查询其DEF表示，然后修改其Field域中的相关参数来实现的，如控制机器人的电机扭矩时就需要首先指定电机节点的DEF，然后调用Webots的接口设置扭矩，但实际其原理也是修改了该节点下隐含的扭矩Field，以下面所建立的刚体为例。

我们在选中刚体后，指定其DEF为TAR_POS_NODE_FR，则其Field中包含了描述其位姿的translation与rotation等参数，通过Supervisor控制器我们就可以对其进行修改，从而直接反馈在可视化的界面中。

例如我们要修改刚体位置来实现对落足点位置的变换可视化，则在代码中首先需要绑定其DEF和对应的translation域：

	WbNodeRef robot_node1 = wb_supervisor_node_get_from_def("TAR_POS_NODE_FR");//直接调节def 节点的位置 适用于刚体与robot
	WbFieldRef trans_field1 = wb_supervisor_node_get_field(robot_node1, "translation");

通过点击translation参数我们知道其是Vector3的数据类型：

因此通过采用vec3f赋值（SF_ API）就可以实现对其位置的动态改变，注意这里使用的域即绑定节点同级的域，如果更深层级域的参数，需要按上面新建节点和域来实现：

double foot_tar_location_global[4][3] = { 0,0,0 };//期望落足位置
wb_supervisor_field_set_sf_vec3f(trans_field1, foot_tar_location_global[0]);

另外需要提醒的一点是这里赋值的参数是XYZ坐标，其与Webots下显示坐标轴一致：

同样如何要获取仿真中刚体的位置、姿态和速度真实数据，同样可以采用类似的方法来实现，例如我需要获取机器人质心的真实位置，则首先我在机器人安装的GPS或机体的Children下增加一个Transform节点并命名为DRAW_COG：

在代码中绑定节点：

WbNodeRef draw_node_cog = wb_supervisor_node_get_from_def("DRAW_COG");//需要在绘制的点的node下增加

则通过get的API函数就可以获取其对应域的参数：

const double *target_translation_cog;
const double *target_rotation_cog;
target_translation_cog = wb_supervisor_node_get_position(draw_node_cog);
target_rotation_cog = wb_supervisor_node_get_orientation(draw_node_cog);

相关的API还有很多，对于四足机器人来目前仅需要使用上述两个：

2.3 采用字符串自动建立节点

在2.2中通过绑定节点操作Field我们可以轻松地改变或者读取节点下的参数，但其前提是在World中已经具建立了相应的节点，对于单个刚体来说这是十分容易的，但是假如我们要建立一个高程图其有1000多个刚体来描述高程关系，那通过人工添加将十分麻烦，另外修改参数时也非常困难，又比如我们在强化学习的项目中往往会向机器人丢随机的刚体添加扰动来提高算法的鲁棒性，在仿真时是没法手段添加的。因此这里就需要通过代码的方式来自动建立相应的节点，这里参考Webots例程采用字符串的方式来实现，我们以建立一个高程地图为例。

首先需要说明的是Webot下每个节点对应的wbo文件实际都是基于字符串来书写的，例如之前我们提到的刚体节点，通过选中右键导出后我们可以用文本编辑器查看：

#VRML_OBJ R2021a utf8
DEF TAR_POS_NODE_FR Solid {
  children [
    Shape {
      appearance Appearance {
        material Material {
          diffuseColor 0.666667 0.666667 0
          emissiveColor 1 1 0
        }
      }
      geometry Sphere {
        radius 0.01
      }
    }
  ]
  name "TAR_POS_NODE"
}

可以看到其通过{}这样的层级关系来描述一个节点下各个Field的属性的参数，因此我们只需要安装例程照猫画虎既可以实现自动建立节点的目的。

例如我们要建立一个由GRID_W和GRID_W描述的高程图，每个高程栅格采用一个Plane来显示，则首先我们建立整个地图对于的Transform，指定其DEF为H_EMAP_DIV，然后确认是否已经存在：

static void create_map_shape() {//自动建立离散高程图绘制控件

	WbNodeRef existing_trail = wb_supervisor_node_get_from_def("H_EMAP_DIV");//绘制shape的名字
	if (existing_trail)
		wb_supervisor_node_remove(existing_trail);

之后我们建立一个字符串通过{}和\n的形式组合，按wbo的描述格式描述一个Transform节点，将其添加到其children下：

	char trail_string[0x10000] = "\0";  // Initialize a big string which will contain the TRAIL node.

	strcat(trail_string, "DEF H_EMAP_DIV Transform {\n");//1
	strcat(trail_string, "  translation 0 0 0\n");
	strcat(trail_string, "  rotation 0 0 0 0\n");
	strcat(trail_string, "  scale 1 1 1\n");
	strcat(trail_string, "}\n");//1

	WbFieldRef root_children_field = wb_supervisor_node_get_field(wb_supervisor_node_get_root(), "children");
	wb_supervisor_field_import_mf_node_from_string(root_children_field, -1, trail_string);

结果：

然后我们在整个局部地图下建立各个栅格，这里我们通过字符串组合为每个栅格Transform附上独立的ID:

		char *grid_num = new char[50];
		sprintf(grid_num, "DEF GRID_%d Transform {\n", i);
		char *grid_num_id = new char[50];
		sprintf(grid_num_id, "GRID_%d", i);


		char trail_string1[0x10000] = "\0";
		strcat(trail_string1, grid_num);//3
		strcat(trail_string1, "  translation 0 0 0\n");
		strcat(trail_string1, "  rotation 0 0 0 0\n");
		strcat(trail_string1, "  scale 1 1 1\n");
		strcat(trail_string1, "}\n");//3

		WbFieldRef root_children_field1 = wb_supervisor_node_get_field(wb_supervisor_node_get_from_def("H_EMAP_DIV"), "children");
		wb_supervisor_field_import_mf_node_from_string(root_children_field1, -1, trail_string1);

建立好栅格的Transform后我们就可以通过Field查找DEF修改其translation中的高度来实现模拟高程变化，为实现显示为其增加一个shape节点和相应的几何Plane形状，同时我们为了修改颜色还给颜色对应的material域设置了独立的DEF：

		char *grid_num_shape = new char[100];
		sprintf(grid_num_shape, " DEF GRID_%d_S Shape{\n", i);
		char *grid_num_shape_id = new char[100];
		sprintf(grid_num_shape_id, "  GRID_%d_S", i);
		char *grid_num_app = new char[100];
		sprintf(grid_num_app, "   appearance DEF GRID_%d_APP Appearance {\n", i);
		char *grid_num_app_id = new char[100];
		sprintf(grid_num_app_id, "  GRID_%d_APP", i);
		char *grid_num_mat = new char[100];
		sprintf(grid_num_mat, " material  DEF GRID_%d_MAT Material {\n", i);
		char *grid_num_mat_id = new char[100];
		sprintf(grid_num_mat_id, "  GRID_%d_MAT", i);

		char trail_string2[0x10000] = "\0";
		strcat(trail_string2, grid_num_shape);
		strcat(trail_string2, grid_num_app);
		strcat(trail_string2, grid_num_mat);
		strcat(trail_string2, "      diffuseColor 1 1 0\n");//颜色
		strcat(trail_string2, "      emissiveColor 1 1 0\n");
		strcat(trail_string2, "    }\n");
		strcat(trail_string2, "  }\n");

		strcat(trail_string2, "  geometry Plane {\n");
		strcat(trail_string2, "   size 0.02 0.02\n");
		strcat(trail_string2, "  }\n");
		strcat(trail_string2, "  }\n");

		WbFieldRef root_children_field2 = wb_supervisor_node_get_field(wb_supervisor_node_get_from_def(grid_num_id), "children");
		wb_supervisor_field_import_mf_node_from_string(root_children_field2, -1, trail_string2);

这里需要注意的是对于二级参数其DEF命名方式不同，例如对于Shape来说其命名为：

 DEF GRID_%d_S Shape{\n

而对于material来说其指定DEF命名为：

material  DEF GRID_%d_MAT Material {\n

注：如果书写错误则在运行时是不会建立该节点，具体命名方法如果无法确定可以采用之前导出的方式，先手工建立然后使用文本编辑器查看

最终建立的结果为：

那之后通过操作Field可以修改每个栅格的位置，另外还可以改变颜色来描述语义，这不在赘述，详细方法可以查看代码。

2.4 如何与机器人控制程序交互

通过上面的讲解相信你对操作域，来实现可视化已经有了基本的了解，那下面的问题就是如何从机器人控制程序中获取对应可视化的数据，另外将仿真的真实数据反馈给机器人控制器作为Ground Truth，由于Supervisor控制器与机器人控制器不属于同一个程序，因此这里可以采用数据共享的方式来实现。

最简单的方式是两个程序同时读写一个txt脚本实现数据的交互（晚月君的方式），但是其存在着格式修改麻烦，而且对读写的实时性和安全性也有问题，这里我采用了windows下共享内存的方式来实现。

简单来说就是两个程序定义相同的结构体然后写入到指定内存中，这样数据的变化会同时在两个程序中同步，例如我定义了机器人发送给Supervisor的结构体如下，其包括了期望落足位置、足端力、着地状态、质心状态、高程图信息：

typedef struct//从控制程序读取
{
	END_POS leg_sw_tar[4];
	int ground[4], touch[4];
	END_POS tar_grf[4];
	double force_rotate[4][4];
	double force_size[4];

	END_POS sw_tar_traj[4][1024];
	END_POS sw_tar_traj_webots[4][1024];
	int sw_traj_cnt[4];
	END_POS cog_pos;
	float cog_att[3];

	float grid_m[64][64];
	float grid_m_good[64][64];
	float grid_m_edge[64][64];
	float fgrid_m[FMAP_NUM][64][64];
	float grid_m_base_z;
	int en_draw_map;
}WEBTOS_DRAWING;
WEBTOS_DRAWING webots_draw;

Supervisor反馈给机器人的数据结构体如下，其包括了真实的执行位置以及人工高程图的原点：

typedef struct//发送到控制程序
{
	END_POS cog_real;
	END_POS fake_map_cog[10];
	double fake_map_ori[10][4];
}WEBTOS_DRAWING_TX;
WEBTOS_DRAWING_TX webots_draw_tx;

则读取机器人共享内存数据的方式为：

HANDLE hmapfile;
int win_mem_rx()//读取控制器内部数据
{
	static int init = 0;
	if (!init) {
		init = 1;
		hmapfile = OpenFileMappingA(FILE_MAP_READ, FALSE, "mem_control_t_draw");
	}
	if (hmapfile == NULL)
	{
		printf("open fail\n");
		return 0;
	}
	//创建指针，指向这片内存
	LPVOID lpbase = MapViewOfFile(hmapfile, FILE_MAP_READ, 0, 0, 0);
	if (lpbase == NULL)
	{
		printf("meme open fail\n");
	}

	float  *p = (float *)lpbase;
	struct WEBTOS_DRAWING* webots_draw_rx= (struct WEBTOS_DRAWING *)lpbase;
	memcpy((struct WEBTOS_DRAWING*)&webots_draw, webots_draw_rx, sizeof(webots_draw));
	return 1;
}

运行该函数后机器人的数据会存储在webots_draw结构体中，我们只需要按需求取出来就行，对于发送给机器人写入仿真数据为：

LPVOID lpdata_tx = NULL;//指针标识首地址
HANDLE hmap_tx;
void win_mem_tx()//向控制器写入仿真数据
{
	static int init = 0;
	if (lpdata_tx != NULL && !init)
	{
		puts("mem exist\n");
	}

	if (!init) {
		init = 1;
		hmap_tx = CreateFileMappingA(INVALID_HANDLE_VALUE, NULL,
			PAGE_READWRITE | SEC_COMMIT, 0, SIZE_MEM, "mem_control_r_draw");
	}

	if (hmap_tx == NULL)
	{
		puts("creat fail\n");
	}
	else
	{
		//映射文件到指针
		lpdata_tx = MapViewOfFile(hmap_tx, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, 0);

		memcpy(lpdata_tx, (struct WEBTOS_DRAWING_TX*)&webots_draw_tx, sizeof(webots_draw_tx));
	}
}

注：注意这里给出的是Supervisor控制器中共享内存的操作方式，即读取内存获取机器人数据，写入内存输出仿真数据，而在机器人控制程序中需要反过来，即机器人写入自己的可视化数据，读取内存获取仿真真实数据，这里一定不要搞错了；

3 Webots下四足机器人可视化框架

通过上述的介绍，参考我提供的代码对于在Webots下基本的可视化方法相信已经能描述的很清楚了，对于四足机器人的可视化来说，我们可以使用绘制多个point来可视化轨迹，通过绘制多个plane来可视化高程图，通过修改一个柱状刚体的角度和长度来可视化力矢量，当然需要说明的是很多Node的自动添加还是比较麻烦，如简单的足端轨迹或质心轨迹空间，只需要提前在World中添加好并指定DEF即可，而对于高程图这样数量大需要动态变化的部分通过代码能更好地实现。

这里说带说一下在实现高程图时我踩了很多坑，最初是使用Grid Map这样的节点之间修改每个grid的高度，但是实际测试时发现随着高程图的尺寸的增加其会越来越卡，另外不得不说的还有质心轨迹绘制开启后Webots占用的系统内存会持续增加，复位仿真环境也不能解决，导致运行一段时间后完全卡住，这一点目前还没找到如何解决，最终基于上述方法实现的简单可视化效果如下：

https://video.zhihu.com/video/1488565272353673216?player=%7B%22autoplay%22%3Afalse%2C%22shouldShowPageFullScreenButton%22%3Atrue%7D

这里贴出了Supervisor控制器的程序与World程序，机器人控制程序抱歉无法提供，因此如果需要直接使用需要自己在机器人控制程序中定义之前提到的共享内存部分，然后按实际需求来修改自己的代码，如果关注这个问题的人比较多，我后续也可能整理一个简单包涵上述可视化功能的机器人控制器例程：

链接：https://pan.baidu.com/s/1IOVytQlteQJVmhkY1aOUxQ 
提取码：tnwz