//******************************************** bool close() { yInfo("[demoAvoidance] Closing module.."); dataPort.close(); if (useLeftArm) { if (driverCartL.isValid()) { data["left"].iarm->stopControl(); data["left"].iarm->restoreContext(contextL); driverCartL.close(); } if (driverJointL.isValid()) { IInteractionMode *imode; driverJointL.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointL.close(); } } if (useRightArm) { if (driverCartR.isValid()) { data["right"].iarm->stopControl(); data["right"].iarm->restoreContext(contextR); driverCartR.close(); } if (driverJointR.isValid()) { IInteractionMode *imode; driverJointR.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointR.close(); } } return true; }
//******************************************** bool close() { dataPort.close(); if (driverCartL.isValid()) { data["left"].iarm->stopControl(); data["left"].iarm->restoreContext(contextL); driverCartL.close(); } if (driverCartR.isValid()) { data["right"].iarm->stopControl(); data["right"].iarm->restoreContext(contextR); driverCartR.close(); } if (driverJointL.isValid()) { IInteractionMode *imode; driverJointL.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointL.close(); } if (driverJointR.isValid()) { IInteractionMode *imode; driverJointR.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointR.close(); } return true; }
bool close() { // Terminate model Controller_terminate(Controller_M); imod->setControlMode(joint,VOCAB_CM_POSITION); if (compliant) iint->setInteractionMode(joint,VOCAB_IM_STIFF); rpc.close(); dataOut.close(); dataIn.close(); drv.close(); return true; }
bool partMover::entry_update(partMover *currentPart) { GdkColor color_black; GdkColor color_grey; GdkColor color_yellow; GdkColor color_green; GdkColor color_green_blue; GdkColor color_dark_green; GdkColor color_red; GdkColor color_fault_red; GdkColor color_pink; GdkColor color_indaco; GdkColor color_white; GdkColor color_blue; color_pink.red=219*255; color_pink.green=166*255; color_pink.blue=171*255; color_fault_red.red=255*255; color_fault_red.green=10*255; color_fault_red.blue=10*255; color_black.red=10*255; color_black.green=10*255; color_black.blue=10*255; color_red.red=255*255; color_red.green=100*255; color_red.blue=100*255; color_grey.red=220*255; color_grey.green=220*255; color_grey.blue=220*255; color_white.red=250*255; color_white.green=250*255; color_white.blue=250*255; color_green.red=149*255; color_green.green=221*255; color_green.blue=186*255; color_dark_green.red=(149-30)*255; color_dark_green.green=(221-30)*255; color_dark_green.blue=(186-30)*255; color_blue.red=150*255; color_blue.green=190*255; color_blue.blue=255*255; color_green_blue.red=(149+150)/2*255; color_green_blue.green=(221+190)/2*255; color_green_blue.blue=(186+255)/2*255; color_indaco.red=220*255; color_indaco.green=190*255; color_indaco.blue=220*255; color_yellow.red=249*255; color_yellow.green=236*255; color_yellow.blue=141*255; GdkColor* pColor= &color_grey; static int slowSwitcher = 0; IControlMode *ictrl = currentPart->ctrlmode2; IInteractionMode *iint = currentPart->iinteract; IPositionControl *ipos = currentPart->pos; IVelocityControl *ivel = currentPart->iVel; IPositionDirect *iDir = currentPart->iDir; IEncoders *iiencs = currentPart->iencs; ITorqueControl *itrq = currentPart->trq; IAmplifierControl *iamp = currentPart->amp; GtkEntry * *pos_entry = (GtkEntry **) currentPart->currPosArray; GtkEntry **trq_entry = (GtkEntry **) currentPart->currTrqArray; GtkEntry **speed_entry = (GtkEntry **) currentPart->currSpeedArray; GtkEntry **inEntry = (GtkEntry **) currentPart->inPosArray; GtkWidget **colorback = (GtkWidget **) currentPart->frameColorBack; GtkWidget **sliderAry = currentPart->sliderArray; bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE; char buffer[40] = {'i', 'n', 'i', 't'}; char frame_title [255]; double positions[MAX_NUMBER_OF_JOINTS]; double torques[MAX_NUMBER_OF_JOINTS]; double speeds[MAX_NUMBER_OF_JOINTS]; double max_torques[MAX_NUMBER_OF_JOINTS]; double min_torques[MAX_NUMBER_OF_JOINTS]; static int controlModes[MAX_NUMBER_OF_JOINTS]; static int controlModesOld[MAX_NUMBER_OF_JOINTS]; static yarp::dev::InteractionModeEnum interactionModes[MAX_NUMBER_OF_JOINTS]; static yarp::dev::InteractionModeEnum interactionModesOld[MAX_NUMBER_OF_JOINTS]; int k; int NUMBER_OF_JOINTS=0; bool done = false; bool ret = false; ipos->getAxes(&NUMBER_OF_JOINTS); if (NUMBER_OF_JOINTS == 0) { fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" ); Time::delay(0.1); pColor=&color_grey; strcpy(frame_title,"DISCONNECTED"); for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++) { if (currentPart->framesArray[k]!=0) { gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); } } return true; } for (k = 0; k < NUMBER_OF_JOINTS; k++) { max_torques[k]=0; min_torques[k]=0; torques[k]=0; } if (!iiencs->getEncoders(positions)) return true; itrq->getTorques(torques); iiencs->getEncoderSpeeds(speeds); //update all joints positions for (k = 0; k < NUMBER_OF_JOINTS; k++) { sprintf(buffer, "%.1f", positions[k]); gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer); sprintf(buffer, "%.3f", torques[k]); gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer); sprintf(buffer, "%.1f", speeds[k]); gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer); } //update all joint sliders for (k = 0; k < NUMBER_OF_JOINTS; k++) if(POS_UPDATE[k]) gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]); // *** update the checkMotionDone box section *** // (only one at a time in order to save badwidth) k = slowSwitcher%NUMBER_OF_JOINTS; slowSwitcher++; #if DEBUG_GUI gtk_entry_set_text((GtkEntry*) inEntry[k], "off"); #else ipos->checkMotionDone(k, &done); if (!done) gtk_entry_set_text((GtkEntry*) inEntry[k], " "); else gtk_entry_set_text((GtkEntry*) inEntry[k], "@"); #endif // *** update the controlMode section *** // the new icubinterface does not increase the bandwidth consumption // ret = true; useless guys! ret=ictrl->getControlModes(controlModes); if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" ); ret=iint->getInteractionModes(interactionModes); if (ret==false) fprintf(stderr,"iint->getInteractionlMode failed\n" ); for (k = 0; k < NUMBER_OF_JOINTS; k++) { if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue; controlModesOld[k]=controlModes[k]; sprintf(frame_title,"Joint %d ",k ); switch (controlModes[k]) { case VOCAB_CM_IDLE: pColor=&color_yellow; strcat(frame_title," (IDLE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_POSITION: pColor=&color_green; strcat(frame_title," (POSITION)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_POSITION_DIRECT: pColor=&color_dark_green; strcat(frame_title," (POSITION_DIRECT)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_MIXED: pColor=&color_green_blue; strcat(frame_title," (MIXED_MODE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_VELOCITY: pColor=&color_blue; strcat(frame_title," (VELOCITY)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_TORQUE: pColor=&color_pink; strcat(frame_title," (TORQUE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_IMPEDANCE_POS: pColor=&color_indaco; strcat(frame_title," (IMPEDANCE POS)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_IMPEDANCE_VEL: pColor=&color_indaco; strcat(frame_title," (IMPEDANCE VEL)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_OPENLOOP: pColor=&color_white; strcat(frame_title," (OPENLOOP)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_HW_FAULT: pColor=&color_fault_red; strcat(frame_title," (HARDWARE_FAULT)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"---"); gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---"); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_CALIBRATING: pColor=&color_grey; strcat(frame_title," (CALIBRATING)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_CALIB_DONE: pColor=&color_grey; strcat(frame_title," (CALIB DONE)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_NOT_CONFIGURED: pColor=&color_grey; strcat(frame_title," (NOT CONFIGURED)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; case VOCAB_CM_CONFIGURED: pColor=&color_grey; strcat(frame_title," (CONFIGURED)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; default: case VOCAB_CM_UNKNOWN: pColor=&color_grey; strcat(frame_title," (UNKNOWN)"); gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title); gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor); break; } } for (k = 0; k < NUMBER_OF_JOINTS; k++) { if (currentPart->first_time==false && interactionModes[k] == interactionModesOld[k]) continue; interactionModesOld[k]=interactionModes[k]; switch (interactionModes[k]) { case VOCAB_IM_STIFF: gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_green); break; case VOCAB_IM_COMPLIANT: gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_fault_red); break; default: case VOCAB_CM_UNKNOWN: gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_white); break; } } currentPart->first_time =false; return true; }
//******************************************** bool configure(ResourceFinder &rf) { if (rf.check("noLeftArm") && rf.check("noRightArm")) { yError("[demoAvoidance] no arm has been selected. Closing."); return false; } useLeftArm = false; useRightArm = false; string name=rf.check("name",Value("avoidance")).asString().c_str(); string robot=rf.check("robot",Value("icub")).asString().c_str(); motionGain = -1.0; if (rf.check("catching")) { motionGain = 1.0; } if (motionGain!=-1.0) { yWarning("[demoAvoidance] motionGain set to catching"); } bool autoConnect=rf.check("autoConnect"); if (autoConnect) { yWarning("[demoAvoidance] Autoconnect mode set to ON"); } bool stiff=rf.check("stiff"); if (stiff) { yInfo("[demoAvoidance] Stiff Mode enabled."); } Matrix R(4,4); R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0; if (!rf.check("noLeftArm")) { useLeftArm=true; data["left"]=Data(); data["left"].home_x[0]=-0.30; data["left"].home_x[1]=-0.20; data["left"].home_x[2]=+0.05; data["left"].home_o=dcm2axis(R); Property optionCartL; optionCartL.put("device","cartesiancontrollerclient"); optionCartL.put("remote","/"+robot+"/cartesianController/left_arm"); optionCartL.put("local",("/"+name+"/cart/left_arm").c_str()); if (!driverCartL.open(optionCartL)) { close(); return false; } Property optionJointL; optionJointL.put("device","remote_controlboard"); optionJointL.put("remote","/"+robot+"/left_arm"); optionJointL.put("local",("/"+name+"/joint/left_arm").c_str()); if (!driverJointL.open(optionJointL)) { close(); return false; } driverCartL.view(data["left"].iarm); data["left"].iarm->storeContext(&contextL); Vector dof; data["left"].iarm->getDOF(dof); dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0; data["left"].iarm->setDOF(dof,dof); data["left"].iarm->setTrajTime(0.9); data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o); data["left"].iarm->waitMotionDone(); IInteractionMode *imode; driverJointL.view(imode); IImpedanceControl *iimp; driverJointL.view(iimp); if (!stiff) { imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03); imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03); imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03); imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01); imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0); } } if (!rf.check("noRightArm")) { useRightArm = true; data["right"]=Data(); data["right"].home_x[0]=-0.30; data["right"].home_x[1]=+0.20; data["right"].home_x[2]=+0.05; data["right"].home_o=dcm2axis(R); Property optionCartR; optionCartR.put("device","cartesiancontrollerclient"); optionCartR.put("remote","/"+robot+"/cartesianController/right_arm"); optionCartR.put("local",("/"+name+"/cart/right_arm").c_str()); if (!driverCartR.open(optionCartR)) { close(); return false; } Property optionJointR; optionJointR.put("device","remote_controlboard"); optionJointR.put("remote","/"+robot+"/right_arm"); optionJointR.put("local",("/"+name+"/joint/right_arm").c_str()); if (!driverJointR.open(optionJointR)) { close(); return false; } driverCartR.view(data["right"].iarm); data["right"].iarm->storeContext(&contextR); Vector dof; data["right"].iarm->getDOF(dof); dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0; data["right"].iarm->setDOF(dof,dof); data["right"].iarm->setTrajTime(0.9); data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o); data["right"].iarm->waitMotionDone(); IInteractionMode *imode; driverJointR.view(imode); IImpedanceControl *iimp; driverJointR.view(iimp); if (!stiff) { imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03); imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03); imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03); imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01); imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0); } } dataPort.open(("/"+name+"/data:i").c_str()); dataPort.setReader(*this); if (autoConnect) { Network::connect("/visuoTactileRF/skin_events:o",("/"+name+"/data:i").c_str()); } rpcSrvr.open(("/"+name+"/rpc:i").c_str()); attach(rpcSrvr); return true; }
bool configure(ResourceFinder &rf) { Time::turboBoost(); string name=rf.check("name",Value("yarpMinJerk")).asString().c_str(); string robot=rf.check("robot",Value("icubSim")).asString().c_str(); string part=rf.check("part",Value("left_arm")).asString().c_str(); joint=rf.check("joint",Value(3)).asInt(); compliant=rf.check("compliant"); Property option; option.put("device","remote_controlboard"); option.put("remote",("/"+robot+"/"+part).c_str()); option.put("local",("/"+name+"/"+part).c_str()); if (!drv.open(option)) return false; drv.view(imod); drv.view(iint); drv.view(ienc); drv.view(ivel); imod->setControlMode(joint,VOCAB_CM_VELOCITY); if (compliant) iint->setInteractionMode(joint,VOCAB_IM_COMPLIANT); IControlLimits *ilim; drv.view(ilim); double min_joint,max_joint; ilim->getLimits(joint,&min_joint,&max_joint); double enc; while (!ienc->getEncoder(joint,&enc)) Time::delay(0.1); Controller_P.Plant_IC=enc; Controller_P.Plant_Max=max_joint; Controller_P.Plant_Min=min_joint; Controller_P.AutoCompensator_ThresHystMax=0.5; Controller_P.AutoCompensator_ThresHystMin=0.1; yInfo("enc=%g in [%g, %g] deg",enc,min_joint,max_joint); // Pack model data into RTM Controller_M->ModelData.defaultParam = &Controller_P; Controller_M->ModelData.blockIO = &Controller_B; Controller_M->ModelData.dwork = &Controller_DW; // Initialize model Controller_initialize(Controller_M, &Controller_U_reference, &Controller_U_compensator_state, &Controller_U_plant_output, &Controller_Y_controller_output, &Controller_Y_controller_reference, &Controller_Y_plant_reference, &Controller_Y_error_statistics, &Controller_Y_enable_compensation); Controller_U_reference=enc; Controller_U_compensator_state=CompensatorState::Off; Controller_U_plant_output=enc; dataIn.open(("/"+name+"/data:i").c_str()); dataOut.open(("/"+name+"/data:o").c_str()); rpc.open(("/"+name+"/rpc").c_str()); attach(rpc); return true; }