Get Jacobian - Transmission - Other from EEHal interface

[torydebra]

Requirments list https://github.com/ADVRHumanoids/ROSEndEffectors-ModelAndControl/issues/98

I report here the things related to this issue:

• Jacobian matrix related to the fingertips
• Transmission matrix
• Actuator Stiffness matrix
• Physical limits, actuator torque limits, and fingertip force limits
• Friction coefficient

[torydebra]

Steps:

HAL SIDE

• New virtual functions in EEHAL with "default" implementation return false
• In each concrete HAL, the user should implement these functions because they are specific for the hand.
• XBot2Hal Special case: The jacobian should be available here from robot model, hence this hal can implement the getJacobian.
  • One can derive XBot2Hal (instead EEHAL directly) if he wants to use xbot in the generic way (ie without device-adapter hyerarchy)
• HeriHal will implement this new methods . Probably it will parse these matrices from yaml file. Maybe the Jacobian will be calculated from the code?

ROSEE Side

main node in the init will call all the new gets: option 1) a ROS server for each matrix: if the gets return false the specific ros server will not be created option 2) (For now I prefer this) a unique ROS server for all these matrices: the client can ask a specific one by filling the request. If a specific matrix is not provided by hal (false return in the get) the response will say "no matrix :C"

[torydebra]

A question: the info available in the urdf (joint limit, motor limit effort) should be 1) arrive to ROSEE main from hal 2) taken from the eeinterace/parser that is already in ROSEE main?

[torydebra]

My (final for now) solution: Matrices are in the config file:

