Skip to content

Commit

Permalink
Robot: PR6497 move return statement to new line
Browse files Browse the repository at this point in the history
  • Loading branch information
chennes committed Mar 29, 2022
1 parent 1e9236a commit ec74d35
Show file tree
Hide file tree
Showing 7 changed files with 36 additions and 18 deletions.
3 changes: 2 additions & 1 deletion src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,8 @@ namespace KDL

const char* ChainIkSolverPos_NR::strError(const int error) const
{
if (E_IKSOLVER_FAILED == error) return "Child IK solver failed";
if (E_IKSOLVER_FAILED == error)
return "Child IK solver failed";
else return SolverI::strError(error);
}
}
Expand Down
3 changes: 2 additions & 1 deletion src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,8 @@ namespace KDL

const char* ChainIkSolverVel_pinv::strError(const int error) const
{
if (E_SVD_FAILED == error) return "SVD failed";
if (E_SVD_FAILED == error)
return "SVD failed";
else return SolverI::strError(error);
}
}
3 changes: 2 additions & 1 deletion src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,8 @@ namespace KDL

const char* ChainIkSolverVel_wdls::strError(const int error) const
{
if (E_SVD_FAILED == error) return "SVD failed";
if (E_SVD_FAILED == error)
return "SVD failed";
else return SolverI::strError(error);
}

Expand Down
3 changes: 2 additions & 1 deletion src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,8 @@ namespace KDL

const char* ChainJntToJacSolver::strError(const int error) const
{
if (E_JAC_FAILED == error) return "Jac Failed";
if (E_JAC_FAILED == error)
return "Jac Failed";
else return SolverI::strError(error);
}
}
Expand Down
6 changes: 4 additions & 2 deletions src/Mod/Robot/App/kdl_cp/tree.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -125,12 +125,14 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string&
parents_chain_root.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
if (parents_chain_root.empty() || parents_chain_root.back() != root_name)
return false;
for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){
parents_chain_tip.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name)
return false;

// remove common part of segment lists
SegmentMap::key_type last_segment = root_name;
Expand Down
3 changes: 2 additions & 1 deletion src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,8 @@ namespace KDL {
delta_twist->second = diff(f_it->second, f_des_it->second);
}
double res = iksolver.CartToJnt(q_out, delta_twists, delta_q);
if (res < eps) return res;
if (res < eps)
return res;

Add(q_out, delta_q, q_out);

Expand Down
33 changes: 22 additions & 11 deletions src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,14 +58,16 @@ namespace KDL{
s += U(k,i)*U(k,i);
}
f=U(i,i); // f is the diag elem
if (!(s>=0)) return -3;
if (!(s>=0))
return -3;
g = -SIGN(sqrt(s),f);
h=f*g-s;
U(i,i)=f-g;
for (j=ppi;j<cols;j++) {
// dot product of columns i and j, starting from the i-th row
for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
if (!(h!=0)) return -4;
if (!(h!=0))
return -4;
f=s/h;
// copy the scaled i-th column into the j-th column
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
Expand All @@ -85,11 +87,13 @@ namespace KDL{
s += U(i,k)*U(i,k);
}
f=U(i,ppi);
if (!(s>=0)) return -5;
if (!(s>=0))
return -5;
g = -SIGN(sqrt(s),f);
h=f*g-s;
U(i,ppi)=f-g;
if (!(h!=0)) return -6;
if (!(h!=0))
return -6;
for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
for (j=ppi;j<rows;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
Expand All @@ -106,7 +110,8 @@ namespace KDL{
for (i=cols-1;i>=0;i--) {
if (i<cols-1) {
if (fabs(g)>epsilon) {
if (!(U(i,ppi)!=0)) return -7;
if (!(U(i,ppi)!=0))
return -7;
for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
Expand All @@ -128,7 +133,8 @@ namespace KDL{
g=1.0/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
if (!(U(i,i)!=0)) return -8;
if (!(U(i,i)!=0))
return -8;
f=(s/U(i,i))*g;
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
}
Expand Down Expand Up @@ -161,7 +167,8 @@ namespace KDL{
g=S(i);
h=PYTHAG(f,g);
S(i)=h;
if (!(h!=0)) return -9;
if (!(h!=0))
return -9;
h=1.0/h;
c=g*h;
s=(-f*h);
Expand All @@ -188,12 +195,15 @@ namespace KDL{
y=S(nm);
g=tmp(nm);
h=tmp(k);
if (!(h!=0&&y!=0)) return -10;
if (!(h!=0&&y!=0))
return -10;
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);

g=PYTHAG(f,1.0);
if (!(x!=0)) return -11;
if (!((f+SIGN(g,f))!=0)) return -12;
if (!(x!=0))
return -11;
if (!((f+SIGN(g,f))!=0))
return -12;
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;

/* Next QR transformation: */
Expand All @@ -206,7 +216,8 @@ namespace KDL{
g=c*g;
z=PYTHAG(f,h);
tmp(j)=z;
if (!(z!=0)) return -13;
if (!(z!=0))
return -13;
c=f/z;
s=h/z;
f=x*c+g*s;
Expand Down

0 comments on commit ec74d35

Please sign in to comment.