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.

Advertisements

iCub Simulator: changing table friction properties

We’ve changed the surface characteristics of the table model which comes built-in with the ODE based iCub simulator. I’m sure there would be more elegant solutions, but this hack seems to suffice for our application.

Following code snippet should be inserted after the following line in the file (“iCub/main/src/simulators/iCubSimulation/odesdl/iCub_Sim.cpp”):

if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

Code to add:

//++ Kadir && Guner
 const dReal* pos_o1;
 const dReal* pos_o2;
 if (dGeomGetClass(o1) != dPlaneClass && dGeomGetClass(o2) != dPlaneClass)
 {
     pos_o1 = dGeomGetPosition(o1);
     pos_o2 = dGeomGetPosition(o2);
 }
 bool table_collision_point = false;
 if( (fabs(pos_o1[1]-0.5) < 0.01 && fabs(pos_o1[2]-0.45) < 0.01 && fabs(pos_o1[3]) < 0.01 ) {
    // this is the table top, change the surface mu to an appropriate value
    // so that it can be used accordingly when dCollide is called
     table_collision_point = true;
 }
 else if( (fabs(pos_o2[1]-0.5) < 0.01 && fabs(pos_o2[2]-0.45) < 0.01 && fabs(pos_o2[3])< 0.01 )
 {
     // this is the table top, change the surface mu to an appropriate value
     // so that it can be used accordingly when dCollide is called
     table_collision_point = true;
 }
 dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
 for (i=0; i contact[i].surface.mode = dContactSlip1| dContactSlip2| dContactBounce | dContactSoftCFM;
 if(table_collision_point)
    contact[i].surface.mu = 10.0;
 else
    contact[i].surface.mu = dInfinity;
 contact[i].surface.mu2 = 0;
 contact[i].surface.bounce = 0.01;
 contact[i].surface.bounce_vel = 0.01;
 contact[i].surface.slip1 = (dReal)0.000001;
 contact[i].surface.slip2 = (dReal)0.000001;
 contact[i].surface.soft_cfm = 0.0001;
 }
 //-- Kadir && Guner