Skip to content

Commit 69c756e

Browse files
meyerjmeyerj
committed
Merge branch 'master' into francofusco-segments-removal
2 parents f591582 + b35c424 commit 69c756e

37 files changed

+530
-414
lines changed

‎.travis.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ env:
99

1010
before_script:
1111
- sudo apt-get -qq update
12-
- sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev
12+
- sudo apt-get install -y libeigen3-dev libcppunit-dev python-sip-dev python-psutil
1313
#build orocos_kdl
1414
- cd orocos_kdl
1515
- mkdir build

‎orocos_kdl/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,9 @@
22
# Test CMake version
33
#
44
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
5+
IF(POLICY CMP0048)
6+
CMAKE_POLICY(SET CMP0048 NEW)
7+
ENDIF()
58
#MARK_AS_ADVANCED( FORCE CMAKE_BACKWARDS_COMPATIBILITY )
69

710

‎orocos_kdl/src/chainfdsolver_recursive_newton_euler.cpp

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -70,64 +70,64 @@ namespace KDL{
7070
return (error);
7171

7272
// Calculate non-inertial internal torques by inputting zero joint acceleration to ID
73-
for(int i=0;i<nj;i++){
73+
for(unsigned int i=0;i<nj;i++){
7474
q_dotdot(i) = 0.;
7575
}
7676
error = IdSolver.CartToJnt(q, q_dot, q_dotdot, f_ext, Tzeroacc);
7777
if (error < 0)
7878
return (error);
7979

8080
// Calculate acceleration using inverse symmetric matrix times vector
81-
for(int i=0;i<nj;i++){
81+
for(unsigned int i=0;i<nj;i++){
8282
Tzeroacc_eig(i) = (torques(i)-Tzeroacc(i));
83-
for(int j=0;j<nj;j++){
83+
for(unsigned int j=0;j<nj;j++){
8484
H_eig(i,j) = H(i,j);
8585
}
8686
}
8787
ldl_solver_eigen(H_eig, Tzeroacc_eig, L_eig, D_eig, r_eig, acc_eig);
88-
for(int i=0;i<nj;i++){
88+
for(unsigned int i=0;i<nj;i++){
8989
q_dotdot(i) = acc_eig(i);
9090
}
9191

9292
return (error = E_NOERROR);
9393
}
9494

95-
void ChainFdSolver_RNE::RK4Integrator(int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
95+
void ChainFdSolver_RNE::RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
9696
KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
9797
KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
9898
KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp)
9999
{
100100
fdsolver.CartToJnt(q, q_dot, torques, f_ext, q_dotdot);
101-
for (int i=0; i<nj; ++i)
101+
for (unsigned int i=0; i<nj; ++i)
102102
{
103103
q_temp(i) = q(i) + q_dot(i)*dt/2.0;
104104
q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
105105
dq(i) = q_dot(i);
106106
dq_dot(i) = q_dotdot(i);
107107
}
108108
fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
109-
for (int i=0; i<nj; ++i)
109+
for (unsigned int i=0; i<nj; ++i)
110110
{
111111
q_temp(i) = q(i) + q_dot_temp(i)*dt/2.0;
112112
q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt/2.0;
113113
dq(i) += 2.0*q_dot_temp(i);
114114
dq_dot(i) += 2.0*q_dotdot(i);
115115
}
116116
fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
117-
for (int i=0; i<nj; ++i)
117+
for (unsigned int i=0; i<nj; ++i)
118118
{
119119
q_temp(i) = q(i) + q_dot_temp(i)*dt;
120120
q_dot_temp(i) = q_dot(i) + q_dotdot(i)*dt;
121121
dq(i) += 2.0*q_dot_temp(i);
122122
dq_dot(i) += 2.0*q_dotdot(i);
123123
}
124124
fdsolver.CartToJnt(q_temp, q_dot_temp, torques, f_ext, q_dotdot);
125-
for (int i=0; i<nj; ++i)
125+
for (unsigned int i=0; i<nj; ++i)
126126
{
127127
dq(i) = (dq(i)+q_dot_temp(i))*dt/6.0;
128128
dq_dot(i) = (dq_dot(i)+q_dotdot(i))*dt/6.0;
129129
}
130-
for (int i=0; i<nj; ++i)
130+
for (unsigned int i=0; i<nj; ++i)
131131
{
132132
q(i) += dq(i);
133133
q_dot(i) += dq_dot(i);

‎orocos_kdl/src/chainfdsolver_recursive_newton_euler.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,7 @@ namespace KDL{
8686
* \param qtemp Intermediate joint positions
8787
* \param qdtemp Intermediate joint velocities
8888
*/
89-
void RK4Integrator(int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
89+
void RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
9090
KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
9191
KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
9292
KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp);
@@ -97,8 +97,8 @@ namespace KDL{
9797
ChainIdSolver_RNE IdSolver;
9898
unsigned int nj;
9999
unsigned int ns;
100-
JntArray Tzeroacc;
101100
JntSpaceInertiaMatrix H;
101+
JntArray Tzeroacc;
102102
Eigen::MatrixXd H_eig;
103103
Eigen::VectorXd Tzeroacc_eig;
104104
Eigen::MatrixXd L_eig;

‎orocos_kdl/src/chainiksolverpos_lma.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -321,4 +321,4 @@ const char* ChainIkSolverPos_LMA::strError(const int error) const
321321
else return SolverI::strError(error);
322322
}
323323

324-
};//namespace KDL
324+
}//namespace KDL

‎orocos_kdl/src/chainiksolverpos_lma.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -246,7 +246,7 @@ class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
246246

247247

248248

249-
}; // namespace KDL
249+
} // namespace KDL
250250

251251

252252

‎orocos_kdl/src/chainiksolvervel_pinv.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ namespace KDL
7777
* not (yet) implemented.
7878
*
7979
*/
80-
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
80+
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
8181

8282
/**
8383
* Retrieve the number of singular values of the jacobian that are < eps;

‎orocos_kdl/src/chainiksolvervel_pinv_givens.cpp

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,6 @@ namespace KDL
3838
B(m,n),
3939
S(n),
4040
tempi(m),
41-
tempj(m),
4241
UY(VectorXd::Zero(6)),
4342
SUY(VectorXd::Zero(nj)),
4443
qdot_eigen(nj),
@@ -59,7 +58,6 @@ namespace KDL
5958
B.conservativeResize(m,n);
6059
S.conservativeResize(n);
6160
tempi.conservativeResize(m);
62-
tempj.conservativeResize(n);
6361
SUY.conservativeResizeLike(VectorXd::Zero(nj));
6462
qdot_eigen.conservativeResize(nj);
6563
}

‎orocos_kdl/src/chainiksolvervel_pinv_givens.hpp

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -30,10 +30,6 @@ namespace KDL
3030
*
3131
* @param chain the chain to calculate the inverse velocity
3232
* kinematics for
33-
* @param eps if a singular value is below this value, its
34-
* inverse is set to zero, default: 0.00001
35-
* @param maxiter maximum iterations for the svd calculation,
36-
* default: 150
3733
*
3834
*/
3935
explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
@@ -44,7 +40,7 @@ namespace KDL
4440
* not (yet) implemented.
4541
*
4642
*/
47-
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);};
43+
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};
4844

