void cartesianMover::cartesian_line_click(GtkTreeView *tree_view, GtkTreePath *path, GtkTreeViewColumn *column, cartesianMover *cm)
{
ICartesianControl *icrt = cm->crt;
int *i = gtk_tree_path_get_indices (path);
//double currPos[NUMBER_OF_CARTESIAN_COORDINATES];
Vector x;
Vector axis;
if(!icrt->getPose(x,axis))
fprintf(stderr, "Troubles in getting the cartesian pose for %s", cm->partLabel);
Matrix R = axis2dcm(axis);
Vector eu = dcm2euler(R);
//fprintf(stderr, "Storing euler angles: %s...", eu.toString().c_str());
//fprintf(stderr, "...corrsponding to axis: %s", axis.toString().c_str());
for (int k =0; k < NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
if(k<3)
cm->STORED_POS[*i][k] = x(k);
else
cm->STORED_POS[*i][k] = eu(k-3)*180/M_PI;
}
gtk_tree_view_set_model (GTK_TREE_VIEW (tree_view), refresh_cartesian_list_model(cm));
gtk_widget_draw(GTK_WIDGET(tree_view), NULL);
return;
}
bool cartesianMover::display_cartesian_pose(cartesianMover *cm)
{
ICartesianControl *icrt = cm->crt;
GtkEntry **entry = (GtkEntry **) cm->currPosArray;
GtkWidget **sliderPosArray = (GtkWidget **) cm->sliderArray;
//fprintf(stderr, "Trying to get the cartesian position...");
Vector x;
Vector axis;
if(!icrt->getPose(x,axis))
fprintf(stderr, "Troubles in gettin the cartesian pose for %s", cm->partLabel);
//else
//fprintf(stderr, "got x=%s, o=%s\n", x.toString().c_str(), o.toString().c_str());
//converting to a Rotation Matrix
Matrix R = axis2dcm(axis);
//converting to a Euler angles
Vector eulerAngles = dcm2euler(R, 1);
//back to the Roation Matrix
//Matrix Rr = euler2dcm(eulerAngles);
//fprintf(stderr, "v: %s\n Res Matrix: %s\n", eulerAngles.toString().c_str(), (Rr-R).toString().c_str());
gboolean focus = false;
char buffer[40] = {'i', 'n', 'i', 't'};
for (int k = 0; k < NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
if (k<3)
sprintf(buffer, "%.2f", x(k));
if (k>=3 && k<= 5)
sprintf(buffer, "%.1f", eulerAngles(k-3)*180/M_PI);
gtk_entry_set_text((GtkEntry*) entry[k], buffer);
//if changing orientation do not update all the three sliders
//if changing position update other sliders
if(k<3)
{
g_object_get((gpointer) sliderPosArray[k], "has-focus", &focus, NULL);
if(!focus)
gtk_range_set_value ((GtkRange *) (sliderPosArray[k]), x(k));
}
else
{
for (int i = 3; i < NUMBER_OF_CARTESIAN_COORDINATES; i++)
{
gboolean tmp;
g_object_get((gpointer) sliderPosArray[i], "has-focus", &tmp, NULL);
focus = focus || tmp;
}
if(!focus)
gtk_range_set_value ((GtkRange *) (sliderPosArray[k]), eulerAngles(k-3) * 180/M_PI);
}
}
return true;
}
Vector Solver::computeTargetUserTolerance(const Vector &xd)
{
Matrix H=commData->get_fpFrame();
Vector z(3,0.0); z[2]=1.0;
Vector xdh=xd; xdh.push_back(1.0);
xdh=SE3inv(H)*xdh;
Vector xdh3=xdh; xdh3.pop_back();
Vector rot=cross(xdh3,z);
double r=norm(rot);
if (r<IKIN_ALMOST_ZERO)
return xd;
rot=rot/r;
rot.push_back(neckAngleUserTolerance*CTRL_DEG2RAD);
rot=H*(axis2dcm(rot)*xdh);
rot.pop_back();
return rot;
}
void vtWThread::run()
{
optFlowPos.resize(3,0.0);
pf3dTrackerPos.resize(3,0.0);
doubleTouchPos.resize(3,0.0);
fgtTrackerPos.resize(3,0.0);
bool isTarget = false;
events.clear();
// process the optFlow
if (optFlowBottle = optFlowPort.read(false))
{
if (optFlowBottle->size()>=3)
{
yDebug("Computing data from the optFlowTracker %g\n",getEstUsed());
optFlowPos.zero();
optFlowPos[0]=optFlowBottle->get(0).asDouble();
optFlowPos[1]=optFlowBottle->get(1).asDouble();
optFlowPos[2]=optFlowBottle->get(2).asDouble();
AWPolyElement el(optFlowPos,Time::now());
optFlowVelEstimate=linEst_optFlow->estimate(el);
events.push_back(IncomingEvent(optFlowPos,optFlowVelEstimate,0.05,"optFlow"));
isTarget=true;
}
}
// process the pf3dTracker
if (pf3dTrackerBottle = pf3dTrackerPort.read(false))
{
if (pf3dTrackerBottle->size()>6)
{
if (pf3dTrackerBottle->get(6).asDouble()==1.0)
{
Vector fp(4);
fp[0]=pf3dTrackerBottle->get(0).asDouble();
fp[1]=pf3dTrackerBottle->get(1).asDouble();
fp[2]=pf3dTrackerBottle->get(2).asDouble();
fp[3]=1.0;
if (!gsl_isnan(fp[0]) && !gsl_isnan(fp[1]) && !gsl_isnan(fp[2]))
{
yDebug("Computing data from the pf3dTracker %g\n",getEstUsed());
Vector x,o;
igaze->getLeftEyePose(x,o);
Matrix T=axis2dcm(o);
T(0,3)=x[0];
T(1,3)=x[1];
T(2,3)=x[2];
pf3dTrackerPos=T*fp;
pf3dTrackerPos.pop_back();
AWPolyElement el(pf3dTrackerPos,Time::now());
pf3dTrackerVelEstimate=linEst_pf3dTracker->estimate(el);
events.push_back(IncomingEvent(pf3dTrackerPos,pf3dTrackerVelEstimate,0.05,"pf3dTracker"));
isTarget=true;
}
}
}
}
if (!rf->check("noDoubleTouch"))
{
// processes the fingertipTracker
if(fgtTrackerBottle = fgtTrackerPort.read(false))
{
if (doubleTouchBottle = doubleTouchPort.read(false))
{
if(fgtTrackerBottle != NULL && doubleTouchBottle != NULL)
{
if (doubleTouchBottle->get(3).asString() != "" && fgtTrackerBottle->get(0).asInt() != 0)
{
yDebug("Computing data from the fingertipTracker %g\n",getEstUsed());
doubleTouchStep = doubleTouchBottle->get(0).asInt();
fgtTrackerPos[0] = fgtTrackerBottle->get(1).asDouble();
fgtTrackerPos[1] = fgtTrackerBottle->get(2).asDouble();
fgtTrackerPos[2] = fgtTrackerBottle->get(3).asDouble();
AWPolyElement el2(fgtTrackerPos,Time::now());
fgtTrackerVelEstimate=linEst_fgtTracker->estimate(el2);
if(doubleTouchStep<=1)
{
Vector ang(3,0.0);
igaze -> lookAtAbsAngles(ang);
}
else if(doubleTouchStep>1 && doubleTouchStep<8)
{
events.clear();
events.push_back(IncomingEvent(fgtTrackerPos,fgtTrackerVelEstimate,-1.0,"fingertipTracker"));
isTarget=true;
}
}
}
}
}
// processes the doubleTouch !rf->check("noDoubleTouch")
if(doubleTouchBottle = doubleTouchPort.read(false))
{
if(doubleTouchBottle != NULL)
{
if (doubleTouchBottle->get(3).asString() != "")
{
Matrix T = eye(4);
Vector fingertipPos(4,0.0);
doubleTouchPos.resize(4,0.0);
currentTask = doubleTouchBottle->get(3).asString();
doubleTouchStep = doubleTouchBottle->get(0).asInt();
fingertipPos = iCub::skinDynLib::matrixFromBottle(*doubleTouchBottle,20,4,4).subcol(0,3,3); // fixed translation from the palm
fingertipPos.push_back(1.0);
if(doubleTouchStep<=1)
{
Vector ang(3,0.0);
igaze -> lookAtAbsAngles(ang);
}
else if(doubleTouchStep>1 && doubleTouchStep<8)
{
if(currentTask=="LtoR" || currentTask=="LHtoR") //right to left -> the right index finger will be generating events
{
iencsR->getEncoders(encsR->data());
Vector qR=encsR->subVector(0,6);
armR -> setAng(qR*CTRL_DEG2RAD);
T = armR -> getH(3+6, true); // torso + up to wrist
doubleTouchPos = T * fingertipPos;
//optionally, get the finger encoders and get the fingertip position using iKin Finger based on the current joint values
//http://wiki.icub.org/iCub/main/dox/html/icub_cartesian_interface.html#sec_cart_tipframe
doubleTouchPos.pop_back(); //take out the last dummy value from homogenous form
}
else if(currentTask=="RtoL" || currentTask=="RHtoL") //left to right -> the left index finger will be generating events
{
iencsL->getEncoders(encsL->data());
Vector qL=encsL->subVector(0,6);
armL -> setAng(qL*CTRL_DEG2RAD);
T = armL -> getH(3+6, true); // torso + up to wrist
doubleTouchPos = T * fingertipPos;
//optionally, get the finger encoders and get the fingertip position using iKin Finger based on the current joint values
//http://wiki.icub.org/iCub/main/dox/html/icub_cartesian_interface.html#sec_cart_tipframe
doubleTouchPos.pop_back(); //take out the last dummy value from homogenous form
}
else
{
yError(" [vtWThread] Unknown task received from the double touch thread!");
}
yDebug("Computing data from the doubleTouch %g\n",getEstUsed());
AWPolyElement el2(doubleTouchPos,Time::now());
doubleTouchVelEstimate=linEst_doubleTouch->estimate(el2);
events.push_back(IncomingEvent(doubleTouchPos,doubleTouchVelEstimate,-1.0,"doubleTouch"));
isTarget=true;
}
}
}
}
}
if (pf3dTrackerPos[0]!=0.0 && pf3dTrackerPos[1]!=0.0 && pf3dTrackerPos[2]!=0.0)
igaze -> lookAtFixationPoint(pf3dTrackerPos);
else if (doubleTouchPos[0]!=0.0 && doubleTouchPos[1]!=0.0 && doubleTouchPos[2]!=0.0)
igaze -> lookAtFixationPoint(doubleTouchPos);
else if (optFlowPos[0]!=0.0 && optFlowPos[1]!=0.0 && optFlowPos[2]!=0.0)
igaze -> lookAtFixationPoint(optFlowPos);
if (isTarget)
{
Bottle& eventsBottle = eventsPort.prepare();
eventsBottle.clear();
for (size_t i = 0; i < events.size(); i++)
{
eventsBottle.addList()= events[i].toBottle();
}
eventsPort.write();
timeNow = yarp::os::Time::now();
sendGuiTarget();
}
else if (yarp::os::Time::now() - timeNow > 1.0)
{
yDebug("No significant event in the last second. Resetting the velocity estimators..");
timeNow = yarp::os::Time::now();
linEst_optFlow -> reset();
linEst_pf3dTracker -> reset();
linEst_doubleTouch -> reset();
linEst_fgtTracker -> reset();
deleteGuiTarget();
}
}
void EyePinvRefGen::run()
{
if (genOn)
{
LockGuard guard(mutex);
double timeStamp;
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&timeStamp);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
// get current target
Vector xd=commData->port_xd->get_xd();
// update neck chain
chainNeck->setAng(nJointsTorso+0,fbHead[0]);
chainNeck->setAng(nJointsTorso+1,fbHead[1]);
chainNeck->setAng(nJointsTorso+2,fbHead[2]);
// ask for saccades (if possible)
if (commData->saccadesOn && (saccadesRxTargets!=commData->port_xd->get_rx()) &&
!commData->saccadeUnderway && (Time::now()-saccadesClock>commData->saccadesInhibitionPeriod))
{
Vector fph=xd; fph.push_back(1.0);
fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;
// estimate geometrically the target tilt and pan of the eyes
Vector ang(3,0.0);
ang[0]=-atan2(fph[1],fabs(fph[2]));
ang[1]=atan2(fph[0],fph[2]);
// enforce joints bounds
ang[0]=sat(ang[0],lim(0,0),lim(0,1));
ang[1]=sat(ang[1],lim(1,0),lim(1,1));
// favor the smooth-pursuit in case saccades are small
if (rot>commData->saccadesActivationAngle)
{
// init vergence
ang[2]=fbHead[5];
// get rid of eyes tilt
Vector axis(4);
axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
fph=axis2dcm(axis)*fph;
// go on iff the point is in front of us
if (fph[2]>0.0)
{
double L,R;
// estimate geometrically the target vergence Vg=L-R
if (fph[0]>=eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
}
else if (fph[0]>-eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
else
{
L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
ang[2]=L-R;
}
// enforce joints bounds
ang[2]=sat(ang[2],lim(2,0),lim(2,1));
commData->set_qd(3,ang[0]);
commData->set_qd(4,ang[1]);
commData->set_qd(5,ang[2]);
Vector vel(3,SACCADES_VEL);
ctrl->doSaccade(ang,vel);
saccadesClock=Time::now();
}
}
// update eyes chains for convergence purpose
updateNeckBlockedJoints(chainEyeL,fbHead); updateNeckBlockedJoints(chainEyeR,fbHead);
chainEyeL->setAng(nJointsTorso+3,qd[0]); chainEyeR->setAng(nJointsTorso+3,qd[0]);
chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);
// converge on target
if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
{
Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));
// update eyes chains in actual configuration for velocity compensation
chainEyeL->setAng(nJointsTorso+3,fbHead[3]); chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
// compensate neck rotation at eyes level
if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
commData->set_counterv(zeros(qd.length()));
else
commData->set_counterv(getEyesCounterVelocity(eyesJ,fp));
// reset eyes controller and integral upon saccades transition on=>off
if (saccadeUnderWayOld && !commData->saccadeUnderway)
{
ctrl->resetCtrlEyes();
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I->reset(qd);
}
// update reference
qd=I->integrate(v+commData->get_counterv());
}
else
commData->set_counterv(zeros(qd.length()));
// set a new target position
commData->set_xd(xd);
commData->set_x(fp,timeStamp);
commData->set_fpFrame(chainNeck->getH());
if (!commData->saccadeUnderway)
{
commData->set_qd(3,qd[0]);
commData->set_qd(4,qd[1]);
commData->set_qd(5,qd[2]);
}
// latch the saccades status
saccadeUnderWayOld=commData->saccadeUnderway;
saccadesRxTargets=commData->port_xd->get_rx();
}
}
Vector Localizer::get3DPoint(const string &type, const Vector &ang)
{
double azi=ang[0];
double ele=ang[1];
double ver=ang[2];
Vector q(8,0.0);
if (type=="rel")
{
Vector torso=commData->get_torso();
Vector head=commData->get_q();
q[0]=torso[0];
q[1]=torso[1];
q[2]=torso[2];
q[3]=head[0];
q[4]=head[1];
q[5]=head[2];
q[6]=head[3];
q[7]=head[4];
ver+=head[5];
}
// impose vergence != 0.0
if (ver<commData->get_minAllowedVergence())
ver=commData->get_minAllowedVergence();
mutex.lock();
q[7]+=ver/2.0;
eyeL->setAng(q);
q[7]-=ver;
eyeR->setAng(q);
Vector fp(4);
fp[3]=1.0; // impose homogeneous coordinates
// compute new fp due to changed vergence
CartesianHelper::computeFixationPointData(*(eyeL->asChain()),*(eyeR->asChain()),fp);
mutex.unlock();
// compute rotational matrix to
// account for elevation and azimuth
Vector x(4), y(4);
x[0]=1.0; y[0]=0.0;
x[1]=0.0; y[1]=1.0;
x[2]=0.0; y[2]=0.0;
x[3]=ele; y[3]=azi;
Matrix R=axis2dcm(y)*axis2dcm(x);
Vector fph, xd;
if (type=="rel")
{
Matrix frame=commData->get_fpFrame();
fph=SE3inv(frame)*fp; // get fp wrt relative head-centered frame
xd=frame*(R*fph); // apply rotation and retrieve fp wrt root frame
}
else
{
fph=invEyeCAbsFrame*fp; // get fp wrt absolute head-centered frame
xd=eyeCAbsFrame*(R*fph); // apply rotation and retrieve fp wrt root frame
}
xd.pop_back();
return xd;
}
cartesianMover::cartesianMover(GtkWidget *vbox_d, PolyDriver *partDd_d, char *partName, ResourceFinder *fnd)
{
finder = fnd;
if (!finder->isNull())
fprintf(stderr, "Setting a valid finder \n");
partLabel = partName;
partDd = partDd_d;
vbox = vbox_d;
interfaceError = false;
if (!partDd->isValid()) {
fprintf(stderr, "Device given to the cartesian interface is not available.\n");
interfaceError = true;
}
fprintf(stderr, "Opening crt interface...");
bool ok;
ok = partDd->view(crt);
if ((!ok) || (crt==0))
fprintf(stderr, "...crt was not ok...ok=%d", ok);
if (!partDd->isValid()) {
fprintf(stderr, "Cartesian device driver was not valid! \n");
dialog_severe_error(GTK_MESSAGE_ERROR,(char *) "Cartesian device not available.", (char *) "Check available devices", true);
interfaceError = true;
}
else if (!ok) {
fprintf(stderr, "Error while acquiring cartesian interfaces \n");
dialog_severe_error(GTK_MESSAGE_ERROR,(char *) "Problems acquiring cartesian interface", (char *) "Check if cartesian interface is running", true);
interfaceError = true;
}
if (interfaceError == false)
{
fprintf(stderr, "Allocating memory \n");
int j,k;
index = new int [MAX_NUMBER_OF_JOINTS];
entry_id = new guint [0];
*entry_id = -1;
frame_slider1 = new GtkWidget* [MAX_NUMBER_OF_JOINTS];
sliderArray = new GtkWidget* [NUMBER_OF_CARTESIAN_COORDINATES];
currPosArray = new GtkWidget* [NUMBER_OF_CARTESIAN_COORDINATES];
//fprintf(stderr, "sliderArray has address 0x%x\n", (unsigned int) sliderArray);
GtkWidget *top_hbox = NULL;
GtkWidget *bottom_hbox = NULL;
GtkWidget *panel_hbox = NULL;
GtkWidget *inv1 = NULL;
GtkWidget *invArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *homeArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *framesArray[NUMBER_OF_CARTESIAN_COORDINATES];
GtkWidget *sw = NULL;
//creation of the top_hbox
top_hbox = gtk_hbox_new (FALSE, 0);
gtk_container_set_border_width (GTK_CONTAINER (top_hbox), 10);
gtk_container_add (GTK_CONTAINER (vbox), top_hbox);
inv1 = gtk_fixed_new ();
gtk_container_add (GTK_CONTAINER (top_hbox), inv1);
Vector o;
Vector x;
while (!crt->getPose(x,o))
Time::delay(0.001);
Matrix R = axis2dcm(o);
Vector eu = dcm2euler(R);
//x(0) = 1; x(1) = 1; x(2) = 1;
//o(0) = 1; o(1) = 0; o(2) = 0; o(3) = 0;
char buffer[40] = {'i', 'n', 'i', 't'};
int numberOfRows = 3;
int height, width;
height = 100;
width = 180;
double min,max;
std::string limitString=partLabel;
limitString=limitString+"_workspace";
Bottle bCartesianLimits;
if (finder->check(limitString.c_str()))
{
//fprintf(stderr, "There seem limits for %s", partLabel);
bCartesianLimits = finder->findGroup(limitString.c_str());
//fprintf(stderr, "...got: %s", bCartesianLimits.toString().c_str());
}
fprintf(stderr, "Starting embedding cartesian GUI widgets \n");
for (k = 0; k<NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
//fprintf(stderr, "Adding invArray \n");
invArray[k] = gtk_fixed_new ();
index[k]=k;
j = k/numberOfRows;
if (k==0)
{
sprintf(buffer, "x");
if (bCartesianLimits.check("xmin") && bCartesianLimits.check("xmax"))
{
min = bCartesianLimits.find("xmin").asDouble();
max = bCartesianLimits.find("xmax").asDouble();
}
else
{
min = x(0) - 0.1 * fabs(x(0));
max = x(0) + 0.1 * fabs(x(0));
}
}
if (k==1)
{
sprintf(buffer, "y");
if (bCartesianLimits.check("ymin") && bCartesianLimits.check("ymax"))
{
min = bCartesianLimits.find("ymin").asDouble();
max = bCartesianLimits.find("ymax").asDouble();
}
else
{
min = x(1) - 0.1 * fabs(x(1));
max = x(1) + 0.1 * fabs(x(1));
}
}
if (k==2)
{
sprintf(buffer, "z");
if (bCartesianLimits.check("zmin") && bCartesianLimits.check("zmax"))
{
min = bCartesianLimits.find("zmin").asDouble();
max = bCartesianLimits.find("zmax").asDouble();
}
else
{
min = x(2) - 0.1 * fabs(x(2));
max = x(2) + 0.1 * fabs(x(2));
}
}
if (k==3)
{
sprintf(buffer, "euler-alpha");
min = -180;
max = 180;
}
if (k==4)
{
sprintf(buffer, "euler-beta");
min = -180;
max = 180;
}
if (k==5)
{
sprintf(buffer, "euler-gamma");
min = -180;
max = 180;
}
frame_slider1[k] = gtk_frame_new ("Value:");
//fprintf(stderr, "Initializing sliders %d \n",k);
if (min<max)
{
sliderArray[k] = gtk_hscale_new_with_range(min, max, 1);
if (k<3)
gtk_scale_set_digits((GtkScale*) sliderArray[k],2);
else
gtk_scale_set_digits((GtkScale*) sliderArray[k],1);
}
else
{
sliderArray[k] = gtk_hscale_new_with_range(1, 2, 1);
if (k<3)
gtk_scale_set_digits((GtkScale*) sliderArray[k],2);
else
gtk_scale_set_digits((GtkScale*) sliderArray[k],1);
}
currPosArray[k] = gtk_entry_new();
//fprintf(stderr, "Initializing the buttons %d \n", k);
homeArray[k] = gtk_button_new_with_mnemonic ("Home");
//fprintf(stderr, "Initializing frames %d \n", k);
framesArray[k] = gtk_frame_new (buffer);
gtk_fixed_put (GTK_FIXED(inv1), invArray[k], 0+(k%numberOfRows)*width, 0+ j*height);
//Positions
//fprintf(stderr, "Positioning buttons %d \n", k);
gtk_fixed_put (GTK_FIXED(invArray[k]), frame_slider1[k], 60, 10 );
gtk_fixed_put (GTK_FIXED(invArray[k]), sliderArray[k], 65, 20 );
gtk_fixed_put (GTK_FIXED(invArray[k]), currPosArray[k], 95, 70);
int buttonDist= 24;
int buttonOffset = 13;
gtk_fixed_put (GTK_FIXED(invArray[k]), homeArray[k], 6, buttonOffset);
gtk_fixed_put (GTK_FIXED(invArray[k]), framesArray[k], 0, 0);
//Dimensions
//fprintf(stderr, "Dimensioning buttons %d \n", k);
gtk_widget_set_size_request (frame_slider1[k], 110, 50);
gtk_widget_set_size_request (sliderArray[k], 90, 40);
gtk_widget_set_size_request (currPosArray[k], 70, 20);
gtk_widget_set_size_request (homeArray[k], 50, 25);
gtk_widget_set_size_request (framesArray[k], width, height);
/*
* Positions commands
*/
//fprintf(stderr, "Assinging callback %d \n", k);
gtk_range_set_update_policy ((GtkRange *) (sliderArray[k]), GTK_UPDATE_DISCONTINUOUS);
if (k<3)
gtk_range_set_value ((GtkRange *) (sliderArray[k]), x(k));
if (k>=3 && k <= 5)
gtk_range_set_value ((GtkRange *) (sliderArray[k]), eu(k-3) * 180/M_PI);
g_signal_connect (sliderArray[k], "value-changed", G_CALLBACK(position_slider_changed), this);
}
/*
* Display current position
*/
*entry_id = gtk_timeout_add(UPDATE_TIME, (GtkFunction) display_cartesian_pose, this);
for (k = 0; k<NUMBER_OF_CARTESIAN_COORDINATES; k++)
gtk_editable_set_editable ((GtkEditable*) currPosArray[k], FALSE);
/*
* Common commands
*/
GtkWidget *frame3;
frame3 = gtk_frame_new ("Commands:");
gtk_fixed_put (GTK_FIXED(inv1), frame3, (NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame3, 180, 240);
//Button 0 in the panel
GtkWidget *button0 = gtk_button_new_with_mnemonic ("Open sequence tab");
gtk_fixed_put (GTK_FIXED (inv1), button0, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 20+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (button0, 150, 25);
fprintf(stderr, "Initializing the table \n");
init_cartesian_table();
fprintf(stderr, "Connecting the callbacks for the table \n");
g_signal_connect (button0, "clicked", G_CALLBACK (cartesian_table_open), this);
//Button1 in the panel
GtkWidget *button1 = gtk_button_new_with_mnemonic ("Stop");
gtk_fixed_put (GTK_FIXED (inv1), button1, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 45+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (button1, 150, 25);
//g_signal_connect (button1, "clicked", G_CALLBACK (stop_motion), crt);
//Velocity
GtkWidget *frame7;
frame7 = gtk_frame_new ("Time[sec]:");
gtk_fixed_put (GTK_FIXED(inv1), frame7, 5+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 70 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame7, 160, 50);
sliderVelocity = new GtkWidget;
sliderVelocity = gtk_hscale_new_with_range(1, 10, 1);
gtk_scale_set_digits((GtkScale*) sliderVelocity,2);
gtk_fixed_put (GTK_FIXED(inv1), sliderVelocity, 60+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 80 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (sliderVelocity, 90, 40);
gtk_range_set_update_policy ((GtkRange *) (sliderVelocity), GTK_UPDATE_DISCONTINUOUS);
gtk_range_set_value ((GtkRange *) (sliderVelocity), 2);
//Diplay axis
po = new GtkWidget*[4];
for (int i=0; i < 4; i++)
{
po[i] = gtk_entry_new();
}
//Display axis
GtkWidget *frame5;
frame5 = gtk_frame_new ("Axis:");
gtk_fixed_put (GTK_FIXED(inv1), frame5, 5+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 140 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame5, 80, 85);
gtk_fixed_put (GTK_FIXED(inv1), po[0], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 155+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_fixed_put (GTK_FIXED(inv1), po[1], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 175+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_fixed_put (GTK_FIXED(inv1), po[2], 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 195+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
//Display angle
GtkWidget *frame6;
frame6 = gtk_frame_new ("Angle:");
gtk_fixed_put (GTK_FIXED(inv1), frame6, 85+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 155 + (NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (frame6, 80, 45);
gtk_fixed_put (GTK_FIXED(inv1), po[3], 90+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 175+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
for (int i=0; i < 4; i++)
{
gtk_widget_set_size_request (po[i], 70, 20);
gtk_editable_set_editable ((GtkEditable*) po[i], FALSE);
}
*entry_id = gtk_timeout_add(UPDATE_TIME, (GtkFunction) display_axis_pose, this);
//CheckButton in the panel
GtkWidget *check= gtk_check_button_new_with_mnemonic ("Tracking Mode");
gtk_fixed_put (GTK_FIXED (inv1), check, 10+(NUMBER_OF_CARTESIAN_COORDINATES%numberOfRows)*width, 120+(NUMBER_OF_CARTESIAN_COORDINATES/numberOfRows)*height);
gtk_widget_set_size_request (check, 150, 25);
//g_signal_connect (check, "clicked", G_CALLBACK (toggle_tracking_mode), crt);
}
}
