From fb05a955c2158b0e1a49c7b4d23623d7ae79a53b Mon Sep 17 00:00:00 2001 From: Alessandro Scalzo Date: Tue, 8 Dec 2015 11:32:45 +0100 Subject: [PATCH] Removed tabs, namespace changed from cer::kinematics2 to cer::kinematics_alt --- .../include/cer_kinematics_alt/Solver.h | 58 ++--- .../cer_kinematics_alt/private/Geometry.h | 26 +- .../cer_kinematics_alt/private/Joints.h | 14 +- .../cer_kinematics_alt/private/Matrix.h | 28 +- libraries/cer_kinematics_alt/src/Joints.cpp | 8 +- libraries/cer_kinematics_alt/src/Solver.cpp | 32 +-- .../test-cer_kinematics_alt-tracking.cpp | 244 +++++++++--------- 7 files changed, 205 insertions(+), 205 deletions(-) diff --git a/libraries/cer_kinematics_alt/include/cer_kinematics_alt/Solver.h b/libraries/cer_kinematics_alt/include/cer_kinematics_alt/Solver.h index 0649f793..fa68f54d 100644 --- a/libraries/cer_kinematics_alt/include/cer_kinematics_alt/Solver.h +++ b/libraries/cer_kinematics_alt/include/cer_kinematics_alt/Solver.h @@ -31,7 +31,7 @@ #include namespace cer { -namespace kinematics2 { +namespace kinematics_alt { #define NJOINTS 22 #define PERIOD 0.01 // seconds @@ -40,7 +40,7 @@ namespace kinematics2 { #define DEFAULT_TORSO_EXTENSION 0.0 #define DEFAULT_POS_THRESHOLD 0.005 -#define SOLVER_TIMEOUT 40 +#define SOLVER_TIMEOUT 40 #define WRIST_MAX_TILT 35.0 #define TORSO_MAX_TILT 30.0 @@ -58,15 +58,15 @@ enum { R = 0, L = 1 }; /** - * Class to handle direct and inverse kinematics of the robot arm. - * + * Class to handle direct and inverse kinematics of the robot arm. + * * @author Alessandro Scalzo */ class LeftSideSolver { public: - LeftSideSolver(); + LeftSideSolver(); virtual ~LeftSideSolver(void) { @@ -87,11 +87,11 @@ class LeftSideSolver /** * Forward Kinematics Law. - * + * * @param qin the DOFs values ([m]-[deg]-[m]). - * @param H the 4-by-4 homogeneous matrix of the specified + * @param H the 4-by-4 homogeneous matrix of the specified * frame ([m]). - * @param frame specify the DOF number whose frame is returned. + * @param frame specify the DOF number whose frame is returned. * Thus, frame is in [0...nDOF-1]; negative * numbers account for the end-effector frame. * @return true/false on success/failure. @@ -99,17 +99,17 @@ class LeftSideSolver bool fkin(const yarp::sig::Vector &qin, yarp::sig::Matrix &H,int frame=LEFT_HAND) { if (frame<0) frame=LEFT_HAND; - + if (frame>NJOINTS) return false; Matrix qconf(NJOINTS); - int N=qin.length(); - + int N=qin.length(); + if (N>NJOINTS) N=NJOINTS; for (int j=0; jcalcPosture(qconf,T_ROOT); Rotation& R=mPart[frame]->Toj.Rj(); @@ -126,11 +126,11 @@ class LeftSideSolver /** * Inverse Kinematics Law. - * - * @param Hd the desired 4-by-4 homogeneous matrix + * + * @param Hd the desired 4-by-4 homogeneous matrix * representing the end-effector frame ([m]). * @param qout the solved DOFs ([m]-[deg]-[m]). - * @param armElong the desired elongation of the left forearm tripod ([m]). + * @param armElong the desired elongation of the left forearm tripod ([m]). * @param torsoElong the desired elongation of the torso tripod ([m]). * @return true/false on success/failure. */ @@ -151,8 +151,8 @@ class LeftSideSolver } QtargetL=Rotation(Vec3(Hd(0,0),Hd(1,0),Hd(2,0)),Vec3(Hd(0,1),Hd(1,1),Hd(2,1)),Vec3(Hd(0,2),Hd(1,2),Hd(2,2))).quaternion(); - - XtargetL=Vec3(Hd(0,3),Hd(1,3),Hd(2,3)); + + XtargetL=Vec3(Hd(0,3),Hd(1,3),Hd(2,3)); q=q_valid=qzero; @@ -161,8 +161,8 @@ class LeftSideSolver int steps=0; int N=qout.length(); - - if (N>NJOINTS) N=NJOINTS; + + if (N>NJOINTS) N=NJOINTS; //leftHand2Target(XtargetL,QtargetL); //for (int j=0; j=qmax[j]) - { + { if (qd(j)>0.0) { qd(j)=0.0; @@ -260,12 +260,12 @@ class LeftSideSolver Matrix Jhand; Matrix J; - Matrix Jv; - Matrix Jw; - Matrix AJvt; - Matrix lva,Rva; + Matrix Jv; + Matrix Jw; + Matrix AJvt; + Matrix lva,Rva; Matrix Lva; - Matrix qv; + Matrix qv; Matrix Z; Matrix OJwt; Matrix Lwa; diff --git a/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Geometry.h b/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Geometry.h index bf79a734..e46d6d8b 100644 --- a/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Geometry.h +++ b/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Geometry.h @@ -27,7 +27,7 @@ const double DEG2RAD=M_PI/180.0; const double RAD2DEG=180.0/M_PI; namespace cer { -namespace kinematics2 { +namespace kinematics_alt { class Vec3 { @@ -43,7 +43,7 @@ class Vec3 if (M.R!=3 || M.C!=1) { printf("Vec3(Matrix &M) invalid dimension ERROR\n"); - exit(-1); + exit(-1); } x=M(0); y=M(1); z=M(2); @@ -127,7 +127,7 @@ class Vec3 a=1.0/a; x*=a; y*=a; z*=a; - + return *this; } @@ -194,13 +194,13 @@ inline Vec3 operator*(double x,const Vec3& V) } inline Vec3 operator%(const Vec3& U,const Vec3& V) -{ - return Vec3(U.y*V.z-U.z*V.y,U.z*V.x-U.x*V.z,U.x*V.y-U.y*V.x); +{ + return Vec3(U.y*V.z-U.z*V.y,U.z*V.x-U.x*V.z,U.x*V.y-U.y*V.x); } inline double operator*(const Vec3& U,const Vec3& V) -{ - return U.x*V.x+U.y*V.y+U.z*V.z; +{ + return U.x*V.x+U.y*V.y+U.z*V.z; } class Quaternion @@ -273,7 +273,7 @@ class Rotation } Rotation(double Rx,double Ry,double Rz) - { + { Rx*=DEG2RAD; Ry*=DEG2RAD; Rz*=DEG2RAD; @@ -319,7 +319,7 @@ class Rotation //B.normalize(); Vec3 u=A%B; - + double st=u.mod(); double ct=A*B; double lct=1.0-ct; @@ -439,7 +439,7 @@ class Rotation double s = 0.5/r; Q.s = (m[0][2]-m[2][0])*s; Q.V.x = (m[0][1]+m[1][0])*s; - Q.V.y = 0.5*r; + Q.V.y = 0.5*r; Q.V.z = (m[2][1]+m[1][2])*s; } else if (m[2][2]>=m[0][0] && m[2][2]>=m[1][1]) @@ -471,11 +471,11 @@ class Rotation if (cosTh<=-1.0) { Vec3 U; - + if (m[0][0]>-1.0) U.x=sqrt(0.5*(1.0+m[0][0])); if (m[1][1]>-1.0) U.y=sqrt(0.5*(1.0+m[1][1])); if (m[2][2]>-1.0) U.z=sqrt(0.5*(1.0+m[2][2])); - + if (m[0][1]<0.0) U.x=-U.x; if (m[1][2]<0.0) U.z=-U.z; @@ -483,7 +483,7 @@ class Rotation } Vec3 U(m[2][1]-m[1][2],m[0][2]-m[2][0],m[1][0]-m[0][1]); - + return U.norm(acos(cosTh)); } diff --git a/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Joints.h b/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Joints.h index 7f5e220d..369a0644 100644 --- a/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Joints.h +++ b/libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Joints.h @@ -29,13 +29,13 @@ #define SET(V,j,W) { V(0,j)=W(0); V(1,j)=W(1); V(2,j)=W(2); } namespace cer { -namespace kinematics2 { +namespace kinematics_alt { class Component { public: - Component(const Transform &T0,Component* parent) + Component(const Transform &T0,Component* parent) : Tdir(T0),Tinv(T0.inv())//,JGoj(3,NJ) { mN=0; @@ -130,7 +130,7 @@ class Component { Vec3 ZjxP_G=Voj[j]+(Zoj[j]%(G-Poj[j])); - Jg(0,j)=ZjxP_G.x; + Jg(0,j)=ZjxP_G.x; Jg(1,j)=ZjxP_G.y; } @@ -199,7 +199,7 @@ class Joint : public Component vqmax[id]=qmax; ID=id++; } - + void setLimits(double q1,double q2) { qmin=q1; @@ -256,7 +256,7 @@ class Joint : public Component class Trifid : public Component { public: - Trifid(int& id,double size,double qa,double qb,double* ret_qmin,double* ret_qmax,Component* parent) + Trifid(int& id,double size,double qa,double qb,double* ret_qmin,double* ret_qmax,Component* parent) : Component(parent) { size*=0.001; @@ -267,7 +267,7 @@ class Trifid : public Component qmin=qa; qmax=qb; - + j0=id++; j1=id++; j2=id++; ret_qmin[j0]=qmin; ret_qmax[j0]=qmax; @@ -409,7 +409,7 @@ class Trifid : public Component else { if (bound[j0]!=1 && bound[j1]!=1 && bound[j2]!=1) - { + { if (q(j0)=m[1][1] && m[0][0]>=m[2][2]) { eig(0)=m[0][0]; if (m[1][1]>=m[2][2]) - { + { eig(1)=m[1][1]; eig(2)=m[2][2]; } @@ -531,7 +531,7 @@ class Matrix eig(0)=m[1][1]; if (m[2][2]>=m[0][0]) - { + { eig(1)=m[2][2]; eig(2)=m[0][0]; } @@ -546,7 +546,7 @@ class Matrix eig(0)=m[2][2]; if (m[0][0]>=m[1][1]) - { + { eig(1)=m[0][0]; eig(2)=m[1][1]; } @@ -571,7 +571,7 @@ class Matrix // In exact arithmetic for a symmetric matrix -1 <= r <= 1 // but computation error can leave it slightly outside this range. - + double phi; if (r<=-1.0) @@ -643,12 +643,12 @@ class Matrix void allocate() { m=new double*[R]; - + FOR_R(r) m[r]=new double[C]; } double abs(double x) const { return x>0.0?x:-x; } - double sgn(double x) const { return x>=0.0?1.0:-1.0; } + double sgn(double x) const { return x>=0.0?1.0:-1.0; } double** m; }; @@ -754,7 +754,7 @@ class DiagMatrix : public Matrix for (int i=0; i -using namespace cer::kinematics2; +using namespace cer::kinematics_alt; double Component::mBodyMass=0.0; double Component::mInvBodyMass=0.0; @@ -43,7 +43,7 @@ void Trifid::calcPosture(Matrix& q,const Transform& Tprec,Component *from,Vec3* Transform T0; - { + { Vec3 N(q2+q3-2.0*q1,SQRT3*(q3-q2),3.0*L); double N2=N*N; @@ -186,7 +186,7 @@ void Trifid::calcPosture(Matrix& q,const Transform& Tprec,Component *from,Vec3* else { static const Vec3 V3(0.0,0.0,1.0/3.0); - + double k1=-0.5*L; double k2= 0.5*SQRT3*L; @@ -278,7 +278,7 @@ void Trifid2::calcPostureUns(Matrix& q,Transform& Tprec,Component *from,Vec3* Po Transform T0; - { + { Vec3 N(q2+q3-2.0*q1,SQRT3*(q3-q2),3.0*L); double n=N.mod(); diff --git a/libraries/cer_kinematics_alt/src/Solver.cpp b/libraries/cer_kinematics_alt/src/Solver.cpp index af2a7002..be436a75 100644 --- a/libraries/cer_kinematics_alt/src/Solver.cpp +++ b/libraries/cer_kinematics_alt/src/Solver.cpp @@ -17,11 +17,11 @@ #include -using namespace cer::kinematics2; +using namespace cer::kinematics_alt; //Vec3 Werr=(mHand[L]->Toj.Rj().Ex()%Uxstar)+(mHand[L]->Toj.Rj().Ey()%Uystar)+(mHand[L]->Toj.Rj().Ez()%Uzstar); -LeftSideSolver::LeftSideSolver() : +LeftSideSolver::LeftSideSolver() : q(NJOINTS), qzero(NJOINTS), q_valid(NJOINTS), @@ -31,14 +31,14 @@ LeftSideSolver::LeftSideSolver() : S(12,12), Jhand(6,NJOINTS), J(6,12), - Jv(3,12), - Jw(3,12), + Jv(3,12), + Jw(3,12), AJvt(12,3), SJt(12,6), lva(3), - Rva(3,3), + Rva(3,3), Lva(3,3), - qv(12), + qv(12), Z(12,12), OJwt(12,3), Lwa(3,3), @@ -124,7 +124,7 @@ LeftSideSolver::LeftSideSolver() : mPart[13] = mRshoulder1; Component* Rshoulder2 = new Joint(id,-90,90,qmin,qmax,mRshoulder1); mJoint[nj++] = (Joint*)Rshoulder2; - Rshoulder2 = new Component(Transform(0,251,-90,90),Rshoulder2); + Rshoulder2 = new Component(Transform(0,251,-90,90),Rshoulder2); mUArm[R] = Rshoulder2; mPart[14] = Rshoulder2; @@ -172,14 +172,14 @@ LeftSideSolver::LeftSideSolver() : /////////////////////////////////////////////////////////////////////////////////// - for (int j=0; jToj.Rj().quaternion().conj()).V; - + Matrix Vref= Vstar; Matrix Wref=M_PI*Wstar; @@ -291,7 +291,7 @@ int LeftSideSolver::leftHand2Target(Vec3& Xstar,Quaternion& Qstar) if (fabs(Vrot(0))>5.0) Vrot(0)=(Vrot(0)>0.0?5.0:-5.0); if (fabs(Vrot(1))>5.0) Vrot(1)=(Vrot(1)>0.0?5.0:-5.0); if (fabs(Vrot(2))>5.0) Vrot(2)=(Vrot(2)>0.0?5.0:-5.0); - + qv=A*Jv.t()*Rv*Vrot; //////////////////////////////////////// @@ -318,7 +318,7 @@ int LeftSideSolver::leftHand2Target(Vec3& Xstar,Quaternion& Qstar) if (Vstar.mod()Toj.Rj().quaternion().conj()).V; - + Matrix Vref= Vstar; Matrix Wref=M_PI*Wstar; @@ -356,11 +356,11 @@ int LeftSideSolver::leftHand2Target(Vec3& Xstar,Quaternion& Qstar) (Jv*AJvt).base(lva,Rva); - //Lva(0,0)=lva(0)/(lva(0)*lva(0)+0.000004); + //Lva(0,0)=lva(0)/(lva(0)*lva(0)+0.000004); //Lva(1,1)=lva(1)/(lva(1)*lva(1)+0.000004); //Lva(2,2)=lva(2)/(lva(2)*lva(2)+0.000004); - Lva(0,0)=1.0/lva(0); + Lva(0,0)=1.0/lva(0); Lva(1,1)=1.0/lva(1); Lva(2,2)=1.0/lva(2); @@ -386,7 +386,7 @@ int LeftSideSolver::leftHand2Target(Vec3& Xstar,Quaternion& Qstar) (Jw*OJwt).base(lwa,Rwa); - Lwa(0,0)=1.0/lwa(0); + Lwa(0,0)=1.0/lwa(0); Lwa(1,1)=1.0/lwa(1); Lwa(2,2)=1.0/lwa(2); diff --git a/libraries/cer_kinematics_alt/tests/test-cer_kinematics_alt-tracking.cpp b/libraries/cer_kinematics_alt/tests/test-cer_kinematics_alt-tracking.cpp index 4a50d3e0..90c32f4d 100644 --- a/libraries/cer_kinematics_alt/tests/test-cer_kinematics_alt-tracking.cpp +++ b/libraries/cer_kinematics_alt/tests/test-cer_kinematics_alt-tracking.cpp @@ -31,7 +31,7 @@ #include -using namespace cer::kinematics2; +using namespace cer::kinematics_alt; class RobotThread : public yarp::os::RateThread { @@ -44,13 +44,13 @@ class RobotThread : public yarp::os::RateThread virtual void onStop(); void sendConfig(yarp::sig::Vector &q); - void sendCOM(std::string& name,int R,int G,int B,Vec3& P,double size,double alpha); - void sendTarget(const std::string& name,int R,int G,int B,double x,double y,double z,double size,double alpha); + void sendCOM(std::string& name,int R,int G,int B,Vec3& P,double size,double alpha); + void sendTarget(const std::string& name,int R,int G,int B,double x,double y,double z,double size,double alpha); protected: LeftSideSolver mRobot; - double finger[9]; + double finger[9]; yarp::os::BufferedPort portEncBase; yarp::os::BufferedPort portEncTorso; @@ -58,16 +58,16 @@ class RobotThread : public yarp::os::RateThread yarp::os::BufferedPort portEncLeftArm; yarp::os::BufferedPort portEncRightArm; - yarp::os::BufferedPort portObjects; + yarp::os::BufferedPort portObjects; }; RobotThread::RobotThread() : RateThread(int(PERIOD*1000.0)) //RateThread(int(PERIOD*1000.0)) { - finger[0]=30.0; - finger[1]=60.0; + finger[0]=30.0; + finger[1]=60.0; - for (int i=2; i<9; ++i) finger[i]=10.0; + for (int i=2; i<9; ++i) finger[i]=10.0; } bool RobotThread::threadInit() @@ -77,7 +77,7 @@ bool RobotThread::threadInit() portEncHead.open("/CERControl/head:o"); portEncLeftArm.open("/CERControl/left_arm:o"); portEncRightArm.open("/CERControl/right_arm:o"); - portObjects.open("/CERControl/objects:o"); + portObjects.open("/CERControl/objects:o"); yarp::os::Network::connect("/CERControl/base:o","/CERGui/base:i"); yarp::os::Network::connect("/CERControl/torso:o","/CERGui/torso:i"); @@ -86,10 +86,10 @@ bool RobotThread::threadInit() yarp::os::Network::connect("/CERControl/right_arm:o","/CERGui/right_arm:i"); yarp::os::Network::connect("/CERControl/objects:o","/CERGui/objects:i"); - yarp::os::Bottle& bot=portObjects.prepare(); - bot.clear(); - bot.addString("reset"); - portObjects.write(); + yarp::os::Bottle& bot=portObjects.prepare(); + bot.clear(); + bot.addString("reset"); + portObjects.write(); srand((unsigned)time(NULL)); @@ -102,89 +102,89 @@ void RobotThread::onStop() void RobotThread::threadRelease() { - portEncBase.interrupt(); + portEncBase.interrupt(); portEncTorso.interrupt(); portEncHead.interrupt(); portEncLeftArm.interrupt(); portEncRightArm.interrupt(); - portObjects.interrupt(); - + portObjects.interrupt(); + portEncBase.close(); portEncTorso.close(); portEncHead.close(); portEncLeftArm.close(); portEncRightArm.close(); - portObjects.close(); + portObjects.close(); } void RobotThread::run() { - yarp::sig::Matrix T(4,4); + yarp::sig::Matrix T(4,4); - static double ti=0.0; + static double ti=0.0; - T(0,0)= 0.0; T(0,1)= 0.0; T(0,2)= 1.0; T(0,3)=0.35;//radius*cos(DEG2RAD*alfa); - T(1,0)= 0.0; T(1,1)= 1.0; T(1,2)= 0.0; T(1,3)=0.1*cos(0.4*2.0*M_PI*ti);//radius*sin(DEG2RAD*alfa); - T(2,0)=-1.0; T(2,1)= 0.0; T(2,2)= 0.0; T(2,3)=0.1*sin(0.4*2.0*M_PI*ti);//0.7-0.63; - T(3,0)= 0.0; T(3,1)= 0.0; T(3,2)= 0.0; T(3,3)=1.0; + T(0,0)= 0.0; T(0,1)= 0.0; T(0,2)= 1.0; T(0,3)=0.35;//radius*cos(DEG2RAD*alfa); + T(1,0)= 0.0; T(1,1)= 1.0; T(1,2)= 0.0; T(1,3)=0.1*cos(0.4*2.0*M_PI*ti);//radius*sin(DEG2RAD*alfa); + T(2,0)=-1.0; T(2,1)= 0.0; T(2,2)= 0.0; T(2,3)=0.1*sin(0.4*2.0*M_PI*ti);//0.7-0.63; + T(3,0)= 0.0; T(3,1)= 0.0; T(3,2)= 0.0; T(3,3)=1.0; - ti+=PERIOD; + ti+=PERIOD; - sendTarget(std::string("target"),0,64,255,T(0,3),T(1,3),T(2,3),10.0,0.666); + sendTarget(std::string("target"),0,64,255,T(0,3),T(1,3),T(2,3),10.0,0.666); - yarp::sig::Vector qsol(22); + yarp::sig::Vector qsol(22); - double timeA,timeB; + double timeA,timeB; - timeA=yarp::os::Time::now(); + timeA=yarp::os::Time::now(); - bool success=mRobot.ikin(T,qsol); + bool success=mRobot.ikin(T,qsol); - timeB=yarp::os::Time::now(); + timeB=yarp::os::Time::now(); - sendConfig(qsol); + sendConfig(qsol); - printf("time = %d ms\n",int(1000.0*(timeB-timeA))); - //fflush(stdout); + printf("time = %d ms\n",int(1000.0*(timeB-timeA))); + //fflush(stdout); - //Vec3 COM=mRobot.getCOM(); - //COM.z=-0.63; - //sendCOM(std::string("G"),192,192,0,COM,16.0,1.0); + //Vec3 COM=mRobot.getCOM(); + //COM.z=-0.63; + //sendCOM(std::string("G"),192,192,0,COM,16.0,1.0); - //sendCOM(std::string("LUarm"),192,192,0,mRobot.G[3],30.0,0.5); - //sendCOM(std::string("LLarm"),192,192,0,mRobot.G[4],30.0,0.5); - //sendCOM(std::string("Lhand"),192,192,0,mRobot.G[5],30.0,0.5); + //sendCOM(std::string("LUarm"),192,192,0,mRobot.G[3],30.0,0.5); + //sendCOM(std::string("LLarm"),192,192,0,mRobot.G[4],30.0,0.5); + //sendCOM(std::string("Lhand"),192,192,0,mRobot.G[5],30.0,0.5); - //sendCOM(std::string("base"),192,192,0,mRobot.G[0],30.0,0.5); - //sendCOM(std::string("body"),192,192,0,mRobot.G[1],30.0,0.5); - //sendCOM(std::string("head"),192,192,0,mRobot.G[2],30.0,0.5); + //sendCOM(std::string("base"),192,192,0,mRobot.G[0],30.0,0.5); + //sendCOM(std::string("body"),192,192,0,mRobot.G[1],30.0,0.5); + //sendCOM(std::string("head"),192,192,0,mRobot.G[2],30.0,0.5); - //sendCOM(std::string("RUarm"),192,192,0,mRobot.G[6],30.0,0.5); - //sendCOM(std::string("RLarm"),192,192,0,mRobot.G[7],30.0,0.5); - //sendCOM(std::string("Rhand"),192,192,0,mRobot.G[8],30.0,0.5); + //sendCOM(std::string("RUarm"),192,192,0,mRobot.G[6],30.0,0.5); + //sendCOM(std::string("RLarm"),192,192,0,mRobot.G[7],30.0,0.5); + //sendCOM(std::string("Rhand"),192,192,0,mRobot.G[8],30.0,0.5); - /* - if (success) - { - printf("SUCCESS\n"); - sendTarget(target_name,0,200,0,T(0,3),T(1,3),T(2,3),10.0,1.0); - if (dumper) - { - fprintf(dumper,"1 %.3f %.3f %.3f ",T(0,3),T(1,3),T(2,3)); - //fprintf(dumper,"%.3f %.3f %.3f\n",COM.x,COM.y,COM.z); - } - } - else - { - printf("OUT_OF_REACH\n"); - sendTarget(target_name,255,0,32,T(0,3),T(1,3),T(2,3),10.0,1.0); - if (dumper) - { - fprintf(dumper,"0 %.3f %.3f %.3f ",T(0,3),T(1,3),T(2,3)); - //fprintf(dumper,"%.3f %.3f %.3f\n",COM.x,COM.y,COM.z); - } - } - */ + /* + if (success) + { + printf("SUCCESS\n"); + sendTarget(target_name,0,200,0,T(0,3),T(1,3),T(2,3),10.0,1.0); + if (dumper) + { + fprintf(dumper,"1 %.3f %.3f %.3f ",T(0,3),T(1,3),T(2,3)); + //fprintf(dumper,"%.3f %.3f %.3f\n",COM.x,COM.y,COM.z); + } + } + else + { + printf("OUT_OF_REACH\n"); + sendTarget(target_name,255,0,32,T(0,3),T(1,3),T(2,3),10.0,1.0); + if (dumper) + { + fprintf(dumper,"0 %.3f %.3f %.3f ",T(0,3),T(1,3),T(2,3)); + //fprintf(dumper,"%.3f %.3f %.3f\n",COM.x,COM.y,COM.z); + } + } + */ } void RobotThread::sendConfig(yarp::sig::Vector &q) @@ -193,7 +193,7 @@ void RobotThread::sendConfig(yarp::sig::Vector &q) { yarp::sig::Vector& enc=portEncBase.prepare(); enc.clear(); - + //Vec3 P=1000.*mRobot.getT().Pj(); //Vec3 R=mRobot.getT().Rj().rpy(); @@ -205,12 +205,12 @@ void RobotThread::sendConfig(yarp::sig::Vector &q) //enc.push_back(P.y); //enc.push_back(P.z); - enc.push_back(0.0); - enc.push_back(0.0); - enc.push_back(0.0); - enc.push_back(0.0); - enc.push_back(0.0); - enc.push_back(0.0); + enc.push_back(0.0); + enc.push_back(0.0); + enc.push_back(0.0); + enc.push_back(0.0); + enc.push_back(0.0); + enc.push_back(0.0); portEncBase.write(); } @@ -219,11 +219,11 @@ void RobotThread::sendConfig(yarp::sig::Vector &q) { yarp::sig::Vector& enc=portEncTorso.prepare(); enc.clear(); - enc.push_back(410.0+1000.0*q(0)); + enc.push_back(410.0+1000.0*q(0)); enc.push_back(410.0+1000.0*q(1)); enc.push_back(410.0+1000.0*q(2)); enc.push_back(q(3)); - portEncTorso.write(); + portEncTorso.write(); } if (portEncLeftArm.getOutputCount()>0) @@ -231,8 +231,8 @@ void RobotThread::sendConfig(yarp::sig::Vector &q) yarp::sig::Vector& enc=portEncLeftArm.prepare(); enc.clear(); for (int i=4; i<9; ++i) enc.push_back(q(i)); - for (int i=9; i<12; ++i) enc.push_back(1000.0*q(i)); - //for (int i=0; i<9; ++i) enc.push_back(finger[i]); + for (int i=9; i<12; ++i) enc.push_back(1000.0*q(i)); + //for (int i=0; i<9; ++i) enc.push_back(finger[i]); portEncLeftArm.write(); } @@ -241,12 +241,12 @@ void RobotThread::sendConfig(yarp::sig::Vector &q) yarp::sig::Vector& enc=portEncRightArm.prepare(); enc.clear(); for (int i=12; i<17; ++i) enc.push_back(q(i)); - for (int i=17; i<20; ++i) enc.push_back(1000.0*q(i)); + for (int i=17; i<20; ++i) enc.push_back(1000.0*q(i)); //for (int i=0; i<9; ++i) enc.push_back(finger[i]); - portEncRightArm.write(); + portEncRightArm.write(); } - if (portEncHead.getOutputCount()>0) + if (portEncHead.getOutputCount()>0) { yarp::sig::Vector& enc=portEncHead.prepare(); enc.clear(); @@ -257,42 +257,42 @@ void RobotThread::sendConfig(yarp::sig::Vector &q) void RobotThread::sendCOM(std::string& name,int R,int G,int B,Vec3& P,double size,double alpha) { - yarp::os::Bottle& botR=portObjects.prepare(); - botR.clear(); - botR.addString("object_with_label"); - botR.addString(name); botR.addString(name); - botR.addDouble(size); - botR.addDouble(0.0); - botR.addDouble(0.0); - botR.addDouble(P.x*1000.0); - botR.addDouble(P.y*1000.0); - botR.addDouble(P.z*1000.0); - //botR.addDouble(-630.0); - - botR.addDouble(0.0); botR.addDouble(0.0); botR.addDouble(0.0); - botR.addInt(R); botR.addInt(G); botR.addInt(B); - botR.addDouble(alpha); - //botR.addString("WORLD"); - portObjects.writeStrict(); + yarp::os::Bottle& botR=portObjects.prepare(); + botR.clear(); + botR.addString("object_with_label"); + botR.addString(name); botR.addString(name); + botR.addDouble(size); + botR.addDouble(0.0); + botR.addDouble(0.0); + botR.addDouble(P.x*1000.0); + botR.addDouble(P.y*1000.0); + botR.addDouble(P.z*1000.0); + //botR.addDouble(-630.0); + + botR.addDouble(0.0); botR.addDouble(0.0); botR.addDouble(0.0); + botR.addInt(R); botR.addInt(G); botR.addInt(B); + botR.addDouble(alpha); + //botR.addString("WORLD"); + portObjects.writeStrict(); } void RobotThread::sendTarget(const std::string& name,int R,int G,int B,double x,double y,double z,double size,double alpha) { - yarp::os::Bottle& botR=portObjects.prepare(); - botR.clear(); - botR.addString("object"); botR.addString(name); - botR.addDouble(size); - botR.addDouble(0.0); - botR.addDouble(0.0); - botR.addDouble(x*1000.0); - botR.addDouble(y*1000.0); - botR.addDouble(z*1000.0); - - botR.addDouble(0.0); botR.addDouble(0.0); botR.addDouble(0.0); - botR.addInt(R); botR.addInt(G); botR.addInt(B); - botR.addDouble(alpha); - //botR.addString("WORLD"); - portObjects.writeStrict(); + yarp::os::Bottle& botR=portObjects.prepare(); + botR.clear(); + botR.addString("object"); botR.addString(name); + botR.addDouble(size); + botR.addDouble(0.0); + botR.addDouble(0.0); + botR.addDouble(x*1000.0); + botR.addDouble(y*1000.0); + botR.addDouble(z*1000.0); + + botR.addDouble(0.0); botR.addDouble(0.0); botR.addDouble(0.0); + botR.addInt(R); botR.addInt(G); botR.addInt(B); + botR.addDouble(alpha); + //botR.addString("WORLD"); + portObjects.writeStrict(); } @@ -313,7 +313,7 @@ class CerTestModule : public yarp::os::RFModule virtual bool configure(yarp::os::ResourceFinder &rf) { yarp::os::Time::turboBoost(); - + mRobotThread=new RobotThread(); if (!mRobotThread->start()) @@ -349,24 +349,24 @@ class CerTestModule : public yarp::os::RFModule } virtual double getPeriod() - { - return 1.0; + { + return 1.0; } - + virtual bool updateModule() { if (isStopping()) { - mRobotThread->stop(); + mRobotThread->stop(); return false; } - - return true; + + return true; } - virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply) + virtual bool respond(const yarp::os::Bottle& command,yarp::os::Bottle& reply) { - if (command.get(0).asString()=="quit") return false; + if (command.get(0).asString()=="quit") return false; return true; }