webots学习记录8：R2023b如何在某个零件上添加一个恒定的力(矩)

在webots安装路径下，从include\controller\c\webots\supervisor.h中可以看到如下定义：

void wb_supervisor_node_add_force(WbNodeRef node, const double force[3], bool relative);
void wb_supervisor_node_add_force_with_offset(WbNodeRef node, const double force[3], const double offset[3], bool relative);
void wb_supervisor_node_add_torque(WbNodeRef node, const double torque[3], bool relative);

然而在include\controller\cpp\webots\Supervisor.hpp中却没有类似的定义，
在include\controller\cpp\webots\Node.hpp中才有类似定义：

void addForce(const double force[3], bool relative);
void addForceWithOffset(const double force[3], const double offset[3], bool relative);
void addTorque(const double torque[3], bool relative);

从include\controller\c\webots\types.h可以看到WbNodeRef的定义：

typedef struct WbNodeStructPrivate *WbNodeRef;

在https://github.com/cyberbotics/webots/blob/master/src/controller/c/supervisor.c中可以找到WbNodeStructPrivate的定义：

typedef struct WbNodeStructPrivate {int id;WbNodeType type;char *model_name;char *def_name;char *content;int parent_id;double *position;                                  // double[3]double *orientation;                               // double[9]double *center_of_mass;                            // double[3]WbNodeWbContactPointListStruct contact_points[2];  // 0 -> without descendants, 1 -> with descendantsbool contact_points_include_descendants;           // TODO: Delete with `wb_supervisor_node_get_contact_point`bool static_balance;double *solid_velocity;  // double[6] (linear[3] + angular[3])bool is_proto;bool is_proto_internal;  // FALSE if the node is visible in the scene tree, otherwise TRUEWbNodeRef parent_proto;int tag;WbNodeRef next;
} WbNodeStruct;

根据https://github.com/cyberbotics/webots/blob/master/docs/reference/supervisor.md中的说明：

The wb_supervisor_node_add_force function adds a force to the Solid node at its center of mass, the relative argument defines if the force is expressed in world coordinate system (relative set to false) or relatively to the node (relative set to true). The wb_supervisor_node_add_force_with_offset function adds a force to the Solid node at the location (expressed in the node coordinate system) defined by the offset argument. The wb_supervisor_node_add_torque function adds a torque to the Solid node.

注意要把Robot节点下面的Supervisor项由FALSE改为TRUE. 在某个零件上添加一个恒定的力的示例代码如下：

webots::Supervisor* robot = webots::Supervisor::getSupervisorInstance();
webots::Node* BaseNode = robot->getFromDef("Base_link_00");
const double RopeForce[3] = { 500.0, 500.0, 500 };
const double RopeForceOffset[3] = { -30.0, 0.0, 0.0 };
//BaseNode->addForce(RopeForce, false);
BaseNode->addForceWithOffset(RopeForce, RopeForceOffset, false);

下面顺便提供用代码修改Robot的translation和rotation的方法，

webots::Supervisor* robot = webots::Supervisor::getSupervisorInstance();
webots::Node* robotNode = robot->getSelf();
webots::Field* translationField = robotNode->getField("translation");
webots::Field* rotationField = robotNode->getField("rotation");
const double translationArray[3] = {-39, 0, 0.9};
translationField->setSFVec3f(translationArray);
robot->step(32);
const double rotationArray[4] = {0.0, 1.0, 0.0, pi/2.0};
rotationField->setSFRotation(rotationArray);
robot->step(32);

当然，你也可以直接调用Webots所采用的开源物理引擎ODE中的函数实现加力(矩)的效果，在webots\include\ode\ode\objects.h中，可以看到如下声明：

/*** @brief Add force at centre of mass of body in absolute coordinates.* @ingroup bodies*/
ODE_API void dBodyAddForce            (dBodyID, dReal fx, dReal fy, dReal fz);/*** @brief Add torque at centre of mass of body in absolute coordinates.* @ingroup bodies*/
ODE_API void dBodyAddTorque           (dBodyID, dReal fx, dReal fy, dReal fz);/*** @brief Add force at centre of mass of body in coordinates relative to body.* @ingroup bodies*/
ODE_API void dBodyAddRelForce         (dBodyID, dReal fx, dReal fy, dReal fz);/*** @brief Add torque at centre of mass of body in coordinates relative to body.* @ingroup bodies*/
ODE_API void dBodyAddRelTorque        (dBodyID, dReal fx, dReal fy, dReal fz);/*** @brief Add force at specified point in body in global coordinates.* @ingroup bodies*/
ODE_API void dBodyAddForceAtPos       (dBodyID, dReal fx, dReal fy, dReal fz,dReal px, dReal py, dReal pz);
/*** @brief Add force at specified point in body in local coordinates.* @ingroup bodies*/
ODE_API void dBodyAddForceAtRelPos    (dBodyID, dReal fx, dReal fy, dReal fz,dReal px, dReal py, dReal pz);
/*** @brief Add force at specified point in body in global coordinates.* @ingroup bodies*/
ODE_API void dBodyAddRelForceAtPos    (dBodyID, dReal fx, dReal fy, dReal fz,dReal px, dReal py, dReal pz);
/*** @brief Add force at specified point in body in local coordinates.* @ingroup bodies*/
ODE_API void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,dReal px, dReal py, dReal pz);

不过要注意，根据ODE官网(http://ode.org/wiki/index.php/Manual)说明，加在物体上的力在每个时间步之后都会被置0，这似乎意味着，想要在物体上加一个类似于重力的恒力，每个时间步都得调用加力函数。我还没仔细研究，以后研究清楚了再来更新文章。

Add forces to bodies (absolute or relative coordinates). The forces are accumulated on to each body, and the accumulators are zeroed after each time step.

The ...RelForce and ...RelTorque functions take force vectors that are relative to the body's own frame of reference.

The ...ForceAtPos and ...ForceAtRelPos functions take an extra position vector (in global or body-relative coordinates respectively) that specifies the point at which the force is applied. All other functions apply the force at the center of mass.

还有一招，Help > How do I apply a force or a torque to an object? 这样加上去的力似乎也只在一个时间步生效，立刻就会消失。