Server Example: Wheeled Robot Force Control

Overview

Loads a wheeled robot (SMB/megabot) and applies wheel forces in force-control mode. Use it to see a basic wheeled robot setup.

Screenshot

Binary

CMake target and executable name: wheeled_robot_force_control.

Run

Build and run from your build directory:

cmake --build . --target wheeled_robot_force_control
./wheeled_robot_force_control

On Windows, run wheeled_robot_force_control.exe instead. This example uses RaisimServer. Start a visualizer client (RaisimUnity, RaisimUnreal, or the rayrai TCP viewer) and connect to port 8080.

Details

• Loads the SMB wheeled robot and applies constant wheel torques.

• Uses FORCE_AND_TORQUE control mode with semi-implicit integration.

• Focuses the camera on the robot for rollout.

Source

// This file is part of RaiSim. You must obtain a valid license from RaiSim Tech
// Inc. prior to usage.

#include "raisim/RaisimServer.hpp"
#include "raisim/World.hpp"
#include "rayrai_tcp_viewer_hint.hpp"

int main(int argc, char* argv[]) {
  auto binaryPath = raisim::Path::setFromArgv(argv[0]);

  raisim::World world;
  world.setTimeStep(0.002);

/// create raisim objects
  auto ground = world.addGround();

  auto robot = world.addArticulatedSystem(binaryPath.getDirectory() + "/rsc/megabot/smb.urdf");

  robot->setName("smb");
  Eigen::VectorXd gc(robot->getGeneralizedCoordinateDim()), gv(robot->getDOF());
  gc.setZero(); gv.setZero();
//  raisim::Vec<4> quat; quat = {0, 0.0499792, 0, 0.9987503}; quat/= quat.norm();
  gc.segment<7>(0) << 0, 0, 0.197, 1, 0, 0, 0;

  robot->setGeneralizedCoordinate(gc);
  robot->setGeneralizedVelocity(gv);
  robot->setGeneralizedForce({0, 0, 0, 0, 0, 0, 30, 30, 30, 30});
  robot->setIntegrationScheme(raisim::ArticulatedSystem::IntegrationScheme::SEMI_IMPLICIT);
  robot->setControlMode(raisim::ControlMode::FORCE_AND_TORQUE);

  /// launch raisim server
  raisim::RaisimServer server(&world);
  server.launchServer();
  raisim_examples::warnIfNoClientConnected(server);
  server.focusOn(robot);

  for (int i=0; i<20000000; i++) {
    RS_TIMED_LOOP(int(world.getTimeStep()*1e6))
    server.integrateWorldThreadSafe();
  }

  server.killServer();
}