In the function setExternalForce, the body force force_b should equal the trranspose of frame orientation times the force in the world frame, the transpose seems to be forgot.
/** set external force (expressed in the world frame) acting on the point specified by the frame
The external force is applied for a single time step only.
You have to apply the force for every time step if you want persistent force
@param[in] frame_name the name of the frame where you want to applied the force. The force is applied to the origin of the frame, on the body where the frame is attached to.
@param[in] force the applied force in the world frame/
void setExternalForce(const std::string &frame_name, const Vec<3> &force) {
auto &frame = getFrameByName(frame_name);
Vec<3> force_b; force_b = frame.orientation force;
setExternalForce(frame.parentId, Frame::BODY_FRAME, force_b, Frame::BODY_FRAME, frame.position);
}
In the function setExternalForce, the body force force_b should equal the trranspose of frame orientation times the force in the world frame, the transpose seems to be forgot.