4945
/// @copydoc KDL::SolverI::updateInternalDataStructures
5046
virtual void updateInternalDataStructures();
@@ -57,7 +53,7 @@ namespace KDL
5753
bool transpose,toggle;
5854
unsigned int m,n;
5955
MatrixXd jac_eigen,U,V,B;
60-
VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen;
56+
VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen;
6157
};
6258
}
6359
#endif

‎orocos_kdl/src/chainiksolvervel_pinv_nso.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ namespace KDL
6969
* not (yet) implemented.
7070
*
7171
*/
72-
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
72+
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
7373

7474
/**
7575
* Request the joint weights for optimization criterion

‎orocos_kdl/src/chainiksolvervel_wdls.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ namespace KDL
102102
* not (yet) implemented.
103103
*
104104
*/
105-
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
105+
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
106106

107107
/**
108108
* Set the joint space weighting matrix

‎orocos_kdl/src/frames.hpp

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -310,8 +310,8 @@ class Rotation
310310
double Xy,double Yy,double Zy,
311311
double Xz,double Yz,double Zz);
312312
inline Rotation(const Vector& x,const Vector& y,const Vector& z);
313-
// default copy constructor is sufficient
314313

314+
inline Rotation(const Rotation& arg);
315315

316316
inline Rotation& operator=(const Rotation& arg);
317317

@@ -1060,6 +1060,8 @@ class Rotation2
10601060

10611061
Rotation2(double ca,double sa):s(sa),c(ca){}
10621062

1063+
Rotation2(const Rotation2& arg);
1064+
10631065
inline Rotation2& operator=(const Rotation2& arg);
10641066
inline Vector2 operator*(const Vector2& v) const;
10651067
//! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set

‎orocos_kdl/src/frames.inl

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -512,6 +512,11 @@ Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z)
512512
data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2];
513513
}
514514

515+
Rotation::Rotation(const Rotation& arg) {
516+
int count=9;
517+
while (count--) data[count] = arg.data[count];
518+
}
519+
515520
Rotation& Rotation::operator=(const Rotation& arg) {
516521
int count=9;
517522
while (count--) data[count] = arg.data[count];
@@ -839,7 +844,9 @@ IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_som
839844
data[1]=tmp(1);
840845
}
841846

842-
847+
IMETHOD Rotation2::Rotation2(const Rotation2& arg) {
848+
c=arg.c;s=arg.s;
849+
}
843850

844851
IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) {
845852
c=arg.c;s=arg.s;

‎orocos_kdl/src/jntarray.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -117,8 +117,8 @@ namespace KDL
117117
return src1.data.isApprox(src2.data,eps);
118118
}
119119

120-
bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
121-
//bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
120+
bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
121+
//bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}
122122

123123
}
124124

‎orocos_kdl/src/joint.hpp

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -202,6 +202,26 @@ namespace KDL {
202202
return inertia;
203203
};
204204

205+
/**
206+
* Request the damping of the joint.
207+
*
208+
* @return const reference to the damping of the joint
209+
*/
210+
const double& getDamping() const
211+
{
212+
return damping;
213+
};
214+
215+
/**
216+
* Request the stiffness of the joint.
217+
*
218+
* @return const reference to the stiffness of the joint
219+
*/
220+
const double& getStiffness() const
221+
{
222+
return stiffness;
223+
};
224+
205225
virtual ~Joint();
206226