################## Specific info for rosee planner ############################
hand_info:
  #The order here will be the order followed by the other info
  fingers_names: [finger_1, finger_2, finger_3, thumb]
  motors_names: [motor_finger1, motor_finger2, motor_finger3, motor_thumb]
  #
  tips_jacobians: #a [n_joints x 6] matrix for each fingertips (ie contact point)
    finger_1:
      - [532.80990646, 0.0, 342.49522219, 1, 2, 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2, 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
    finger_2:
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
    finger_3:
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
    thumb:
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
      - [0.0, 0.0, 1.0, 1, 2, 3]
      - [532.80990646, 0.0, 342.49522219, 1 , 2 , 3]
      - [0.0, 532.93344713, 233.88792491, 1, 2, 3]
  #
  transmission_matrix: #[n_joints x n_motors]
    - [532.80990646, 0.0, 342.49522219, 1]
    - [0.0, 532.93344713, 233.88792491, 1]
    - [0.0, 0.0, 1.0, 1, 2, 3]
    - [532.80990646, 0.0, 342.49522219, 1 ]
    - [0.0, 532.93344713, 233.88792491, 1]
    - [0.0, 0.0, 1.0, 1]
    - [532.80990646, 0.0, 342.49522219, 1 ]
    - [0.0, 532.93344713, 233.88792491, 1]
    - [0.0, 0.0, 1.0, 1]
    - [532.80990646, 0.0, 342.49522219, 1]
    - [0.0, 532.93344713, 233.88792491, 1]
  #
  motors_stiffness_diagonal: [1, 2, 3, 4]
  tips_frictions: [1, 1, 1, 1]
  tips_force_limits: [1, 1, 1, 1]
  # I know this below can be in the urdf... but EEHal for now does not have access to urdf
  motors_torque_limits: [1, 1, 1, 1]

These are parsed by EEHal base class, since the parsing is not really specific to a hand. If the node "hand_info" is present in the yaml, it will be parsed and a ros service initialized (single one, so I chose option 2). This service is called by ROSEE main node, which simply create another service(update below). We have decided this to keep ROSEE main node centered, so the grasp planner comunicate only with ROSEE and not also with EEHAL (so again option 2 of the previous comment).

Hence, the planner will call the "/ros_end_effector/hand_info" service to receive this message:

#all the subsequent info will follow the order specified in motors_names and fingertips_names
string[] fingers_names
string[] motors_names

std_msgs/Float32MultiArray[] tips_jacobians 
std_msgs/Float32MultiArray transmission_matrix
float32[] motors_stiffness_diagonal

float32[] tips_frictions
float32[] tips_force_limits
float32[] motors_torque_limits

update I had problem with this method: because sometimes the ros service on the hal is not ready when rosee main calls it. Hence I modified the code putting the call to the hal service directly in the rosee service callback (that is executed when someone external ask for the service). The call in the callback is not blocking, if the hal service is not active, it returns false:

bool ROSEE::RosServiceHandler::handInfoCallback(
        rosee_msg::HandInfo::Request& request,
        rosee_msg::HandInfo::Response& response) {

    //empty request for now

    if (! ros::service::exists("/EEHalExecutor/hand_info", false) ) {
        return false;
    }

    ros::ServiceClient handInfoClient = 
        _nh->serviceClient<rosee_msg::HandInfo>("/EEHalExecutor/hand_info");
    rosee_msg::HandInfo handInfoMsg;
    if (handInfoClient.call(handInfoMsg)) {

        response = handInfoMsg.response;

    } else {
        return false;
    }

    return true;
}

[lia2790]

How did you compute it?

[torydebra]

How did you compute it?

Numbers in the example are random

[lia2790]

Sorry I did not see the answer.

So how will we proceed?

I am close to testing the part of the planner where this information is required.

[liesrock]

@torydebra when you can update us here on your work on the above. Thanks a lot!

[torydebra]

I have some problems with my brain making some smoke. As discussed, we want

1. to keep workspace finder as it is to be general for any ee defined by urdf.
2. Add a new repo iit-heri-ii-model , specific for Heri, which will compute the jacobians, trasmission matrix and fingertip forces.

We have the workspace finder that emit in a file the fingertip position, for some motor positions. The workspace alone can not give fingertips forces, because jacobian and transmission are necessary, and these are specific of the Heri. Hence, the model should give the forces. SO: The workspace emitted file should be parsed by the heri II model class, which must give the fingertip forces for each workspace point, using the jacobian and the transmission. BUT: In this way, ROSEE planner will read the workspace from a file, but will receive the forces associated to each point of the workspace from a ROS service. Is this good?

[torydebra]

Possible solutions (in update)

1. Go in that direction (previous message) Cons

   • forces and workspace coming from different sources
   • Theoretically, tip forces computation is an offline work that could be computed once, instead we would do this every time we launch the HAL (which load the model and send the ROS service)
   • Workspace finder should emit also the finger joint positions of each point. This because the model can not find joint position from only tip pose and motor pose (not a real con, they are only some other vectors)

2. Workspace finder inside Model class, to make point and tip forces coming through ROS service togheter Cons

   • We do even more computation that could have be done offiline (the worspace finder)
   • We sent more data through ROS services

3. Workspace calling the model class, and emitting the tip forces togheter with workspace point. So, all offline, all things from a yaml file, no tremendous amount of data through ROS services Cons

   • Workspace lost in generality, or at least we must add an argument to its main to tell to compute or not also the forces specifically for the Heri.

[liesrock]

HI @torydebra, trying to simplify the above, the best solution in my perspective is actually the first one since we can embedded the worskpace finder in the planner (both generic) and keep the specific model of the actual hand we are using to compute the tip forces.

[torydebra]

Ok, thanks Luca

About the model, what I am doing is a class where the motor AND joint finger positions must be set. From these, there are all the math magic inside. Hence it would no be really a "model" because it does not "move" the fingers according to actuator commands

[torydebra]

The way I would go is: Premixes Planner, at the beginning, needs the workspace points (of the tips) + fingertips forces in each point. Then it selects one best point for each tip. Then, it needs the tip jacobian (one tip jacobian for each fingertip) and the transmission matrix relative to the pose formed by the selected points (plus the other not-dependant-on-motor-position info which are simple to provide from a file).

How I would go

• Planner read all workspace points (each workspace point has : tip's position frame, motor position, finger's joint position (the joint of the finger which the tip belongs to) ).
• For each point of the workspace, Planner calls the ROS service HandInfo with as request the motor position and finger joint position associated with this workspace point (this request must be added in the service). The ROS service response will contain the fingertip forces (which Planner needs at this this step), plus jacobian and transmission (which planner does not need at this step), plus fixed not-dependant-on-motor-position info
  • We can decide to provide different services for each different step, to provide only the necessary info at a specific step.
• Planner does its magic and select the best fingertip pose (one point for each finger). At this point, it also wants the tip jacobian and the transmission matrix for the particular hand pose chosen. Either it already have it, or it will request it through another ROS service. Similarly, Planner will request for the not-dependant-on-motor-position info.

Do you see any flaws in this? @liesrock @lia2790

---

UPDATE I explained myself better

[liesrock]

@torydebra thanks a lot for the above: something is not clear for me so I believe it's better to discuss this asap in a call. Then we can updated this issue.

[lia2790]

I think I will wait for your update since as far as I see it is more related to ROSEE than the planner

[torydebra]

@torydebra thanks a lot for the above: something is not clear for me so I believe it's better to discuss this asap in a call. Then we can updated this issue.

Yes I re-read and the comment itself had a lot of flaws, sorry :D . I rewrote some part but obviously we can have more convenient call.

[torydebra]

I should have completed this feature. EEHal (the base class) parse all the info that are accessible from a YAML file, i.e. the ones not dependant on robot state (like motor torque, stiffness and so on). Specific concrete HALs should implement their logic to provide the info dependant on robot state. From the external, the hand_info service will provide motor_state, motor_reference, and finger joints positions, that can be used by the HALs.

I implemented a specific hal for heri, (in iit_heri_ii_model repo) that will use the yifang calculations to provide jacobians, transmissions and fingertip forces depending on the provided motor_state, motor_reference, and finger joints positions.

Obviously we can discuss this in the next meeting

[torydebra]

I made a scheme to show the workflow

[torydebra]

Ok, if you are all ok with it, I will close this. Specific fixes can be commented in the relative repositories/other issues

[liesrock]

Great job! Yes please close