int main(int argc, char *argv[]){
Network yarp;
Port armPlan;
Port armPred;
BufferedPort<Vector> objAngles;
BufferedPort<Vector> armOut;
armPlan.open("/learnedReach/plan");
armPred.open("/learnedReach/pred");
objAngles.open("/learnedReach/loc:i");
armOut.open("/learnedReach/arm:o");
bool fwCvOn = 0;
fwCvOn = Network::connect("/learnedReach/plan","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/learnedReach/pred");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
return -1;
}
Property params;
params.fromCommand(argc,argv);
if (!params.check("robot")){
fprintf(stderr, "Please specify robot name\n");
fprintf(stderr, "e.g. --robot icub\n");
return -1;
}
std::string robotName = params.find("robot").asString().c_str();
std::string remotePorts = "/";
remotePorts += robotName;
remotePorts += "/";
if (params.check("arm")){
remotePorts += params.find("arm").asString().c_str();
}
else{
remotePorts += "left";
}
remotePorts += "_arm";
std::string localPorts = "/learnedReach/cmd";
if(!params.check("map")){
fprintf(stderr, "Please specify learned visuomotor map file\n");
fprintf(stderr, "e.g. --map map.dat\n");
return -1;
}
string fName = params.find("map").asString().c_str();
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
PolyDriver robotDevice(options);
if (!robotDevice.isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IEncoders *enc;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return 0;
}
int nj = 0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector commandCart;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
commandCart.resize(3);
for (int i = 0; i < nj; i++) {
tmp[i] = 25.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nj; i++) {
tmp[i] = 20.0;
pos->setRefSpeed(i, tmp[i]);
}
command = 0;
//set the arm joints to "middle" values
command[0] = -45;
command[1] = 45;
command[2] = 0;
command[3] = 45;
pos->positionMove(command.data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
//not really yaw and pitch
int azMin = -80; int azMax = 0;
int elMin = -60; int elMax = 0;
int verMin = 0; int verMax = 20;
int Y; int P; int V;
int mmapSize; int usedJoints;
//read in first lines to get map dimensions
string line;
string buf;
ifstream mapFile(fName.c_str());
if(mapFile.is_open()){
getline(mapFile,line);
stringstream ss(line);
ss >> buf;
Y = atoi(buf.c_str());
ss >> buf;
P = atoi(buf.c_str());
ss >> buf;
V = atoi(buf.c_str());
ss.clear();
getline(mapFile,line);
ss.str(line);
ss >> buf;
mmapSize = atoi(buf.c_str());
ss >> buf;
usedJoints = atoi(buf.c_str());
}
bool configure(ResourceFinder &rf)
{
std::string robotname = rf.check("robot",yarp::os::Value("icubSim")).asString();
Property options;
options.put("robot", robotname); // typically from the command line.
options.put("device", "remote_controlboard");
string s("/");
s += robotname;
s += "/right_arm/keyBabbling";
options.put("local", s.c_str());
s.clear();
s += "/";
s += robotname;
s += "/right_arm";
options.put("remote", s.c_str());
if (!dd.open(options)) {
cout << "Device not available. Here are the known devices:\n"<< endl;
cout << Drivers::factory().toString().c_str() << endl;;
return false;
}
portStreamer.open("/keyboardBabbling/stream:o");
portFromCart.open("/keyboardBabbling/stream:i");
portRpc.open("/keyboarBabbling/rpc");
attach(portRpc);
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote", "/" + robotname +"/cartesianController/right_arm");
optionCart.put("local","/keyboardBabbling/cartesian_client/right_arm");
if (!driverCart.open(optionCart))
return false;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(armCtrlModeUsed);
ok &= dd.view(armImpUsed);
ok &= driverCart.view(armCart);
ok &= dd.view(ilimRight);
iCubFinger* fingerRight = new iCubFinger("right_index");
deque<IControlLimits*> limRight;
limRight.push_back(ilimRight);
fingerRight->alignJointsBounds(limRight);
if (!ok) {
cout << "Device not able to acquire views" << endl;
dd.close();
return false;
}
int jnts = 0;
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
vector<bool> mask;
mask.resize(jnts);
mask[0] = false;
mask[1] = false;
mask[2] = false;
mask[3] = false;
mask[4] = true;
mask[5] = false;
mask[6] = true;
mask[7] = false;
mask[8] = true;
mask[9] = true;
mask[10] = true;
mask[11] = true;
mask[12] = true;
mask[13] = true;
mask[14] = true;
mask[15] = true;
cout << "Closing the hand" << endl;
// closing the hand:
pos->positionMove(4, 30.0);
pos->positionMove(6, 10.0);
pos->positionMove(8, 10.0);
pos->positionMove(9, 17.0);
pos->positionMove(10, 170.0);
pos->positionMove(11, 0.0);
pos->positionMove(12, 0.0);
pos->positionMove(13, 90.0);
pos->positionMove(14, 180.0);
pos->positionMove(15, 180.0);
cout << "Waiting to be in initial position...";
bool motionDone=false;
while (!motionDone)
{
motionDone=true;
for (int i=0; i<jnts; i++)
{
if (mask[i])
{
bool jntMotionDone = false;
pos->checkMotionDone(i, &jntMotionDone);
motionDone &= jntMotionDone;
}
}
Time::yield(); // to avoid killing cpu
}
cout << "ok" << endl;
// wait for the hand to be in initial position
Matrix R=zeros(3,3);
R(0,0)=-1.0; R(1,1)=1; R(2,2)=-1.0;
orientation=dcm2axis(R);
// enable torso movements as well
// in order to enlarge the workspace
Vector dof;
armCart->getDOF(dof);
dof=1.0; dof[1]=0.0; // every dof but the torso roll
armCart->setDOF(dof,dof);
armCart->setTrajTime(1.0);
Vector initPos(3,0.0);
if (rf.check("rightHandInitial"))
{
Bottle *botPos = rf.find("rightHandInitial").asList();
initPos[0] = botPos->get(0).asDouble();
initPos[1] = botPos->get(1).asDouble();
initPos[2] = botPos->get(2).asDouble();
cout<<"Reaching initial position with right hand"<<initPos.toString(3,3).c_str()<<endl;
armCart->goToPoseSync(initPos,orientation);
armCart->waitMotionDone(0.1,3.0);
}
else
{
cout << "Cannot find the inital position" << endl << "closing module" << endl;
return false;
}
minY = rf.find("min").asDouble();
maxY = rf.find("max").asDouble();
gap = rf.find("gap").asDouble();
delay = rf.find("delay").asDouble();
thrMove = rf.find("threshold_move").asDouble();
yDivisions = rf.check("yDivisions",Value(10)).asInt();
Bottle* bsequenceToPlay = rf.find("sequence").asList();
if (bsequenceToPlay)
{
cout << "Using sequence, not random." << endl;
for (int i = 0; i < bsequenceToPlay->size(); i++)
sequenceToPlay.push_back(bsequenceToPlay->get(i).asInt());
indexInSequence = 0;
}
tempPos=initPos;
state=up;
forward = false;
timeBeginIdle = Time::now();
Rand::init();
return true;
}
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 bool threadInit(){
if(!handleParams()){
return false;
}
armPlan = new Port;
armPred = new Port;
armLocJ = new BufferedPort<Vector>;
headLoc = new BufferedPort<Vector>;
armPlan->open("/babbleTrack/plan:o");
armPred->open("/babbleTrack/pred:i");
armLocJ->open("/babbleTrack/arm:o");
headLoc->open("/babbleTrack/head:o");
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc(T);
igaze = NULL;
Property options;
options.put("device","gazecontrollerclient");
options.put("remote","/iKinGazeCtrl");
options.put("local","/client/gaze");
clientGazeCtrl = new PolyDriver;
clientGazeCtrl->open(options);
options.clear();
string localPorts = "/babbleTrack/cmd";
string remotePorts = "/" + robotName + "/" + arm + "_arm";
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
robotDevice = new PolyDriver;
robotDevice->open(options);
if(clientGazeCtrl->isValid()){
clientGazeCtrl->view(igaze);
}
else{
return false;
}
if (!robotDevice->isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
bool ok;
ok = robotDevice->view(pos);
ok = ok && robotDevice->view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return false;
}
pos->getAxes(&nj);
command = new Vector;
tmp = new Vector;
command->resize(nj);
tmp->resize(nj);
for (int i = 0; i < nj; i++) {
(*tmp)[i] = 25.0;
}
pos->setRefAccelerations(tmp->data());
for (int i = 0; i < nj; i++) {
(*tmp)[i] = 20.0;
pos->setRefSpeed(i, (*tmp)[i]);
}
*command = 0;
//set the arm joints to "middle" values
(*command)[0] = -45;
(*command)[1] = 45;
(*command)[2] = 0;
(*command)[3] = 45;
//flex hand
(*command)[4] = 60;
(*command)[7] = 20;
(*command)[10] = 15;
(*command)[11] = 15;
(*command)[12] = 15;
(*command)[13] = 15;
(*command)[14] = 15;
(*command)[15] = 15;
pos->positionMove(command->data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
bool fwCvOn = 0;
fwCvOn = Network::connect("/babbleTrack/plan:o","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/babbleTrack/pred:i");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
return false;
}
return true;
}
int main(int argc, char **argv)
{
Network yarp;
printf("Going to stress rpc connections to the robot\n");
printf("Run as --id unique-id\n");
printf("Optionally:\n");
printf("--part robot-part\n");
printf("--prot protocol\n");
printf("--time dt (seconds)\n");
printf("--robot name \n");
Property parameters;
parameters.fromCommand(argc, argv);
ConstString part=parameters.find("part").asString();
int id=parameters.find("id").asInt();
double time=0;
if (parameters.check("time"))
{
time=parameters.find("time").asDouble();
}
else
time=-1;
ConstString protocol;
if (parameters.check("prot"))
{
protocol=parameters.find("prot").asString();
}
else
protocol="tcp";
ConstString rname;
if (parameters.check("robot"))
{
rname=parameters.find("robot").asString();
}
else
rname="controlboard";
PolyDriver dd;
Property p;
Random::seed((unsigned int)Time::now());
string remote=string("/")+rname.c_str();
if (part!="")
{
remote+=string("/");
remote+=part;
}
string local=string("/")+string(rname.c_str());
if (part!="")
{
local+=string("/");
local+=part;
}
local+=string("/stress");
stringstream lStream;
lStream << id;
local += lStream.str();
p.put("device", "remote_controlboard");
p.put("local", local.c_str());
p.put("remote", remote.c_str());
p.put("carrier", protocol.c_str());
dd.open(p);
if (!dd.isValid())
{
fprintf(stderr, "Error, could not open controlboard\n");
return -1;
}
IEncoders *ienc;
IPositionControl *ipos;
IAmplifierControl *iamp;
IPidControl *ipid;
IControlLimits *ilim;
IControlCalibration2 *ical;
dd.view(ienc);
dd.view(ipos);
dd.view(iamp);
dd.view(ipid);
dd.view(ilim);
dd.view(ical);
int c=100;
int nj;
Vector encoders;
ienc->getAxes(&nj);
encoders.resize(nj);
int count=0;
bool done=false;
double startT=Time::now();
double now=0;
while((!done) || (time==-1))
{
count++;
double v;
int jj=0;
for(jj=0; jj<nj; jj++)
{
// ienc->getEncoder(jj, encoders.data()+jj);
// iamp->enableAmp(jj);
// ilim->setLimits(jj, 0, 0);
// double max;
// double min;
// ilim->getLimits(jj, &min, &max);
fprintf(stderr, ".");
// Pid pid;
// ipid->getPid(jj, &pid);
bool done;
ipos->checkMotionDone(jj, &done);
fprintf(stderr, "#\n");
}
fprintf(stderr, "%u\n", count);
Time::delay(0.1);
now=Time::now();
if ((now-startT)>time)
done=true;
}
printf("bye bye from %d\n", id);
return 0;
}
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;
}
int main()
{
Network::init();
srand(time(NULL));
Property options;
options.put("robot", "icub"); // typically from the command line.
options.put("device", "remote_controlboard");
Value& robotname = options.find("robot");
string s("/");
s += robotname.asString();
s += "/right_arm/babbling";
options.put("local", s.c_str());
s.clear();
s += "/";
s += robotname.asString();
s += "/right_arm";
options.put("remote", s.c_str());
PolyDriver dd(options);
if (!dd.isValid()) {
cout << "Device not available. Here are the known devices:\n"<< endl;
cout << Drivers::factory().toString().c_str() << endl;;
Network::fini();
return 0;
}
IPositionControl *pos;
IVelocityControl *vel;
IEncoders *enc;
IPidControl *pid;
IAmplifierControl *amp;
IControlLimits *lim;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(enc);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(lim);
if (!ok) {
cout << "Device not able to acquire views" << endl;
Network::fini();
dd.close();
return 0;
}
int jnts = 0;
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
// limits
vector<pair<int, int> > vLimitJoints;
for (int i = 0; i < jnts; i++) {
double min, max;
lim->getLimits(i, &min, &max);
vLimitJoints.push_back(pair<int, int> ((int)min, (int)max));
}
// get value of the arm
// we move : 4 5 6 7 8 9 10 11 12 13 14 15
// we don't move: 0 1 2 3
vector<bool> mask;
mask.resize(jnts);
mask[0] = false;
mask[1] = false;
mask[2] = false;
mask[3] = false;
mask[4] = false;
mask[5] = false;
mask[6] = false;
mask[7] = false;
mask[8] = false;
mask[9] = false;
mask[10] = false;
mask[11] = true;
mask[12] = true;
mask[13] = true;
mask[14] = true;
mask[15] = true;
Vector tmp;
tmp.resize(jnts,0.0);
for (int i = 4; i < jnts; i++)
{
pos->positionMove(i, 0.0);
}
bool initDone = false;
while (!initDone)
{
initDone = true;
for (int i = 4; i < jnts; i++)
{
bool jntMotionDone = false;
pos->checkMotionDone(i, &jntMotionDone);
initDone &= jntMotionDone;
}
}
while(true)
{
cout << "Moving to new posture..." << endl;
for (int i = 4; i < jnts; i++)
{
if (mask[i])
{
double newValue = yarp::os::Random::uniform(vLimitJoints[i].first, vLimitJoints[i].second);
tmp[i] = newValue;
pos->positionMove(i, tmp[i]);
}
}
cout << "Waiting for posture to be reached... ("<<tmp.toString(3,3)<<" ) ..." ;
bool motionDone = false;
while (!motionDone)
{
motionDone = true;
for (int i = 4; i < jnts; i++)
{
if (mask[i])
{
bool jntMotionDone = false;
pos->checkMotionDone(i, &jntMotionDone);
motionDone &= jntMotionDone;
}
}
}
Time::delay(15.0);
cout << "ok" << endl;
}
dd.close();
return 0;
}
int main(int argc, char *argv[])
{
Network yarp;
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
return -1;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/left_arm";
std::string localPorts="/test/client";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str()); //local port names
options.put("remote", remotePorts.c_str()); //where we connect to
// create a device
PolyDriver robotDevice(options);
if (!robotDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionControl *pos;
IEncoders *encs;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
int i;
for (i = 0; i < nj; i++) {
tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < nj; i++) {
tmp[i] = 4.0;
pos->setRefSpeed(i, tmp[i]);
}
//pos->setRefSpeeds(tmp.data()))
//fisrst zero all joints
//
command=0;
//now set the shoulder to some value
command[0]=-26.0;
command[1]=20.0;
command[2]=0.0;
command[3]=49.0;
command[4]=0.0;
pos->positionMove(command.data());
bool done=false;
while(!done) {
Time::delay(0.5);
pos->checkMotionDone(&done);
}
printf("\niCub @ HOME. Press any key...");
mygetch();
int times=0;
while(true)
{
times++;
if (times%2)
{
printf("\n\nSet pos1: ");
//command[0]=-50;
command[1]=64.0;
//command[2]=-10;
//command[3]=50;
//command[4]=0;
}
else
{
printf("\n\nSet pos2: ");
//command[0]=-20;
command[1]=20.0;
//command[2]=-10;
//command[3]=30;
//command[4]=0;
}
pos->positionMove(command.data());
printf("waiting");
bool done3=false;
while(!done3) {
Time::delay(1.0);
pos->checkMotionDone(&done3);
printf(".");
}
printf("ok!\n");
mygetch();
}
robotDevice.close();
return 0;
}
int main(int argc, char *argv[])
{
Network yarp;
int maxSpeed;
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
return -1;
}
if (!params.check("repetitions"))
{
fprintf(stderr, "Please specify number of repetitions\n");
fprintf(stderr, "--repetitions num (e.g. 10)\n");
return -1;
}
if (!params.check("speed"))
{
fprintf(stderr, "Speed not specified using default\n");
fprintf(stderr, "--speed num (e.g. 2)\n");
maxSpeed = 10.0;
}
else
{
maxSpeed = params.find("speed").asInt();
}
// sanity check on argument value
if(maxSpeed <0 || maxSpeed>50)
{
maxSpeed = 10;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/head";
int numTimes = params.find("repetitions").asInt();
std::string localPorts="/headMovement_koroibot/client";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str()); //local port names
options.put("remote", remotePorts.c_str()); //where we connect to
// create a device
PolyDriver robotDevice(options);
if (!robotDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionControl *pos;
IEncoders *encs;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
int i;
for (i = 0; i < nj; i++) {
tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < nj; i++) {
tmp[i] = 10.0;
pos->setRefSpeed(i, tmp[i]);
}
//first read all encoders
printf("waiting for encoders");
while(!encs->getEncoders(encoders.data()))
{
Time::delay(0.1);
printf(".");
}
printf("\n;");
int ctr =0;
bool done = false;
while(ctr<numTimes)
{
printf("Starting headMovement\n");
command=encoders;
if(ctr%2 == 0)
command[2]=HEAD_YAW_MAX;
else
command[2]=HEAD_YAW_MIN;
done = false;
pos->positionMove(command.data());
while(!done)
{
pos->checkMotionDone(&done);
Time::delay(0.1);
}
ctr++;
}
command[2] = 0;
pos->positionMove(command.data());
while(!done)
{
pos->checkMotionDone(&done);
Time::delay(0.1);
}
robotDevice.close();
return 0;
}
bool partMover::entry_update(partMover *currentPart)
{
GdkColor color_grey;
GdkColor color_yellow;
GdkColor color_green;
GdkColor color_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_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_blue.red=150*255;
color_blue.green=190*255;
color_blue.blue=255*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->ctrlmode;
IPositionControl *ipos = currentPart->pos;
IVelocityControl *ivel = currentPart->iVel;
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];
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" );
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_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;
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;
}
// pColor=&color_blue;
// gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
int curr_amp_status=0;
int amp_status[60]; //fix this!!!
for (int i=0; i<60; i++) amp_status[i]=0; //fix this!!!
iamp->getAmpStatus(amp_status); //fix this!!!
curr_amp_status=amp_status[k]; //fix this!!!
#if 0
if ((amp_status[k] & 0xFF)!=0)
{
//fprintf(stderr, "FAULT DETECTED: %x\n", curr_amp_status);
//pColor=&color_red;
//strcat(frame_title," (FAULT)");
//gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
//gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
#endif
}
currentPart->first_time =false;
return true;
}
int main(int argc, char *argv[]){
Network yarp;
//Port<Bottle> armPlan;
//Port<Bottle> armPred;
Port armPlan;
Port armPred;
armPlan.open("/randArm/plan");
armPred.open("/randArm/pred");
bool fwCvOn = 0;
fwCvOn = Network::connect("/randArm/plan","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/randArm/pred");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o");
return 1;
}
const gsl_rng_type *T;
gsl_rng *r;
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc(T);
Property params;
params.fromCommand(argc,argv);
if (!params.check("robot")){
fprintf(stderr, "Please specify robot name");
fprintf(stderr, "e.g. --robot icub");
return -1;
}
std::string robotName = params.find("robot").asString().c_str();
std::string remotePorts = "/";
remotePorts += robotName;
remotePorts += "/";
if (params.check("side")){
remotePorts += params.find("side").asString().c_str();
}
else{
remotePorts += "left";
}
remotePorts += "_arm";
std::string localPorts = "/randArm/cmd";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
PolyDriver robotDevice(options);
if (!robotDevice.isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IEncoders *enc;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return 0;
}
int nj = 0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector commandCart;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
commandCart.resize(nj);
for (int i = 0; i < nj; i++) {
tmp[i] = 25.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nj; i++) {
tmp[i] = 5.0;
pos->setRefSpeed(i, tmp[i]);
}
command = 0;
//set the arm joints to "middle" values
command[0] = -45;
command[1] = 45;
command[2] = 0;
command[3] = 45;
pos->positionMove(command.data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
while (true){
tmp = command;
command[0] += 15*(2*gsl_rng_uniform(r)-1);
command[1] += 15*(2*gsl_rng_uniform(r)-1);
command[2] += 15*(2*gsl_rng_uniform(r)-1);
command[3] += 15*(2*gsl_rng_uniform(r)-1);
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] < -90){
command[0] = tmp[0];
}
if (command[1] > 160 || command[1] < -0){
command[1] = tmp[1];
}
if (command[2] > 100 || 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);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
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());
done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
}
else{
printf("Self collision detected!\n");
}
}
robotDevice.close();
gsl_rng_free(r);
return 0;
}