207227
private:

‎orocos_kdl/src/kinfam_io.cpp

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121

2222
#include "kinfam_io.hpp"
2323
#include "frames_io.hpp"
24+
#include <sstream>
2425

2526
namespace KDL {
2627
std::ostream& operator <<(std::ostream& os, const Joint& joint) {
@@ -111,5 +112,21 @@ std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspac
111112
std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix) {
112113
return is;
113114
}
115+
116+
117+
std::string tree2str(const SegmentMap::const_iterator it, const std::string& separator, const std::string& preamble, unsigned int level) {
118+
std::stringstream out;
119+
out << preamble;
120+
for(unsigned int i=0; i<level; i++)
121+
out << separator;
122+
out << it->first << "(q_nr: " << GetTreeElementQNr(it->second) << ")\n";
123+
for (unsigned int i = 0; i < GetTreeElementChildren(it->second).size(); i++)
124+
out << tree2str(GetTreeElementChildren(it->second)[i], separator, preamble, level+1);
125+
return out.str();
114126
}
115127

128+
std::string tree2str(const Tree& tree, const std::string& separator, const std::string& preamble) {
129+
return tree2str(tree.getRootSegment(), separator, preamble, 0);
130+
}
131+
132+
}

‎orocos_kdl/src/kinfam_io.hpp

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,13 @@ std::istream& operator >>(std::istream& is, Jacobian& jac);
5353
std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix);
5454
std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix);
5555

56+
//Builds a string containing the "branches" of a Tree using indentation or another
57+
//user-supplied pattern, so that it is easier to visualize its structure. It is
58+
//also possible to specify a "preamble", ie, a string to be included at the
59+
//beginning of each new line.
60+
std::string tree2str(const Tree& tree, const std::string& separator=" ", const std::string& preamble="");
61+
std::string tree2str(const SegmentMap::const_iterator it, const std::string& separator=" ", const std::string& preamble="", unsigned int level=0);
62+
5663
/*
5764
template<typename T>
5865
std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) {
@@ -72,4 +79,3 @@ std::istream& operator >>(std::istream& is, std::vector<T>& vec) {
7279
*/
7380
}
7481
#endif
75-

‎orocos_kdl/src/path_roundedcomposite.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -189,8 +189,8 @@ void Path_RoundedComposite::GetCurrentSegmentLocation(double s,
189189

190190

191191
Path_RoundedComposite::~Path_RoundedComposite() {
192-
if (aggregate)
193-
delete orient;
192+
if (aggregate)
193+
delete orient;
194194
delete comp;
195195
}
196196

‎orocos_kdl/src/rotational_interpolation_sa.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ namespace KDL {
4747

4848

4949
RotationalInterpolation_SingleAxis::RotationalInterpolation_SingleAxis()
50-
{};
50+
{}
5151

5252
void RotationalInterpolation_SingleAxis::SetStartEnd(Rotation start,Rotation end) {
5353
R_base_start = start;

0 commit comments

Comments
 (0)