Create a name to index map for Robots

[costashatz]

We should create a mapping between names and indices of robots' joints/dofs for being able to query their states with the names instead of just indices.

[PedroDesRobots]

We can implement this function like we did :

std::map<std::string, int> bridge::dart_robot_loader::getIndexMap() {
  int numjoints = dRobot_->getNumJoints();
  std::vector<dart::dynamics::Joint*> joints;
  for (int i = 0; i < numjoints; ++i) {
    joints.push_back(dRobot_->getJoint(i));
    index_map_[joints[i]->getName()] = i;
  }
  return index_map_;
}

Then something like that :

int getIndex(std::string jointName){
  return index_map_(jointName);
}

[costashatz]

@PedroDesRobots thanks for the code.. One thing to be careful is the fact that 1 joint does not mean one DoF. Many joints have zero-DoFs and one joint can have multiple-DoFs. We should discuss how to handle this. How do you see this? Another important thing is the mimic joints. I'll try to come up with a plan. If you have any ideas, let me know..

[PedroDesRobots]

Yes sorry for the confusion. In fact, getForceTorque(joint_index) uses auto jt = _skeleton->getJoint(joint_index). So it's joint related and not DoF.

In terms of physical quantities, we are always looking at the joint level. DoFs are more useful for geometrical aspects/ kinematics.

Another important thing is the mimic joints

What do you mean by mimic joints?

[costashatz]

What do you mean by mimic joints?

http://wiki.ros.org/urdf/XML/joint#Elements (search for ).

Joints that move according to another joint. Sometimes you have robots that have hardware mimicing (e.g. NAO humanoid), while in other cases it is quite useful for defining gripper joints (e.g. Talos humanoid).

These need extra caring while controlling them, because they appear as DoFs while you can only control one of them in practice (e.g., open/close gripper).

[dalinel]

So maybe we could have one joint map for the controllable joints/dofs with some naming convention for the MultiDofs case and mimic joint case ?

And maybe another joint map with the non controllable and zero-DoFs ?

[costashatz]

So maybe we could have one joint map for the controllable joints/dofs with some naming convention for the MultiDofs case and mimic joint case ?

And maybe another joint map with the non controllable and zero-DoFs ?

For me the mapping makes sense only for the controllable dofs. There are the ones that you will pass to a control framework or any other piece of code. For the non-controllable joints, what would you want to do? Query their coordinate frame or their parents? You can do that already by name; you don't need their index.

[dalinel]

For the non-controllable joints, what would you want to do? Query their coordinate frame or their parents? You can do that already by name; you don't need their index.

Yes this was it so no need of the joint map for non controllable joint

[costashatz]

@dalinel do you want/have time to do this as a PR?

[dalinel]

@costashatz Ok I will do this

[dalinel]

For the mapping , I think that in the end it is better to focus on a dof_map than a joint_map For me the mapping makes sense from a control perspective for functions such as :

• update
• positions
• set_positions
• velocities
• set_velocities
• accelerations
• set_accelerations
• forces
• set_forces

Then the user would be able to give a list of dofs and set/get/update only the desired list For example we can have something like this for the position here :

std::map<std::string, size_t> Robot::dof_map()
 {
     std::map<std::string, size_t> map;
     for (size_t i = 0; i < _skeleton->getNumDofs(); ++i) {
          map[_skeleton->getDof(i)->getName()] = i;
      }
      return map;
}

 void Robot::set_positions(const Eigen::VectorXd& positions, const std::vector<std::string>& dof_names)
 {
      if(!dof_names.empty()){
          Eigen::VectorXd ordered_pos =  Robot::positions();
          std::map<std::string, size_t> map = dof_map();
          for (size_t i = 0; i < dof_names.size(); i++) {
              ordered_pos(map[dof_names[i]]) = positions(i);
          }
           _skeleton->setPositions(ordered_pos);
      }
      else{
           _skeleton->setPositions(positions);
      }
 }

Then the user would need some other function utils such as size_t joint_index(std::string joint_name) as mentioned by @PedroDesRobots here and so maybe also other functions such as size_t body_index(std::string body_name) size_t dof_index(std::string dof_name)

But I don't think it is necessary to have a body_map or joint_map

What do you think about this ?

[costashatz]

I think that in the end it is better to focus on a dof_map than a joint_map

Indeed! I completely agree with you..

void Robot::set_positions(const Eigen::VectorXd& positions, const std::vector& dof_names)

I am not sure if we need this kind of functions. But if you do them, make sure to do it for all the elements (positions, velocities, accelerations, ...).

size_t joint_index(std::string joint_name) as mentioned by @PedroDesRobots here and so maybe also other functions such as size_t body_index(std::string body_name) size_t dof_index(std::string dof_name)

We can have these functions yes.

But I don't think it is necessary to have a body_map or joint_map

Yes I do not think that we need these...

[dalinel]

I am not sure if we need this kind of functions. But if you do them, make sure to do it for all the elements (positions, velocities, accelerations, ...).

Some use can be if you need to reset the robot or specific part of the robot with specific positions, velocities, accelerations, etc ...

Or if you want to control a graphical robot where no physics apply For example to have a graphical robot object which perfectly follows the position command

Yes I do not think that we need these...

I will focus on a dof_map then !

[costashatz]

Some use can be if you need to reset the robot or specific part of the robot with specific positions, velocities, accelerations, etc ...

Or if you want to control a graphical robot where no physics apply For example to have a graphical robot object which perfectly follows the position command

Good. We can have those functions; they will hurt the performance in any way.

I will focus on a dof_map then !

Good!

[dalinel]

I was looking at the mimic joint case I have seen that you have already done some work in robot_control.cpp with Eigen::VectorXd RobotControl::_set_vector_mimic(const Eigen::VectorXd& vec) const And then in the update fonction you are using the reordered command You also have functions such as set_positions , get_positions

I was going to handle the dof_map in robot.cpp but it does not make any sense to have some order handling both in robot_control.cpp and robot.cpp

What do you think about moving those mimic functionalities in robot.cpp? Or do you prefer to extend the robot_control.cpp class with dof_map functionalities ?

I am just not sure of how constraining it is for a user to have this in robot_control.cpp (For example for a user who doesn't want to use a controller class abstraction)

[costashatz]

What do you think about moving those mimic functionalities in robot.cpp? Or do you prefer to extend the robot_control.cpp class with dof_map functionalities ?

I am just not sure of how constraining it is for a user to have this in robot_control.cpp (For example for a user who doesn't want to use a controller class abstraction)

We should move this functionalities in the Robot class. We are not sure yet if we are going to keep RobotControl and even if we are, we still want someone to be able to use the Robot class independently.

Thanks!

[costashatz]

@dalinel any news on this one?

[dalinel]

I have made a pull request for a first version You have the choice to get/set positions, velocities, accelerations, forces and commands either : -with a vector of dof names (with an assert if mimic) -by filtering mimic dofs as in robot_control.cpp -with no filtering and no ordering

Tell me if I need to improve/add functionalities @costashatz @PedroDesRobots