How to use iCub velocity control

[guiolpei]

Hello,

I am a master student working on the iCub at Munich ICS (the robot is iCubMunich01 with upgrades) and we are trying to control the robot using the velocity commands, through the 'IVelocityControl' class.

The joints of the robot can be set to velocity control using the 'IControlMode' class or manually in the 'yarpmotorgui' control panel, but it doesn't seem to respond to the 'velocityMove' commands nor we can modify the velocity value using the slider in the 'yarpmotorgui'.

We have noticed that the 'Velocity PID' controller values are not defined, and we can't modify them using the PID window from the 'yarpmotorgui':

When the desired values are changed and sent, current values won't update. However, the values of the 'Position PID' controller can be changed without any problems.

We have found the file where the parameters are defined, 'icub_right_arm.xml'. There are PID controllers defined for position and torque but not for velocity.

Is defining values in this 'xml' file the solution to be able to change the values of the PID controller, or the problem is that the joints can't be velocity-controlled?

Regards,
Guillermo

icub_right_arm.txt

[pattacini]

Hi @guiolpei welcome to our community 👋

Let's leave the GUI aside for a moment.

Could you post on Gist the relevant portion of the code you're using to control the robot in velocity? This way we can inspect, run and comment on it.

What I suspect is that you're not aware of the safety timeout of 100 ms (if I'm not mistaken) that is used underneath the velocity control to avoid integrating indefinitely the command in case e.g. there's a network drop or a similar critical event.

With this in mind, you have to keep sending velocity commands (even the same command as a sort of heartbeat) at a proper rate to sustain the control mechanism and prevent timeout expiration.

At the low level, the velocity commands are integrated to yield position setpoints; thereby, the relevant PID gains are still those of the position domain. In this respect, I don't know actually what are pure velocity PID gains used for. Perhaps, they're effective only for those devices where a native low-level velocity control can be performed (@randaz81 might be of help here).

[guiolpei]

Thank you for your quick response, Ugo.

The code we are using is here:

https://gist.github.com/guiolpei/383cc7f24a5c8f233b5d90ed9719458a

The control is done through another class that acts as a wrapper to the 'IControlMode' class.

This code works correctly on the iCub simulator but fails to work on the real robot. In the simulator, the joint moves at 5 deg/s until the 0 velocity command is sent. The GUI also shows the correct value for the velocity.

We weren't aware that there was a safety timeout of 100 ms, so probably this is the problem...

I understand then that velocity PID is not needed as you said, and as it is depicted in the following figure:

[pattacini]

Yep, the diagram is correct 👍

In the following piece of your main

iCubRightArmCtrl->velocityMove(3, 5.0);
iCubRightArmCtrl->velocityMove(3, 0);

you're basically commading a movement in velocity and soon afterward you stop it.

A typical velocity control loop resembles these snippets instead:

1. To drive the joint toward setpoint:

   while (abs(error)>threshold) {
       error=setpoint-feedback;
       double velocity=Kp*error;
       ivelocity->velocityMove(joint,velocity);
       yarp::os::Time::delay(0.01);    // just some way to enforce the control rate
   }

2. To make the joint move at constant speed for T seconds:

   double t0=yarp::os::Time::now();
   while (yarp::os::Time::now()-t0<T) {
       ivelocity->velocityMove(joint,velocity);
       yarp::os::Time::yield();    // release the quantum and prevent CPU spinning
   }
   ivelocity->stop(joint);

[hamaduddin]

i am unable to control velocity in icub by using velocityMove in python further unable to understand where velocity change effect can be seen in icub object or its eye velocity that is being affected.
this is my snippet of code :

headOptions = yarp.Property()
        headOptions.put("device","remote_controlboard")
        headOptions.put("local","/client/head")
        headOptions.put("remote","/icubSim/head")
        headOptions.put("local","/controller")
        #  create remote driver
        headDriver = yarp.PolyDriver(headOptions)
        # iMode = headDriver.viewIControlMode()
        headPos = headDriver.viewIPositionControl()
        iMode = headDriver.viewIControlMode()
        headVel = headDriver.viewIVelocityControl()
        headEncs = headDriver.viewIEncoders()
        joints=headPos.getAxes()

further these setPosition Mode and setVelocityMode commands give attribute error in trying to implement them

      for j in range(joints):
            # iMode.setPositionMode(j)
            # iMode.setPositionMode(j)
            # iMode.setVelocityMode(j)

Kindly help in both issues i will be very thankful to you.

[pattacini]

Hi @hamaduddin

I'm not a Python user, so I don't think I can really help you with the code. You'd better search for other discussions for that or open up a new one.

That said, a few comments:

• You just need one local, whereas you defined two headOptions.put("local"...).
• If you want to control in velocity the head of the robot, you ought to resort to the Python equivalent of IControlMode::setControlMode().

[hamaduddin]

ok Sir noted with thanks.