iCub simple safety check for end-effector cartesian coordinates

One simple idea to check if the end-effector (hand) collides to robot’s own body is checking the cartesian coordinate of the end effector if it stays inside the bounding box that represents robot’s workspace.

Well, this -obviously- neglects the obstacles in the configuration space, in addition to neglecting the self-collision checking of the arms, but still it helps us to understand if robot is trying to do something stupid (hitting itself in the face for instance ).

In order to do this, I have inserted a function inside “iCub/main/src/libraries/icubmod/cartesianController/ServerCartesianController.cpp” so that the generated velocity is checked before being sent (inside “executingTraj” before “ctrl->isInTarget()” if statements ).

//++Kadir
/************************************************************************/
bool ServerCartesianController::isVelocitySafe(const yarp::sig::Vector &v)
{
    bool safe = true;

    if(kinPart == "arm")
    {
        // get current joint positions (feedback)
        Vector q_cur = fb;

        // find intermediate joint angles
        // -constant acceleration assumed
        // -control board is assumed to be run 1000 times more frequent than this thread
        Vector q_inter = q_cur + v*getRate()/1000; 

        // find end effector position via fwdKinematics
        Vector x_inter = chain->EndEffPose(q_inter);

        //for(int i=0;i<x_inter.size();i++)
        //    printf("%f ", x_inter[i]);
        //printf("\n");

        if(kinType=="left")
        {
            safe= 
                (x_inter[0]<-0.15) && (x_inter[0]>-0.5) && 
                (x_inter[1]<0.05) && (x_inter[1]>-0.4) && 
                (x_inter[2]<0.3) && (x_inter[2]>-0.10);
            if(!safe)
                printf( "!WARNING: joint vals for left arm are not safe\n");
        }
        else if(kinType=="right")
        {
            safe= 
                (x_inter[0]<-0.15) && (x_inter[0]>-0.5) && 
                (x_inter[1]>-0.05) && (x_inter[1]<0.4) && 
                (x_inter[2]<0.3) && (x_inter[2]>-0.10);
            if(!safe)
                printf( "!WARNING: joint vals for right arm are not safe\n");
        }
        else
            printf( "Wrong end effector type, try LEFT or RIGHT\n");
    }
    return safe;
}
//--Kadir

This function is then called inside “run” function:

//++Kadir
//check if the iterated joint velocities are safe.
// --send them if it is safe, otherwise stop the limb
if(isVelocitySafe(ctrl->get_qdot()))
    // send joints velocities to the robot [deg/s]
  sendVelocity(CTRL_RAD2DEG*ctrl->get_qdot());
else
  stopLimbVel();
//--Kadir

Moduler Cartesian solver/controller diagram

This safety check takes the role just before sending the q_dot joint velocities by evaluating if the corresponding velocity request takes the arm outside the predefined workspace. In that case, velocity values zeroed.

About these ads