如何在webots中绘制运动轨迹

我们希望在webots里仿真时，机器人的运动轨迹能够在3维仿真环境中显示出来。解决办法如下：

1. 添加一个Robot节点，并将其“supervisor”域设为“true”；
2. 使用VRML语言撰写一个Shape节点，该Shape节点的几何体是IndexedLineSet，IndexedLineSet可以看作是点；
3. 在1中Robot的控制器文件中，利用supervisor获取机器人质心的坐标，并将该点的坐标添加至2中的Shape节点中，然后将Shape中的点连接起来，便得到机器人质心的运动轨迹。

1 添加Robot节点

添加一个Robot节点，将其“supervisor”域设置为true后，才能够使用在编写Robot的控制器时使用supervisor类。

2 VRML撰写一个Shape节点

#VRML_OBJ R2022a utf8
DEF trail Shape{
 geometry DEF indexLineSet IndexedLineSet{
 coord DEF coord Coordinate{
            point [ 0 0 0
                    0 0 0
                  ]
        }  
                 coordIndex[0
                            0
                            -1
                           ]
    }
}

用C++来写一个VRML文本，用于生成Shape节点。首先确定Shape中点的数目，此处设MaxCount=65000；域coordIndex的作用是将点连接起来，假设coordIndex是[0 1 -1 1 2]，其作用就是将Point域中索引为0和1的点连接起来，然后-1是分割符号，之后再将索引为1和2的点再连接起来，在仿真环境中就显示出了一条线。下面是一个用于生成Shape节点的字符串：

 const int MaxCount=65000;

 std::string mystring_1="DEF trail Shape { \n
    geometry DEF indexLineSet IndexedLineSet {\n \
    coord DEF coord Coordinate {\n \
      point [0 0 0\n";
      
  std::string mystring_2="\
      0 0 0\n \
   ";
   
  for(int i=0;i<MaxCount-2;i++)
  mystring_2=mystring_2+"0 0 0 \n \ ";
  mystring_2 = mystring_2+"]";//添加域结束符
   
  std::string mystring_3=" }\n \
    coordIndex [0 \n ";
  std::string mystring_4="\  
    0\n ";
    
  for(int i=0;i<MaxCount-2;i++)
  mystring_4=mystring_4+"0\n \ ";
  mystring_4 = mystring_4+"]";//添加域结束符
    
  std::string mystring_5="}\n \
}";
  
  std::string mystring=mystring_1+mystring_2+mystring_3+mystring_4+mystring_5;

3 编写用来绘制运动轨迹的Robot控制器

注意这里的Robot控制器是专门用来生成运动轨迹的，而不是控制机器人运动的那个控制器，也就是说，为了生成运动轨迹，额外地添加了一个Robot节点，此时左边的场景树下至少有2个Robot节点。

几个将要用到的函数：

Node* Supervisor::getRoot();//返回场景树根目录
Node * Supervisor::getFromDef(const std::string &name);//从DEF中得到节点

Field* Node::getField(const std::string &fieldName) const;//获取域

void Field::importMFNodeFromString(int position, const std::string &nodeString);//将string转换成节点导入

const double * Node::getPosition() const;//获取节点的位置坐标

void Field::setMFVec3f(int index, const double values[3]);//设置域值
void Field::setMFInt32(int index, int value);//设置域值

需要用到的头文件

#include <webots/Supervisor.hpp>
#include <webots/Node.hpp>
#include <webots/Field.hpp>
#include <string>
#include <iostream>

主要程序

  Field *root = supervisor->getRoot()->getField("children");
  root->importMFNodeFromString(-1,mystring);
  
  Node *RTARGET=supervisor->getFromDef("RTARGET");
  if(RTARGET!=NULL)std::cout<<"获取RTARGET成功\n";
  
  Field *point=supervisor->getFromDef("coord")\
  ->getField("point");
   Field *coordIndex=supervisor->getFromDef("indexLineSet")\
  ->getField("coordIndex");




  while (supervisor->step(timeStep*3) != -1) {
    //读取连杆2的质心位置
    const double* position = RTARGET->getPosition();
    std::cout<<"x is "<<position[0]<<std::endl;
    std::cout<<"y is "<<position[1]<<std::endl;  
    std::cout<<"z is "<<position[2]<<std::endl;
    std::cout<<"/******************************/ \n";
    point->setMFVec3f(pointIndex,position);
    // if(mark==0)
    // {
      // indexC++;
      // mark = 1;
    // }
    // else mark=0;
    if(pointIndex>1){//代表point Index运行了2次
        coordIndex->setMFInt32((pointIndex-2)*3+0,indexC);
        coordIndex->setMFInt32((pointIndex-2)*3+1,indexC+1);
        coordIndex->setMFInt32((pointIndex-2)*3+2,-1);
        indexC++;
      }
    pointIndex++;
    
  };

 时间比较匆忙，写得不清楚，后面有时间详细说说