kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Robot: PR6497 move return statement to new line

Browse Source
This commit is contained in:
Chris Hennes
2022-03-29 13:24:17 -05:00
parent 1e9236abd4
commit ec74d351f0
7 changed files with 36 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp
View File

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

3
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp
View File

@@ -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
src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp
View File

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

3
src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp
View File

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

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

@@ -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;

3
src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp
View File

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

33
src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp
View File

@@ -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);
@@ -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);
@@ -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);
@@ -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);
}
@@ -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);
@@ -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: */
@@ -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;