Skip to content

Commit

Permalink
Removed tabs, namespace changed from cer::kinematics2 to cer::kinemat…
Browse files Browse the repository at this point in the history 
…ics_alt
  • Loading branch information
@ale-git
ale-git committed Dec 8, 2015
1 parent 9b95cc8 commit fb05a95
Show file tree
Hide file tree
Showing 7 changed files with 205 additions and 205 deletions.
58 changes: 29 additions & 29 deletions libraries/cer_kinematics_alt/include/cer_kinematics_alt/Solver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
#include <cer_kinematics_alt/private/Joints.h>

namespace cer {
namespace kinematics2 {
namespace kinematics_alt {

#define NJOINTS 22
#define PERIOD 0.01 // seconds
Expand All @@ -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
Expand All @@ -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)
{
Expand All @@ -87,29 +87,29 @@ 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.
*/
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; j<N; ++j) qconf(j)=qin(j);

mRoot->calcPosture(qconf,T_ROOT);

Rotation& R=mPart[frame]->Toj.Rj();
Expand All @@ -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.
*/
Expand All @@ -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;

Expand All @@ -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<N; ++j) qout(j)=q(j);
Expand Down Expand Up @@ -190,7 +190,7 @@ class LeftSideSolver

return true;
}

return false;
}

Expand All @@ -210,7 +210,7 @@ class LeftSideSolver
{
for (int j=0; j<NJOINTS; ++j)
{
if (ql(j)<=qmin[j])
if (ql(j)<=qmin[j])
{
ql(j)=qmin[j];
}
Expand All @@ -224,16 +224,16 @@ class LeftSideSolver
void limitJointSpeeds(Matrix& ql,Matrix& qd)
{
for (int j=0; j<NJOINTS; ++j)
{
if (ql(j)<=qmin[j])
{
if (ql(j)<=qmin[j])
{
if (qd(j)<0.0)
{
qd(j)=0.0;
}
}
else if (ql(j)>=qmax[j])
{
{
if (qd(j)>0.0)
{
qd(j)=0.0;
Expand All @@ -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;
Expand Down
26 changes: 13 additions & 13 deletions libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Geometry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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);
Expand Down Expand Up @@ -127,7 +127,7 @@ class Vec3
a=1.0/a;

x*=a; y*=a; z*=a;

return *this;
}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -273,7 +273,7 @@ class Rotation
}

Rotation(double Rx,double Ry,double Rz)
{
{
Rx*=DEG2RAD;
Ry*=DEG2RAD;
Rz*=DEG2RAD;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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])
Expand Down Expand Up @@ -471,19 +471,19 @@ 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;

return M_PI*U.norm();
}

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));
}

Expand Down
14 changes: 7 additions & 7 deletions libraries/cer_kinematics_alt/include/cer_kinematics_alt/private/Joints.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -199,7 +199,7 @@ class Joint : public Component
vqmax[id]=qmax;
ID=id++;
}

void setLimits(double q1,double q2)
{
qmin=q1;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -409,7 +409,7 @@ class Trifid : public Component
else
{
if (bound[j0]!=1 && bound[j1]!=1 && bound[j2]!=1)
{
{
if (q(j0)<qmax && q(j1)<qmax && q(j2)<qmax)
{
qp(j0)+=v; qp(j1)+=v; qp(j2)+=v;
Expand Down
Loading

0 comments on commit fb05a95

Please sign in to comment.