Read out actor animation skeleton node pose with plugin

edit

Hi community,

this is less a question than an answer. I was looking for a solution to output the pose of each armature bone for an animated <actor>. Possible use cases could be to get the position of the hand of a human for human robot interaction.

Since I had to dig a bit deeper into the gazebo source code, I thought I would share my findings.

Here is the core part of the actor_plugin.cc:

/////////////////////////////////////////////////
void ActorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);

  this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
          std::bind(&StickPlugin::OnUpdate, this, std::placeholders::_1)));

  // Initialize ros, if it has not already bee initialized.
  if (!ros::isInitialized())
  {
    int argc = 0;
    char **argv = NULL;
    ros::init(argc, argv, "gazebo_client",
        ros::init_options::NoSigintHandler);
  }
}

/////////////////////////////////////////////////
void ActorPlugin::OnUpdate(const common::UpdateInfo &_info)
{
  ignition::math::Pose3d pose = this->actor->WorldPose();
  ROS_INFO_STREAM("Actor world Pose: " << pose.Pos().X() << ", " << pose.Pos().Y() << ", " << pose.Pos().Z());
  auto skelAnims = this->actor->SkeletonAnimations();
  auto folding_animation = skelAnims.find("animation_name");
  if (folding_animation == skelAnims.end())
  {
    ROS_ERROR_STREAM("Skeleton animation " << "animation_name" << " not found.");
  }
  else
  {
    std::map<std::string, ignition::math::Matrix4d> node_pose = folding_animation->second->PoseAt(this->actor->ScriptTime(), true);
    for (auto const& [key, val] : node_pose)
    {
      ROS_INFO_STREAM("Pose of node " << key << " is \n" <<
            val(0,0) << " " << val(0,1) << " " <<  val(0,2) << " " <<  val(0,3) << "\n" << 
            val(1,0) << " " << val(1,1) << " " <<  val(1,2) << " " <<  val(1,3) << "\n" << 
            val(2,0) << " " << val(2,1) << " " <<  val(2,2) << " " <<  val(2,3) << "\n" << 
            val(3,0) << " " << val(3,1) << " " <<  val(3,2) << " " <<  val(3,3));
    }
  }
}

Since I used the C++17 map iteration, I add add_definitions(-std=c++17) to my CMakeList.txt

You can multiply the transformation matrices to get the pose of each joint (simple forward kinematics).

I hope this might be useful for other gazeboneers.