How to calculate the six MTM's axises angular velocities?

[alexunder]

Inside the source code of qladisp, I noticed one interface of AmpIO

AmpIO_Int32 AmpIO::GetEncoderVelocity(unsigned int index)

But how can I calculate the final angular velocity from this encoder velocity? I hope the unit would be "rad per second". I have read some comments and documents, but I still can't get the answer. Please give me some descriptions and hints, thanks very much!

[pkazanzides]

GetEncoderVelocity returns the encoder period (time measured between two consecutive edges of the same type) -- it was a poor choice of name. I recommend to instead call GetEncoderVelocityCountsPerSecond which, as the name implies, returns the velocity in counts/second. To get rad/sec, you need to know how many counts/rad. For the dVRK, you can find this in the XML configuration files, in the Encoder section, specifically the BitsToPosSI scale.

[alexunder]

Thanks for your careful explanation. Based on your comments, I found another field in xml configuration file is more fit for my scenario:

<Encoder>
        <BitsToPosSI Scale="-0.00141933"/>
        <BitsToDeltaPosSI Offset="0" Scale="-4360.20"/>
        <BitsToDeltaT Offset="0" Scale="-1.000"/>
        <CountsPerTurn Value="4000"/>
</Encoder>

The value of CountsPerTurn is more reasonable from the name: one turn is 2*M_PI, so 4000/(2*M_PI) would be (count/rad).

If I use BitsToPosSI , it means that the seven or eight motors have different rotating directions, isn't it? Can you explain more about the relationship between CountsPerTurn and BitsToPosSI ?

[pkazanzides]

Using CountsPerTurn will give you the rotation of the motor/encoder, but not of the joint because there is also gearing. BitsToPosSI also takes into account the gear ratio. I believe the signs are to match the convention for positive rotation.

I assume you are asking these questions because you want low-level access to the hardware; otherwise, it is better to use the processed values (in SI units, rather than raw "bits" or "counts") available via the C++ software or via ROS topics.

[pkazanzides]

Also, I saw a comment from you that you cannot use GetEncoderVelocityCountsPerSecond because you have Firmware Version 5. You will get more accurate velocity measurements and be able to use GetEncoderVelocityCountsPerSecond if you upgrade to Version 6. That is easy to do -- see https://github.com/jhu-cisst/mechatronics-firmware/wiki/FPGA-Program

[alexunder]

Your information is very useful, thanks! @pkazanzides

The reason why I want to use low level APIs is that we only use mechatronics-software library at present. Actually my objection is to get the joint(axises) speed in (radian per second) accurately, if there are some other more convenient solutions to get them, please tell me, like something you said about processed values or SI units , sort of.

BTW: What's means of SI? I occasionally saw them in dvrk codes.

[pkazanzides]

Here is a wikipedia entry about SI units: https://en.wikipedia.org/wiki/International_System_of_Units. For angular velocity, radians/second is the SI unit.

[alexunder]

Thanks for your information. I have copied the version 6's velocity calculating algorithm from the repo sawRobotIO1394

inline double getVelocityV6() const {
     double velocity = 0.0;
     const double vel_term = mEncoderVelocityCountsPerSecond;
     const double acc_term = mEncoderAccelerationCountsPerSecSec * mEncoderVelocityDelay;
     // Don't decelerate past a zero-crossing; first expression checks whether vel_term and
     // acc_term have different signs (i.e., one positive and one negative).
     if ((vel_term * acc_term < 0.0) && (abs(acc_term) > abs(vel_term))) {
         velocity = 0.0;
     } else {
         velocity = mBitsToPosSI * (vel_term + acc_term);
      }
      return velocity;
  }

Then I let the one axis of them(For example, A6) do sine movements. The shape of the Velocity is reasonable, but the amplitude of the velocity curve is little large, it ranges from -200 to 200:

I think this amplitude is not very reasonable to a velocity in Radian per second. What do you think of my calculation? Do you think whether it's ok? @pkazanzides

[pkazanzides]

The algorithm looks fine, but there are two things that I have questions about:

1. Velocity is a vector (based on number of joints). In our code, variables such as mEncoderVelocityCountsPerSecond are vectors, not scalars. Perhaps you have redefined them to be scalars elsewhere in your code and your getVelocityV6 only computes velocity for a single (hard-coded) joint. Otherwise, perhaps the compiler is doing something unexpected (for example, mEncoderAccelerationCountsPerSecSec * mEncoderVelocityDelay will compute a dot product if both terms are vectors).
2. Have you checked that you have the correct values for all the conversion constants?

[alexunder]

@pkazanzides Sorry to bother you.

I think my velocity calculations have problems. The higher level software you mentioned before is sawIntuitiveResearchKit and sawRobotIO1394 ? I think I should do some tests on these software to compare with my calculations. I think only sawRobotIO1394 is ok?

[pkazanzides]

You could use just sawRobotIO1394, but if you do not use sawIntuitiveResearchKit you would need to write an application that periodically sends motor currents and receives feedback.

[alexunder]

@pkazanzides We found that the encoder velocities from Axis 2 and Axis 3 are strong related, sometimes one is still, and move the other, they have almost the same velocity curve, this behavior will lead some other calculations. Can you tell me the relationship between MTM a2 and a3?

[pkazanzides]

I am not aware of any relationship between MTM axis 2 and axis 3. Are you using the full dVRK software and testing with the Qt GUI?

[adeguet1]

You are likely looking at the actuator velocities while moving the joints. The actuator to joint coupling matrices can be found in the dVRK sawRobotIO1394 configuration files: https://github.com/jhu-dvrk/sawIntuitiveResearchKit/blob/master/share/jhu-dVRK/sawRobotIO1394-MTML-22723.xml#L136 You can see the 3rd row has a -1.0 and 1.0 hence the two matching velocities you're seeing.

If you plan to write your own software instead of using the current dVRK stack, I would strongly recommend to look at the current stack first to understand what you might need to (re)implement. You can start with the high level description of the architecture in: https://github.com/jhu-dvrk/sawIntuitiveResearchKit/wiki/Software-Architecture. Please note that the code comes as-is and has limited documentation so you will have to dig in the code to understand what it does.

[alexunder]

@pkazanzides We are using mechatronics-software as a library to get the MTM's joint velocities, then using Jacobean algorithm to calculate the MTM's clip velocity in Cartesian space, finally map the clip veclocity to our own PSM's clip. We found the calculated MTM clip velocity is not ideal, therefore we analyzed the scenario. Thanks for your help @adeguet1 , we will figure out your information.

[alexunder]

close this issue.

[ming1259]

Omg, This problem is so hard for me!