Skip to content

Commit

Permalink
Alpha version
Browse files Browse the repository at this point in the history
  • Loading branch information
Kirill Vostrecov committed Jun 21, 2011
1 parent 80030df commit e253c73
Show file tree
Hide file tree
Showing 37 changed files with 15,698 additions and 0 deletions.
26 changes: 26 additions & 0 deletions CAlgoAbstract.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
#ifndef __CALGOABSTRACT__
#define __CALGOABSTRACT__

#include "SharedTypes.h"
#include "RobotTypes.h"
#include "CRobot.h"

enum AlgType
{
JACOBIANTRANSPOSE,
JACOBIANPSEVDOINVERSE,
DUMPEDLEASTSQUARES,
SELECTIVEDUMPEDLEASTSQUARES
};

//Abstarct class for all algorithm
class CAlgoAbstract
{
public:
CAlgoAbstract(){}
virtual ~CAlgoAbstract(){}
virtual OUT VectorXf CalculateData() = 0;
virtual void SetAdditionalParametr(IN float & add_in) = 0;
};

#endif
31 changes: 31 additions & 0 deletions CAlgoFactory.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
#include "CAlgoFactory.h"
#include "CJacobianTranspose.h"
#include "CJacobianPseudoInverse.h"
#include "CDumpedLeastSquares.h"

CAlgoAbstract * CAlgoFactory::GiveMeSolver(
IN AlgType atype,
IN VectorXf & desired_position ,
IN CRobot & ptrRobot
)
{
CAlgoAbstract * ptr = NULL;

switch(atype)
{
case JACOBIANTRANSPOSE:
ptr = new CJacobianTranspose(desired_position , ptrRobot);
break;
case JACOBIANPSEVDOINVERSE:
ptr = new CJacobianPseudoInverse(desired_position , ptrRobot);
break;
case DUMPEDLEASTSQUARES:
ptr = new CDumpedLeastSquares(desired_position , ptrRobot);
break;
case SELECTIVEDUMPEDLEASTSQUARES:
//TODO
break;
}

return ptr;
}
18 changes: 18 additions & 0 deletions CAlgoFactory.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
#ifndef __CALGOFACTORY__
#define __CALGOFACTORY__

#include "CAlgoAbstract.h"
#include "CRobot.h"

class CAlgoFactory
{
public:
CAlgoFactory(){}
CAlgoAbstract * GiveMeSolver(
IN AlgType atype,
IN VectorXf & desired_position ,
IN CRobot & ptrRobot
);
};

#endif
112 changes: 112 additions & 0 deletions CConfigLoader.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,112 @@
#include "CConfigLoader.h"
#include <iostream>
#include "lexer.h"


dh_table & CConfigLoader::GetTable()
{
return for_load;
}

bool CConfigLoader::LoadXml()
{
dh_parametrs temp_struct;

std::string temp_string;
std::string out_temp_string;

Token tkn;

temp_struct.z_joint_type = NOTSET;

pugi::xml_document doc;
pugi::xml_parse_result result = doc.load_file(fname.c_str());
//Show robot name
#ifdef DEBUGOUTPUTFORXML
std::cout << "Load result: " << result.description() <<std::endl;
std::cout << "Robot name: " << doc.child("Robot").attribute("name").value() << std::endl;
#endif
pugi::xml_node tools = doc.child("Robot").child("Joints");
//Second method
for (pugi::xml_node tool = tools.child("joint"); tool; tool = tool.next_sibling("joint"))
{
//Get joint name
#ifdef DEBUGOUTPUTFORXML
std::cout << "Name: " << tool.attribute("name").value()<<std::endl;
#endif
temp_struct.joint_name = tool.attribute("name").value();
//Id is currently not needed
#ifdef DEBUGOUTPUTFORXML
std::cout << "id : " << tool.attribute("id").value()<<std::endl;
#endif
//Get alphai of joint
#ifdef DEBUGOUTPUTFORXML
std::cout << "alphai :" << tool.attribute("alphai").value()<<std::endl;
#endif
temp_struct.alpha = tool.attribute("alphai").as_float();
//Get a of joint
#ifdef DEBUGOUTPUTFORXML
std::cout << "ai :" << tool.attribute("ai").value()<<std::endl;
#endif
temp_struct.a = tool.attribute("ai").as_float();
//Get d of joint
#ifdef DEBUGOUTPUTFORXML
std::cout << "di :" << tool.attribute("di").value()<<std::endl;
#endif
temp_string = tool.attribute("di").value();
tkn = scan_string(temp_string.c_str(),temp_string.size(),out_temp_string);
switch (tkn)
{
case INT:
case FLOAT:
temp_struct.d = tool.attribute("di").as_float();
break;
case VAR:
temp_struct.d = (float)strtod(out_temp_string.c_str(), 0);
temp_struct.z_joint_type = PRISMATIC;
break;
case ERROR:
std::cout<<"Error. Cannot convert :"<< temp_string<<std::endl;
return false;
}
//Get theta of joint
#ifdef DEBUGOUTPUTFORXML
std::cout << "theta :" << tool.attribute("theta").value()<<std::endl;
#endif
temp_string = tool.attribute("theta").value();
tkn = scan_string(temp_string.c_str(),temp_string.size(),out_temp_string);
switch (tkn)
{
case INT:
case FLOAT:
temp_struct.theta = tool.attribute("di").as_float();
break;
case VAR:
temp_struct.theta = (float)strtod(out_temp_string.c_str(), 0);

if (temp_struct.z_joint_type == PRISMATIC)
{
std::cout<<"Error. Joint cannot be REVOLUTE and PRISMATIC at the same time."<<std::endl;
return false;
}

temp_struct.z_joint_type = REVOLUTE;
break;
case ERROR:
std::cout<<"Error. Cannot convert :"<< temp_string<<std::endl;
return false;
}

if (temp_struct.z_joint_type == NOTSET)
{
temp_struct.z_joint_type = CONSTANTJOINT;
}

//Save DH table field
for_load.push_back(temp_struct);
//Clear temporary string
temp_string.clear();
}

return true;
}
18 changes: 18 additions & 0 deletions CConfigLoader.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
#ifndef __CCONFIGLOADER__
#define __CCONFIGLOADER__

