bool updateModule()
{
Vector *imuData=iPort.read();
if (imuData==NULL)
return false;
Stamp stamp;
iPort.getEnvelope(stamp);
double t0=Time::now();
Vector gyro=imuData->subVector(6,8);
Vector gyro_filt=gyroFilt.filt(gyro);
gyro-=gyroBias;
gyro_filt-=gyroBias;
Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));
adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
double dt=Time::now()-t0;
if (oPort.getOutputCount()>0)
{
Vector &outData=oPort.prepare();
outData=*imuData;
outData.setSubvector(6,gyro);
oPort.setEnvelope(stamp);
oPort.write();
}
if (bPort.getOutputCount()>0)
{
bPort.prepare()=gyroBias;
bPort.setEnvelope(stamp);
bPort.write();
}
if (verbose)
{
yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
yInfo("gyro = %s",gyro.toString(3,3).c_str());
yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
yInfo("dt = %.0f [us]",dt*1e6);
yInfo("\n");
}
return true;
}
bool updateModule()
{
LockGuard lg(mutex);
double dumpTime=Time::now();
Bottle &bDump=dumpPort.prepare();
bDump.clear();
bDump.addString(actionTag);
bDump.addString(objectTag);
opc.checkout();
// agent body + position
agentName = getPartnerName();
if (Entity *e=opc.getEntity(agentName))
{
if (Agent *agent=dynamic_cast<Agent*>(e))
{
bDump.addList()=agent->m_body.asBottle();
Bottle &bAgent=bDump.addList();
bAgent.addString(agent->name());
bAgent.addDouble(agent->m_ego_position[0]);
bAgent.addDouble(agent->m_ego_position[1]);
bAgent.addDouble(agent->m_ego_position[2]);
bAgent.addInt(agent->m_present==1.0?1:0);
}
}
// objects position
list<Entity*> lEntity=opc.EntitiesCache();
for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++)
{
string entityName=(*itEnt)->name();
string entityType=(*itEnt)->entity_type();
if (entityType==EFAA_OPC_ENTITY_OBJECT)
{
if (Object *object=dynamic_cast<Object*>(*itEnt))
{
Bottle &bObject=bDump.addList();
bObject.addString(object->name());
bObject.addDouble(object->m_ego_position[0]);
bObject.addDouble(object->m_ego_position[1]);
bObject.addDouble(object->m_ego_position[2]);
bObject.addInt(object->m_present==1.0?1:0);
}
}
}
bDump.addInt(gate);
dumpStamp.update(dumpTime);
dumpPort.setEnvelope(dumpStamp);
dumpPort.writeStrict();
yInfo()<<bDump.toString();
return true;
}
void sendContacts(BufferedPort<skinContactList> &outPort, const skinContactList &sCL)
{
ts.update();
skinContactList &sCLout = outPort.prepare();
sCLout.clear();
sCLout = sCL;
outPort.setEnvelope(ts);
outPort.write();
}
virtual void run(){
tmp = command;
(*command)[0] = -60*(gsl_rng_uniform(r));
(*command)[1] = 100*(gsl_rng_uniform(r));
(*command)[2] = -35 + 95*(gsl_rng_uniform(r));
(*command)[3] = 10 + 90*(gsl_rng_uniform(r));
printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]);
//above 0 doesn't seem to be safe for joint 0
if ((*command)[0] > 0 || (*command)[0] < -60){
(*command)[0] = (*tmp)[0];
}
if ((*command)[1] > 100 || (*command)[1] < -0){
(*command)[1] = (*tmp)[1];
}
if ((*command)[2] > 60 || (*command)[2] < -35){
(*command)[2] = (*tmp)[2];
}
if ((*command)[3] > 100 || (*command)[3] < 10){
(*command)[3] = (*tmp)[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add((*command)[i]);
}
armPlan->write(plan);
armPred->read(pred);
Vector commandCart(3);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
printf("Cartesian safety check\n");
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command->data());
bool done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
printf("Moved arm to new location\n");
Vector &armJ = armLocJ->prepare();
Vector encoders(nj);
enc->getEncoders(encoders.data());
armJ = encoders;
Vector noisyArm(3);
for(int i = 0; i < 3; i++){
//noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1);
//sanity check
noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1);
}
//insert here:
//read off peak saliences
//fixate there
//calculate cartesian value, compare to real cart. value of arm
printf("Looking at arm\n");
igaze->lookAtFixationPoint(noisyArm);
done = false;
while(!done){
igaze->checkMotionDone(&done);
Time::delay(0.5);
}
//igaze->waitMotionDone(0.1,30);
printf("Saw arm\n");
Vector &headAng = headLoc->prepare();
igaze->getAngles(headAng);
Bottle tStamp;
tStamp.clear();
tStamp.add(Time::now());
headLoc->setEnvelope(tStamp);
headLoc->write();
armLocJ->write();
headLoc->unprepare();
armLocJ->unprepare();
}
else{
printf("Self collision detected!\n");
}
}
virtual void run()
{
int i_touching_left=0;
int i_touching_right=0;
int i_touching_diff=0;
info.update();
skinContactList *skinContacts = port_skin_contacts->read(false);
if(skinContacts)
{
for(skinContactList::iterator it=skinContacts->begin(); it!=skinContacts->end(); it++){
if(it->getBodyPart() == LEFT_ARM)
i_touching_left += it->getActiveTaxels();
else if(it->getBodyPart() == RIGHT_ARM)
i_touching_right += it->getActiveTaxels();
}
}
i_touching_diff=i_touching_left-i_touching_right;
if (abs(i_touching_diff)<5)
{
fprintf(stdout,"nothing!\n");
}
else
if (i_touching_left>i_touching_right)
{
fprintf(stdout,"Touching left arm! \n");
if (!left_arm_master) change_master();
}
else
if (i_touching_right>i_touching_left)
{
fprintf(stdout,"Touching right arm! \n");
if (left_arm_master) change_master();
}
if (left_arm_master)
{
robot->ienc[LEFT_ARM] ->getEncoders(encoders_master);
robot->ienc[RIGHT_ARM]->getEncoders(encoders_slave);
if (port_left_arm->getOutputCount()>0)
{
port_left_arm->prepare()= Vector(16,encoders_master);
port_left_arm->setEnvelope(info);
port_left_arm->write();
}
if (port_right_arm->getOutputCount()>0)
{
port_right_arm->prepare()= Vector(16,encoders_slave);
port_right_arm->setEnvelope(info);
port_right_arm->write();
}
// robot->ipos[RIGHT_ARM] ->positionMove(3,encoders_master[3]);
for (int i=jjj; i<5; i++)
{
robot->ipid[RIGHT_ARM]->setReference(i,encoders_master[i]);
}
}
else
{
robot->ienc[RIGHT_ARM]->getEncoders(encoders_master);
robot->ienc[LEFT_ARM] ->getEncoders(encoders_slave);
for (int i=jjj; i<5; i++)
{
robot->ipid[LEFT_ARM]->setReference(i,encoders_master[i]);
}
}
}
