Your browser was unable to load all of the resources. They may have been blocked by your firewall, proxy or browser configuration.
Press Ctrl+F5 or Ctrl+Shift+R to have your browser try again.

Change the low-level reference using SetPIDCommand #7

#1

Hi Prof. Hauser,

I am not pretty clear about how to change the low-level configuration reference with SetPIDCommand function. For example, if I have two configurations:

  1. config_init is the initial robot configuration.
  2. config_goal is the new goal configuration.

My idea is to use the SetPIDCommand to let the robot move to config_goal and remain here. However, if I use this command directly into the simulation loop, the controller does not bring the robot towards config_goal but still stays at config_init for the whole duration of simulation. So is there something wrong with the way I use the SetPIDCommand?

This is the sample code.

#include <Klampt/Interface/SimulationGUI.h>

int main(int argc,const char** argv) {
  //create a world
  RobotWorld world;

  SimGUIBackend backend(&world);
  WorldSimulation& sim=backend.sim;

  //line arguments like SimTest
  if(!backend.LoadAndInitSim(argc,argv)) {
    cerr<<"Error loading simulation from command line"<<endl;
    return 1;
  }

  //pick some duration between printouts in main loop
  double dt = 0.1;
  //run the simulation

  Config config_goal;
  sim.robotControllers[0]->GetCommandedConfig(config_goal);
  config_goal[7] -= 1.0;
  std::cout<<"Goal Config: ";
  std::cout<<config_goal<<endl;

  while(sim.time < 5) {
    sim.robotControllers[0]->SetPIDCommand(config_goal);

    sim.Advance(dt);
    //update the world
    sim.UpdateModel();
    //print time, robot 0's configuration
    cout<<sim.time<<'\t'<<world.robots[0]->q<<endl;

    //Uncomment the following line to log the true state of
    //the robot to disk.
    //backend.DoStateLogging_LinearPath(0,"test_state.path");
  }
  return 0;
}
  • replies 4
  • views 76
  • likes 0
#2

This looks correct, but do you know whether the motion is indeed possible for your chosen robot? If the movement is for example a leg pushing against the environment then you will see no movement unless the actuator is strong enough to push the robot over. You may consider putting your simulation loop into a visualization to see what is happening between the commanded and actual configurations.

#3

Yeah, I have tried to see this visualization and this desired configuration should be feasible.
Take the athlete_fractal_1.xml as an example. The plots of these two configurations are as follows:

config_init
initial_config.png

config_goal
desired_config.png

Then I compare two methods to send the control command:

  1. Use sim.robotControllers[0]->SendCommand("set_q",LexicalCast(config_goal)) inside the simulation loop.
    This works to bring the robot into the desired configuration and remain there.

  2. Use sim.robotControllers[0]->SetPIDCommand(config_goal)
    I would expect the controller bring the robot to the desired configuration faster than the previous SendCommand method, but from the visualization of the simulation trajectories, the robot does not lift up one of its limbs but remains in the config_init.

Does it mean that there are some other commands I have to call first before I use this SetPIDCommand?

#4

Ah yes, I forgot that the C++ structure requires you to set up and interact with a subclass of the RobotController class.

By default, the simulator sets up the controller as a PolynomialPathController. The SetPIDCommand call is overridden by the controller, which does its own thing to follow the path.

You can either cast robotControllers[0] to a PolynomialPathController and call SetConstant, or set up a custom subclass of JointTrackingController in which you override GetDesiredState.

#5

Yes! Thank you very much for the help. I have successfully commanded the controller to behave in a way that I want. The main idea is to cast the default simulation controller into a PolynomialPathController and then use SetConstant method to adjust the new configuration reference. Some sample codes are attached here:

#include <Klampt/Interface/SimulationGUI.h>
#include "Control/PathController.h"

int main(int argc,const char** argv) {
  //create a world
  RobotWorld world;

  SimGUIBackend backend(&world);
  WorldSimulation& sim=backend.sim;

  //line arguments like SimTest
  if(!backend.LoadAndInitSim(argc,argv)) {
    cerr<<"Error loading simulation from command line"<<endl;
    return 1;
  }

  //pick some duration between printouts in main loop
  double dt = 0.1;
  //run the simulation

  Config config_goal;
  sim.robotControllers[0]->GetCommandedConfig(config_goal);
  config_goal[7] -= 1.0;

  # Introduce a PolynomialPathController here!
  PolynomialPathController NewController(*sim.world->robots[0]);
  std::shared_ptr<PolynomialPathController> NewControllerPtr = std::shared_ptr<PolynomialPathController>(&NewController);

  # Set this NewController to be the default simulated controller
  sim.SetController(0, NewControllerPtr);

  while(sim.time < 5) {

    NewControllerPtr->SetConstant(config_goal);

    //move the sim forward by the given time

    sim.Advance(dt);
    //update the world
    sim.UpdateModel();
    //print time, robot 0's configuration
    cout<<sim.time<<'\t'<<world.robots[0]->q<<endl;

    //Uncomment the following line to log the true state of
    //the robot to disk.
    backend.DoStateLogging_LinearPath(0,"test_state.path");
  }
  return 0;
}