#include "src\pugixml.hpp"
#include "SharedTypes.h"
#include <string>

class CConfigLoader
{
std::string fname;
dh_table for_load;
public:
CConfigLoader(std::string & xml_name):fname(xml_name){}
bool LoadXml();
OUT dh_table & GetTable();
};

#endif
100 changes: 100 additions & 0 deletions CDumpedLeastSquares.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,100 @@
#include "CDumpedLeastSquares.h"
#include "CJacobian.h"
#include <iostream>

CDumpedLeastSquares::CDumpedLeastSquares( IN VectorXf & desired_position , IN CRobot & robot ):
mtxinstance(CMatrixFactory::GetInstance()),
_desired_position(desired_position),
_robot(robot)
{
current_position.resize(6);
}

OUT VectorXf CDumpedLeastSquares::CalculateData()
{
//Result vector
VectorXf delta_theta(_robot.GiveMeMatrixHolder().size());
//Vector for delta moving in space
VectorXf delta_translation(6);

VectorXf _temp = _desired_position;

CJacobian * jac = CJacobian::GetInstance();
//TODO
//should be a parameter of function
//Desired accuracy
float epsilon = 0.1f;

for (;;)
{
jac->CalculateJacobian(_robot.GiveMeMatrixHolder(),_robot.GiveMeJoints(),_robot.GiveMeFullHM());
//calculation delta
current_position << _robot.GiveMeFullHM()(0,3) , //X
_robot.GiveMeFullHM()(1,3) , //Y
_robot.GiveMeFullHM()(2,3) , //Z
0.0f, //Orientation
0.0f, //Orientation
0.0f; //Orientation

#ifdef JACOBIANDEBUGOUTPUT
std::cout<<"Current position"<<std::endl<<current_position<<std::endl;
std::cout<<"Desired position"<<std::endl<<_desired_position<<std::endl;
#endif

delta_translation = _desired_position - current_position;

#ifdef JACOBIANDEBUGOUTPUT
std::cout<<"Delta position"<<std::endl<<delta_translation<<std::endl;
#endif
//compare delta with desired accuracy
float n = delta_translation.norm();
if (n < epsilon)
{
//Done
break;
}

//Algorithm
//TODO optimization needed

MatrixXf one = jac->GetJacobian();
MatrixXf two = one * jac->GetJacobian().transpose();
MatrixXf id = MatrixXf::Identity(two.rows() , two.cols());
id = (nu*nu) * id;

MatrixXf result = two + id ;
MatrixXf result_out;

JacobiSVD<MatrixXf> svd;
svd.compute(result , Eigen::ComputeThinU | Eigen::ComputeThinV);
svd.pinv(result_out);

#ifdef JACOBIANDEBUGOUTPUT
std::cout<<"Result"<<std::endl<<result_out<<std::endl;
#endif

result_out = jac->GetJacobian().transpose() * result_out;

delta_theta = result_out * delta_translation;

#ifdef JACOBIANDEBUGOUTPUT
std::cout<<"Delta theta"<<std::endl<<delta_theta<<std::endl;
#endif
UpdateJoints(delta_theta);
}

return _temp;
}

void CDumpedLeastSquares::UpdateJoints( VectorXf & delta_theta )
{
JointHandler & _j = _robot.GiveMeJoints();

for (unsigned int i = 0 ; i < delta_theta.size() ; i++)
{
//TODO ADD CONSTRAINTS
//new var = old var + delta var
float res = delta_theta[i] + _j[i].GiveMeVariableParametr();
_robot.SetJointVariable(i,res);
}
}
25 changes: 25 additions & 0 deletions CDumpedLeastSquares.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
#ifndef __CDUMPLEASTSQUARES__
#define __CDUMPLEASTSQUARES__

#include "CAlgoAbstract.h"
#include "CMatrixFactory.h"
#include "CRobot.h"

class CDumpedLeastSquares : public CAlgoAbstract
{
CMatrixFactory * mtxinstance;
VectorXf & _desired_position;
CRobot & _robot;
VectorXf current_position;
float nu;
public:
CDumpedLeastSquares(
IN VectorXf & desired_position ,
IN CRobot & robot
);
OUT VectorXf CalculateData();
void SetAdditionalParametr(IN float & add_in){ nu = add_in;}
void UpdateJoints(VectorXf & delta_theta);
};

#endif
Loading

0 comments on commit e253c73

Please sign in to comment.