random segments of code

May you do good and not evil. May you find forgiveness for yourself and forgive others. May you share freely, never taking more than you give! – D. Richard Hipp

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

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

Information

This entry was posted on October 10, 2011 by in Daily, iCub.
Follow

Get every new post delivered to your Inbox.

%d bloggers like this: