Log the robot state by `realRobot` returns null values

[wyqsnddd]

Dear all!

I tried to log the robot states by the real robot: return realRobots().robot("panda_foot"); and the QP robot: return robots().robot("panda_foot");.

However, the joint states readings from realRobots().robot("panda_foot") are null. For instance, the following one:

auto & robot = realRobots().robot("panda_foot")
return rbd::paramToVector(robot.mb(), robot.mbc().alpha);

Could anyone help me with this please?

I launched the controller via the following configuration:

MainRobot: PandaFoot
Enabled: [PandaPushWall]
Default: [PandaPushWall]
Timestep: 0.001

LogPolicy: threaded # Avoid slowdowns due to disk flush

# Franka specific configuration
Franka:
  ControlMode: Velocity # Can be: Position/Velocity/Torque
  # ControlMode: Position # Can be: Position/Velocity/Torque
  panda_default: # Name of the robot in the controller
    ip: 172.16.1.7 # IP of the robot
  panda_2: # Name of an extra panda in the controller
    ip: 172.16.1.6
  # Actuated robots that are not controlled via mc_franka
  ignored: [env/door, env/box]

[gergondet]

Hi @wyqsnddd

You don't have a panda_foot section in your Franka configuration so I'm guessing nothing gets passed to your robot instance (and is related to your issue in https://gite.lirmm.fr/yuquan/pandapushwallcontroller/-/issues/1)

[wyqsnddd]

Hi Pierre!

Adding the panda_foot section solved the connection issue. However, my logEntry added by:

    logEntryNames_.emplace_back(getRobot().name() + "_" + prefix + "_Joint_Velocity");
    logger().addLogEntry(logEntryNames_.back(), [&robot]() {
      return rbd::paramToVector(robot.mb(), robot.mbc().alpha);
    });

still only works for the simulated QP robot: robots().robot("panda_foot"). If I use realRobots().robot("panda_foot"), it only logs null values, for instance:

I noticed the entry panda_foot_jointvel_measured_0 works. Hence, digged into mc_panda and found the following snippets:

mc_rbdyn::DevicePtr Robot::clone() const
{
  if(robot_)
  { 
    mc_rtc::log::error_and_throw<std::runtime_error>("Cannot clone a connected Robot");
  }
  auto device = new Robot();
  device->state_ = state_;
  return mc_rbdyn::DevicePtr(device);
}

  // Measured joint velocity
  logger.addLogEntry(prefix + "_jointvel_measured", [this]() -> const std::array<double, 7> & { return state_.dq; });

Is this the way to obtain the Measured values than realRobot().robot("...")?

[gergondet]

Hi Yuquan,

robots().robot("panda_foot").encoderVelocities() has the data you're looking for.

With a simple observer pipeline setup, as below, you will also get these values from realRobots().mbc().alpha

ObserverPipelines:
- name: MainPipeline
  observers:
  - type: Encoder
    config:
      position: encoderValues
      velocity: encoderValues

[wyqsnddd]

Hi Pierre!

Thanks for the reply. Yes, now the joint velocities are there. But, we need a small change:

      velocity: encoderVelocities

Whereas, the joint torques and accelerations are still not set. Hence, I'll use the mc_panda::Robot (Child of mc_rbdyn::device) to access the real robot states:

mc_panda::Robot * robotPtr = mc_panda::Robot::get(robots().robot(defaultRobotName_));

[wyqsnddd]

One more side-effect: The observer pipeline: velocity: encoderVelocities messes with the body velocity computation:

robot.mbc().bodyVelW[bodyIndex];

Returns:

[gergondet]

Hi @wyqsnddd

This seems like a bug, can you send the log file you're showing here? It's hard to tell from the figure which values are erroneous.

[wyqsnddd]

Hi Pierre!

You can check it here: https://seafile.lirmm.fr/f/9d2e444098b5476eb7b4/

Please let me know if it does not work. I generated it with two steps:

1. The pipeline:

- name: MainPipeline
    observers:
    - type: Encoder
      config:
        position: encoderValues
        velocity: encoderVelocities

2. log:

logEntryNames_.emplace_back(robot.name() + "_" + prefix +  "_" + body.name() + "_World_Velocity");
     logger().addLogEntry(logEntryNames_.back(), [&robot,bodyIndex]() {
             return robot.mbc().bodyVelW[bodyIndex];
             });

[gergondet]

Hi Yuquan,

Looking at the logs, the values in World_Velocity entries seem ok. However the values in body_acceleration entries are not which is normal as we don't call robot().forwardAcceleration() by default