suzanne.soy/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added changes to KDL brought after the SVN merge, where applicable

Browse Source 
Taken from commits 470880e, df4c99f, cd0ae20, efb0823, c519989
This commit is contained in:
Yorik van Havre 2015-10-14 17:54:59 -03:00
parent 9304f7a78a
commit ba2e003b92
10 changed files with 32 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/Mod/Robot/App/kdl_cp/chaindynparam.hpp
View File

@ -57,7 +57,7 @@ namespace KDL {
const Chain chain;
int nr;
unsigned int nj;
unsigned int ns;
unsigned int ns;
Vector grav;
Vector vectornull;
JntArray jntarraynull;

6
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp
View File

@ -28,6 +28,10 @@
namespace KDL
{
// FIXME: seems this class is unused/unmaintained/unfinished for several years
// it supposed to be either fixer or removed
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the generalize pseudo inverse to calculate the velocity
@ -60,6 +64,8 @@ namespace KDL
* @param alpha the null-space velocity gain
*
*/
// FIXME: alpha is int but is initialized with a float value.
ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, double alpha = 0.25);
explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25);
~ChainIkSolverVel_pinv_nso();

24
src/Mod/Robot/App/kdl_cp/frameacc.hpp
View File

@ -45,18 +45,18 @@ class FrameAcc;
class RotationAcc;
class VectorAcc;
IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps);
IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps);
IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps);
IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps);
IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps);
class VectorAcc
{

2
src/Mod/Robot/App/kdl_cp/frames.cpp
View File

@ -387,7 +387,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const {
bool operator==(const Rotation& a,const Rotation& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b);
return Equal(a,b,epsilon);
#else
return ( a.data[0]==b.data[0] &&
a.data[1]==b.data[1] &&

10
src/Mod/Robot/App/kdl_cp/frames.inl
View File

@ -1267,7 +1267,7 @@ IMETHOD void posrandom(Frame& F) {
IMETHOD bool operator==(const Frame& a,const Frame& b ) {
#ifdef KDL_USE_EQUAL
return Equal(a,b);
return Equal(a,b,epsilon);
#else
return (a.p == b.p &&
a.M == b.M );
@ -1280,7 +1280,7 @@ IMETHOD bool operator!=(const Frame& a,const Frame& b) {
IMETHOD bool operator==(const Vector& a,const Vector& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b);
return Equal(a,b,epsilon);
#else
return (a.data[0]==b.data[0]&&
a.data[1]==b.data[1]&&
@ -1294,7 +1294,7 @@ IMETHOD bool operator!=(const Vector& a,const Vector& b) {
IMETHOD bool operator==(const Twist& a,const Twist& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b);
return Equal(a,b,epsilon);
#else
return (a.rot==b.rot &&
a.vel==b.vel );
@ -1307,7 +1307,7 @@ IMETHOD bool operator!=(const Twist& a,const Twist& b) {
IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
#ifdef KDL_USE_EQUAL
return Equal(a,b);
return Equal(a,b,epsilon);
#else
return (a.force==b.force &&
a.torque==b.torque );
@ -1323,7 +1323,7 @@ IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
IMETHOD bool operator==(const Vector2& a,const Vector2& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b);
return Equal(a,b,epsilon);
#else
return (a.data[0]==b.data[0]&&
a.data[1]==b.data[1] );

4
src/Mod/Robot/App/kdl_cp/jacobian.cpp
View File

@ -126,12 +126,12 @@ namespace KDL
bool Jacobian::operator ==(const Jacobian& arg)const
{
return Equal((*this),arg);
return Equal((*this),arg,epsilon);
}
bool Jacobian::operator!=(const Jacobian& arg)const
{
return !Equal((*this),arg);
return !Equal((*this),arg,epsilon);
}
bool Equal(const Jacobian& a,const Jacobian& b,double eps)

2
src/Mod/Robot/App/kdl_cp/jacobian.hpp
View File

@ -29,7 +29,7 @@ namespace KDL
{
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
class Jacobian;
bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon);
bool Equal(const Jacobian& a,const Jacobian& b,double eps);
class Jacobian

2
src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp
View File

@ -115,7 +115,7 @@ namespace KDL
return src1.data.isApprox(src2.data,eps);
}
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2,epsilon);};
//bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
}

4
src/Mod/Robot/App/kdl_cp/path_composite.cpp
View File

@ -132,13 +132,13 @@ int Path_Composite::GetNrOfSegments() {
Path* Path_Composite::GetSegment(int i) {
assert(i>=0);
assert(i<dv.size());
assert(i<static_cast<int>(dv.size()));
return gv[i].first;
}
double Path_Composite::GetLengthToEndOfSegment(int i) {
assert(i>=0);
assert(i<dv.size());
assert(i<static_cast<int>(dv.size()));
return dv[i];
}

2
src/Mod/Robot/App/kdl_cp/tree.cpp
View File

@ -33,8 +33,8 @@ Tree::Tree(const std::string& _root_name) :
Tree::Tree(const Tree& in) {
segments.clear();
nrOfSegments = 0;
nrOfJoints = 0;
nrOfSegments = 0;
root_name = in.root_name;
segments.insert(make_pair(root_name, TreeElement::Root(root_name)));