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 ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {
mutex.wait();
if(stamp.getTime() == 0) {
std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
stamp.update();
}
// store the current (start) indices in the circular buffer
int tIdx = torsoIdx;
int hIdx = headIdx;
// helpers
int idxH = -1, idxT = -1;
int hH, hT;
double smallestDiffH = 10000;
double smallestDiffT = 10000;
double tDiff;
// the bottles to be copied out of the circ buffer
Bottle head, torso;
// going through the circular buffer
idxH = 1;
idxT = 1;
// for(int k = 0; k < LIST_LENGTH; k++) {
// hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;
// hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//
// tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
// std::cout << stamp.getTime() << " idx: " << hT << " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;
// if( tDiff < smallestDiffT ) {
// idxT = hT;
// smallestDiffT = tDiff;
// }
//
// tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
// if( tDiff < smallestDiffH ) {
// idxH = hH;
// smallestDiffH = tDiff;
// }
// }
if( idxH != -1 ) head = headState[idxH];
if( idxT != -1 ) torso = torsoState[idxT];
std::cout << "idxT: " << idxT << "/" << idxT-tIdx << " val: " << torso.toString() << std::endl;
std::cout << "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() << std::endl;
for(int i = 0; i < 6; i++) {
headjnt_pos[i] = head.get(i).asDouble();
if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
}
mutex.post();
}
void comBalancingThread::run()
{
// if(!Opt_display){
//updating Time Stamp
static Stamp timeStamp;
timeStamp.update();
//***************************** Reading F/T measurements and encoders ****************
#ifdef FOR_PLOTS_ONLY
Vector* F_ext_RLt = port_ft_foot_right->read(true);
Vector* F_ext_LLt = port_ft_foot_left->read(true);
if (F_ext_LL==0) F_ext_LL= new Vector(6,0.0);
if (F_ext_RL==0) F_ext_RL = new Vector(6,0.0);
(*F_ext_LL) = -1.0*((*F_ext_LLt) - (Offset_Lfoot));
(*F_ext_RL) = -1.0*((*F_ext_RLt) - (Offset_Rfoot));
//Transformation from ankle to f/t sensor
Matrix foot_hn(4,4); foot_hn.zero();
foot_hn(0,2)=1; foot_hn(0,3)=-7.75;
foot_hn(1,1)=-1;
foot_hn(2,0)=-1;
foot_hn(3,3)=1;
Vector tmp1, tmp2;
tmp1 = (*F_ext_RL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
tmp2 = (*F_ext_RL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;
for (int i=0; i<3; i++) (*F_ext_RL)[i] = tmp1[i];
for (int i=3; i<6; i++) (*F_ext_RL)[i] = tmp2[i-3];
tmp1 = (*F_ext_LL).subVector(0,2); tmp1.push_back(0.0); tmp1 = foot_hn * tmp1;
tmp2 = (*F_ext_LL).subVector(3,5); tmp2.push_back(0.0); tmp2 = foot_hn * tmp2;
for (int i=0; i<3; i++) (*F_ext_LL)[i] = tmp1[i];
for (int i=3; i<6; i++) (*F_ext_LL)[i] = tmp2[i-3];
//fprintf(stderr, "F_EXT_RL from module: %s\n", (*F_ext_RL).toString().c_str());
#else
if(Opt_ankles_sens)
{
F_ext_RL = EEWRightAnkle->read(true);
F_ext_LL = EEWLeftAnkle->read(true);
}
else{
F_ext_RL = EEWRightLeg->read(true);
F_ext_LL = EEWLeftLeg->read(true);
}
#endif
Ienc_RL->getEncoders(encs_r.data());
Ienc_LL->getEncoders(encs_l.data());
// check if the robot is not in contact with the ground
static bool on_ground = true;
//printf("%f %f \n", (*F_ext_LL)[0], (*F_ext_RL)[0]);
if ((*F_ext_LL)[0] > 50 &&
(*F_ext_RL)[0] > 50 )
{
on_ground = true;
}
else
{
on_ground = false;
for (int jj=0; jj<6; jj++) (*F_ext_LL)[jj] = (*F_ext_RL)[jj] = 1e-20;
(*F_ext_LL)[0] = (*F_ext_RL)[0] = 1e+20;
}
//printf("%s\n", (*F_ext_LL).toString().c_str());
//gif ((*F_ext_LL)[3])
//***************************** Computing ZMP ****************************************
computeZMP(&zmp_xy, F_ext_LL, F_ext_RL, encs_l, encs_r);
#ifdef VERBOSE
fprintf(stderr, "ZMP coordinates: %f %f %d \n",zmp_xy[0],zmp_xy[1],(int)(torso));
fprintf(stderr, "ZMP desired: %f %f\n",zmp_des[0], zmp_des[1]);
#endif
//*********************** SENDING ZMP COORDINATES TO GuiBalancer *********************
// if (objPort->getOutputCount() > 0)
// {
objPort->prepare() = zmp_xy;
objPort->setEnvelope(timeStamp);
objPort->write();
// }
//********************** SENDING ZMP COORDS TO BE PLOT BY ICUBGUI *****************************
if (objPort2->getOutputCount() > 0)
{
Matrix Trans_lastRot(4,4); Trans_lastRot.zero();
Trans_lastRot.setSubmatrix(rot_f,0,0);
Trans_lastRot(3,3) = 1;
Trans_lastRot(1,3) = -separation(encs_r,encs_l)/2;
Vector zmp_xy_trans(4); zmp_xy_trans.zero();
zmp_xy_trans.setSubvector(0,zmp_xy);
zmp_xy_trans(3)=1;
zmp_xy_trans = Hright*(Right_Leg->getH())*SE3inv(Trans_lastRot)*zmp_xy_trans;
objPort2->prepare() = zmp_xy_trans.subVector(0,1);
objPort2->write();
}
//*********************** Reading ZMP derivative **********************************************
iCub::ctrl::AWPolyElement el;
el.data=zmp_xy;
el.time=Time::now();
zmp_xy_vel = zmp_xy_vel_estimator->estimate(el);
//*********************** Obtaining Transformation Matrix from Root to WRF ********************
Matrix RLeg_RotTrans(4,4); RLeg_RotTrans.zero();
RLeg_RotTrans = Hright * Right_Leg->getH();
Matrix lastRotTrans(4,4);
lastRotTrans.zero();
lastRotTrans(0,2) = lastRotTrans(2,0) = lastRotTrans(3,3) = 1;
lastRotTrans(1,1) = -1;
lastRotTrans(1,3) = -1*RLeg_RotTrans(1,3);
Matrix H_r_f(4,4); H_r_f.zero();
H_r_f = RLeg_RotTrans * lastRotTrans;
Matrix H_f_r = SE3inv(H_r_f);
// fprintf(stderr, "\n Right foot H_f_r Matrix: \n %s\n", H_f_r.toString().c_str());
// Hip_from_foot->prepare() = H_f_r.getCol(3);
// Hip_from_foot->write();
//********************** CONTACT WITH HUMAN DETECTION ******************************************
/*
skinContactList *skinContacts = port_skin_contacts->read(false);
static double start_time = 0;
bool contact_detected=false;
if(skinContacts)
{
for(skinContactList::iterator it=skinContacts->begin(); it!=skinContacts->end(); it++){
if(it->getBodyPart() == RIGHT_ARM)
contact_detected=true;
}
}
if(contact_detected) //if CONTACT happened.
{
start_time = yarp::os::Time::now();
fprintf(stderr, "Switching to Torso Control Mode\n");
torso=true;
knees=false;
contacto[0]=1;
}
else{
double end_time = yarp::os::Time::now();
if((end_time - start_time)>=5.0 && torso==true){
fprintf(stderr, "Switching back to previous control strategy\n");
torso=false;
knees=true;
contacto[0]=0;
}
}
contact_sig->prepare() = contacto;
contact_sig->write();
*/
//********************** END CONTACT DETECTION ***********************************************
//********************** SUPPORT POLYGON DEFINITION ****************************
/* double bound_fw = 0.10;
double bound_bw = -0.05;
double security_offset = 0.02;
//zmp support polygon
if((zmp_xy[0]>bound_fw) || (zmp_xy[0]<bound_bw)){
if(zmp_xy[0]<bound_bw){
zmp_des[0] = bound_bw + security_offset;
}
else{
if(zmp_xy[0]>bound_fw){
zmp_des[0] = bound_fw - security_offset;
}
}
}*/
//********************** CONTROL BLOCK 1 **************************************
double delta_zmp [2];
delta_zmp [0] = zmp_xy[0] - zmp_des[0];
delta_zmp [1] = zmp_xy[1] - zmp_des[1];
double delta_zmp_vel [2];
delta_zmp_vel[0] = zmp_xy_vel[0] - 0.0;
delta_zmp_vel[1] = zmp_xy_vel[1] - 0.0;
double delta_com [2];
double delta_theta;
double delta_phi;
delta_com [0] = + Kp_zmp_x * delta_zmp[0] - Kd_zmp_x * delta_zmp_vel[0];
delta_com [1] = + Kp_zmp_y * delta_zmp[1] - Kd_zmp_y * delta_zmp_vel[1];
delta_theta = - Kp_tetha * delta_zmp[0] - Kd_tetha * delta_zmp_vel[0];
delta_phi = - Kp_phi * delta_zmp[1] - Kd_phi * delta_zmp_vel[1];
double der_part = Kd_zmp_x * delta_zmp_vel[0];
#ifdef VERBOSE
fprintf(stderr, "\n Kd*delta_zmp_vel[0] = %f , %f \n", der_part, zmp_xy_vel[0]);
#endif
//********************** CONTROL BLOCK 2 **************************************
double COM_des[2];
double phi_des;
double theta_des;
double COM_ref[2];
double phi_ref;
double theta_ref;
//From initial standing position
COM_des[0] = 0.0;
COM_des[1] = 0.0;
theta_des = 0;
phi_des = 0;
COM_ref[0] = delta_com[0] + COM_des[0];
COM_ref[1] = delta_com[1] + COM_des[1];
theta_ref = delta_theta + theta_des;
phi_ref = delta_phi + phi_des;
Vector COM_r(2); COM_r[0] = COM_ref[0]; COM_r[1] = COM_ref[1];
COM_ref_port->prepare() = COM_r;
COM_ref_port->setEnvelope(timeStamp);
COM_ref_port->write();
//********************** CONTROL BLOCK 3 **************************************
double q_torso_theta = 0.0;
double q_torso_phi = 0.0;
double q_left_ankle_theta = 0.0;
double q_left_ankle_phi = 0.0;
double q_right_ankle_theta = 0.0;
double q_right_ankle_phi = 0.0;
double length_leg = 0.47; //empirically taken this value from robot standing. COM z coord. from w.r.f.
#ifdef VERBOSE
fprintf(stderr, "COM_ref = %+6.6f ",COM_ref[0]);
#endif
q_right_ankle_theta = asin(-COM_ref[0]/length_leg)*CTRL_RAD2DEG;
//q_right_ankle_phi = -asin(-COM_ref[1]/length_leg)*CTRL_RAD2DEG;
q_right_ankle_phi = 0.0;
q_left_ankle_phi = -2.0;
// q_torso_theta = theta_ref;
q_torso_theta = 15.0;
//********************** SEND COMMANDS **************************************
// SATURATORS
//torso forward/backward
double limit = 20;
if (q_torso_theta>limit) q_torso_theta=limit;
if (q_torso_theta<-limit) q_torso_theta=-limit;
//torso right/left
if (q_torso_phi>limit) q_torso_phi=limit;
if (q_torso_phi<-limit) q_torso_phi=-limit;
//ankle backward/forward
if (q_right_ankle_theta>limit) q_right_ankle_theta=limit;
if (q_right_ankle_theta<-limit) q_right_ankle_theta=-limit;
//ankle right/left
if (q_right_ankle_phi>10) q_right_ankle_phi=10;
if (q_right_ankle_phi<-5) q_right_ankle_phi-5;
//copying movement
q_left_ankle_theta = q_right_ankle_theta;
//q_left_ankle_phi = q_right_ankle_phi; //WE NEED TO CALIBRATE AGAIN THIS JOINT BECAUSE -2 DEG FOR RIGHT LEG ARE ACTUALLY 0 FOR THE LEFT ONE.
#ifdef VERBOSE
fprintf(stderr, "q theta to be sent: %+6.6f ", q_right_ankle_theta);
fprintf(stderr, "q phi to be sent: %+6.6f ", q_right_ankle_phi);
#endif
Vector ankle_ang(2); ankle_ang.zero();
ankle_ang[0]=q_right_ankle_theta;
ankle_angle->prepare()=ankle_ang;
ankle_angle->setEnvelope(timeStamp);
ankle_angle->write();
#ifdef VERBOSE
fprintf(stderr, "q torso to be sent: %+6.6f\n", q_torso_theta);
#endif
if(!Opt_display)
{
if (on_ground)
{
Ipid_TO->setReference(2,q_torso_theta);
// Ipid_TO->setReference(1,q_torso_phi);
Ipid_RL->setReference(4,q_right_ankle_theta);
Ipid_LL->setReference(4,q_left_ankle_theta);
//open legs version
Ipid_RL->setReference(5,q_right_ankle_phi);
Ipid_LL->setReference(5,q_left_ankle_phi);
//closed leg version
//Ipid_RL->setReference(5,q_right_ankle_phi);
//Ipid_RL->setReference(1,-q_right_ankle_phi);
//Ipid_LL->setReference(5,-q_left_ankle_phi);
//Ipid_LL->setReference(1,q_left_ankle_phi);
}
}
// ********************** OLD CONTROL ******************************************
/*//TORSO CONTROL MODE
if(torso && (*sample_count)>=2){
if((zmp_xy[0]>0.05) || (zmp_xy[0]<-0.01)){
if(zmp_xy[0]<0){
zmpXd = 0;
}
else{
if(zmp_xy[0]>0){
zmpXd = 0.03;
}
}
zmpd[0] = zmpXd;
switch_ctrl = true;
}
if(switch_ctrl){
double error_zmp = zmp_xy[0]-zmpXd;
command[2]=-Kp_T*(error_zmp) + Kd_T*((zmpVel)[0]);
if(error_zmp<0.005){
switch_ctrl = false;
zmpd[0]=0;
command[2]=0;
}
}
torso_ctrl_sig->prepare() = command;
torso_ctrl_sig->write();
desired_zmp->prepare() = zmpd;
desired_zmp->write();
}
//KNEES CONTROL MODE
if(knees && (*sample_count)>=2){
if((zmp_xy[0]>0.05) || (zmp_xy[0]<-0.02)){
if(zmp_xy[0]<0){
zmpXd = 0;
}
else{
if(zmp_xy[0]>0){
zmpXd = 0.03;
}
}
zmpd[0] = zmpXd;
switch_ctrl = true;
}
if(switch_ctrl){
double error_zmp = zmp_xy[0]-zmpXd;
command_RL[3]=-Kp_K*(error_zmp) + Kd_K*((zmpVel)[0]);
command_LL[3]= command_RL[3];
if(error_zmp<0.005){
switch_ctrl = false;
zmpd[0]=0;
command_RL[3]=0;
command_LL[3]=0;
}
}
knee_ctrl_sig->prepare() = command_RL;
knee_ctrl_sig->write();
desired_zmp->prepare() = zmpd;
desired_zmp->write();
}
*/
// END OLD CONTROL
}
int main(int argc, char *argv[]) {
// initialization
// Open the network
Network yarp;
//BufferedPort<Sound> p;
Port p;
p.open("/sender");
// Get a portaudio read device.
Property conf;
conf.put("device","portaudio");
conf.put("read", "");
// conf.put("samples", rate * rec_seconds);
conf.put("samples", fixedNSample);
conf.put("rate", rate);
PolyDriver poly(conf);
IAudioGrabberSound *get;
// Make sure we can read sound
poly.view(get);
if (get==NULL) {
printf("cannot open interface");
return 1;
}
else{
printf("correctly opened the interface rate: %d, number of samples: %d, number of channels %d \n",rate, rate*rec_seconds, 2);
}
//Grab and send
Sound s;
//Bottle b;
//b.addString("hello");
//unsigned char* dataSound;
//short* dataAnalysis;
//int v1, v2;
//int i = 0, j = 0;
//NetInt16 v;
// i = sample amd j = channels;
get->startRecording(); //this is optional, the first get->getsound() will do this anyway.
Stamp ts;
while (true)
{
double t1=yarp::os::Time::now();
ts.update();
//s = p.prepare();
get->getSound(s);
//v1 = s.get(i,j);
//v = (NetInt16) v1;
//v2 = s.get(i+1,j+1);
//dataAnalysis = (short*) dataSound;
p.setEnvelope(ts);
p.write(s);
double t2=yarp::os::Time::now();
printf("acquired %f seconds \n", t2-t1);
}
get->stopRecording(); //stops recording.
return 0;
}
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]);
}
}
}
