[Fawkes Git] branch/tneumann/angle_distance_fix: 4 revs updated. (0.5.0-3084-g636d76b)

Tobias Neumann tobias.neumann at alumni.fh-aachen.de
Tue Feb 9 23:44:44 CET 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, tneumann/angle_distance_fix has been updated
  discards  27237fec6689b242dfae0b280123845f7f48170a (commit)
        to  636d76b2640a90582006efe163e47c08ed5cc919 (commit)
       via  f057b95c53f6652033d74df50b0ac453cc6bc4f1 (commit)
       via  fcaa24b7368abb1fa9f0389f1be995f6075e6b3f (commit)
       via  19e65a99f415740d5b06ef8d3229845fa36341be (commit)

This update added new revisions after undoing existing revisions.  That is
to say, the old revision is not a strict subset of the new revision.  This
situation occurs when you --force push a change and generate a repository
containing something like this:

 * -- * -- B -- O -- O -- O (27237fec6689b242dfae0b280123845f7f48170a)
            \
             N -- N -- N (636d76b2640a90582006efe163e47c08ed5cc919)

When this happens we assume that you've already had alert emails for all
of the O revisions, and so we here report only the revisions in the N
branch from the common base, B.

http://git.fawkesrobotics.org/fawkes.git/tneumann/angle_distance_fix

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 19e65a99f415740d5b06ef8d3229845fa36341be
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Tue Feb 9 22:07:45 2016 +0100
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Feb 9 23:27:49 2016 +0100

    angle.h: fix angle_distance, removed the signed return value
    
    the calculation returned a signed value which should be implemented in a
    function like angle_distance_signed(angle_to, angle_from)

http://git.fawkesrobotics.org/fawkes.git/commit/19e65a9
http://trac.fawkesrobotics.org/changeset/19e65a9

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit fcaa24b7368abb1fa9f0389f1be995f6075e6b3f
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Tue Feb 9 23:25:22 2016 +0100
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Feb 9 23:27:49 2016 +0100

    angle.h: add angle_distance_signed
    
    a function that returnes the distance from one angle towards the other
    angle

http://git.fawkesrobotics.org/fawkes.git/commit/fcaa24b
http://trac.fawkesrobotics.org/changeset/fcaa24b

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit f057b95c53f6652033d74df50b0ac453cc6bc4f1
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Tue Feb 9 23:40:23 2016 +0100
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Feb 9 23:43:56 2016 +0100

    jaco: removed call of abs around angle_distance
    
    the call of abs was not needed anymore since the
    semantic change of angle_distance to an unsigned function.

http://git.fawkesrobotics.org/fawkes.git/commit/f057b95
http://trac.fawkesrobotics.org/changeset/f057b95

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 636d76b2640a90582006efe163e47c08ed5cc919
Author:     Tobias Neumann <t.neumann at fh-aachen.de>
AuthorDate: Tue Feb 9 23:43:12 2016 +0100
Commit:     Tobias Neumann <t.neumann at fh-aachen.de>
CommitDate: Tue Feb 9 23:44:08 2016 +0100

    navgraph: removed call of abs around angle_distance
    
    the call of abs was not needed anymore since the semantic change of
    angle_distance to an unsigned function.

http://git.fawkesrobotics.org/fawkes.git/commit/636d76b
http://trac.fawkesrobotics.org/changeset/636d76b

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 src/libs/utils/math/angle.h               |   14 ++++++++++++--
 src/plugins/jaco/bimanual_goto_thread.cpp |    4 ++--
 src/plugins/jaco/goto_thread.cpp          |   14 +++++++-------
 src/plugins/navgraph/navgraph_thread.cpp  |    4 ++--
 4 files changed, 23 insertions(+), 13 deletions(-)


- *Diffs* -------------------------------------------------------------

- *commit* 19e65a99f415740d5b06ef8d3229845fa36341be - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Tue Feb 9 22:07:45 2016 +0100
Subject: angle.h: fix angle_distance, removed the signed return value

 src/libs/utils/math/angle.h |    4 +---
 1 files changed, 1 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/utils/math/angle.h b/src/libs/utils/math/angle.h
index b1420ab..f370f9f 100644
--- a/src/libs/utils/math/angle.h
+++ b/src/libs/utils/math/angle.h
@@ -120,8 +120,6 @@ normalize_bigmirror_rad(float angle_rad)
 
 
 /** Determines the distance between two angle provided as radians. 
- * Quadrants of the angles are considered to determine really the minimal
- * angle difference.
  * @param angle_rad1 first angle in radian
  * @param angle_rad2 second angle in radian
  * @return distance between the two angles
@@ -130,7 +128,7 @@ inline float
 angle_distance(float angle_rad1,
 	       float angle_rad2)
 {
-  return normalize_mirror_rad(angle_rad2 - angle_rad1);
+  return fabs( normalize_mirror_rad(angle_rad2 - angle_rad1) );
 }
 
 

- *commit* fcaa24b7368abb1fa9f0389f1be995f6075e6b3f - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Tue Feb 9 23:25:22 2016 +0100
Subject: angle.h: add angle_distance_signed

 src/libs/utils/math/angle.h |   12 ++++++++++++
 1 files changed, 12 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/utils/math/angle.h b/src/libs/utils/math/angle.h
index f370f9f..db4aad8 100644
--- a/src/libs/utils/math/angle.h
+++ b/src/libs/utils/math/angle.h
@@ -131,6 +131,18 @@ angle_distance(float angle_rad1,
   return fabs( normalize_mirror_rad(angle_rad2 - angle_rad1) );
 }
 
+/** Determines the signed distance between from "angle_from" to "angle_to" provided as radians. 
+ * @param angle_to angle to which the signed value is calculated
+ * @param angle_from angle from which the signed value is calculated
+ * @return signed distance from angle "angle_from" to "angle_to"
+ */
+inline float 
+angle_distance_signed(float angle_to, float angle_from)
+{
+  return normalize_mirror_rad(angle_to - angle_from);
+}
+
+
 
 } // end namespace fawkes
 

- *commit* f057b95c53f6652033d74df50b0ac453cc6bc4f1 - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Tue Feb 9 23:40:23 2016 +0100
Subject: jaco: removed call of abs around angle_distance

 src/plugins/jaco/bimanual_goto_thread.cpp |    4 ++--
 src/plugins/jaco/goto_thread.cpp          |   14 +++++++-------
 2 files changed, 9 insertions(+), 9 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/jaco/bimanual_goto_thread.cpp b/src/plugins/jaco/bimanual_goto_thread.cpp
index fe456f4..ea0b804 100644
--- a/src/plugins/jaco/bimanual_goto_thread.cpp
+++ b/src/plugins/jaco/bimanual_goto_thread.cpp
@@ -436,8 +436,8 @@ JacoBimanualGotoThread::_check_final()
       case TARGET_ANGULAR:
         //logger->log_debug(name(), "check[%u] final for TARGET ANGULAR", i);
         for( unsigned int j=0; j<6; ++j ) {
-          final &= std::abs( angle_distance(deg2rad(__v_arms[i]->target->pos.at(j)),
-                                            deg2rad(__v_arms[i]->arm->iface->joints(j))) ) < 0.05;
+          final &= angle_distance(deg2rad(__v_arms[i]->target->pos.at(j)),
+                                  deg2rad(__v_arms[i]->arm->iface->joints(j))) < 0.05;
         }
         break;
 
diff --git a/src/plugins/jaco/goto_thread.cpp b/src/plugins/jaco/goto_thread.cpp
index dbe3fea..15d620e 100644
--- a/src/plugins/jaco/goto_thread.cpp
+++ b/src/plugins/jaco/goto_thread.cpp
@@ -391,7 +391,7 @@ JacoGotoThread::_check_final()
       //logger->log_debug(name(), "check final for TARGET ANGULAR");
       //final = __arm->arm->final();
       for( unsigned int i=0; i<6; ++i ) {
-        final &= std::abs( angle_distance(deg2rad(__target->pos.at(i)), deg2rad(__arm->iface->joints(i))) ) < 0.05;
+        final &= (angle_distance(deg2rad(__target->pos.at(i)), deg2rad(__arm->iface->joints(i))) < 0.05);
       }
       __final_mutex->lock();
       __final = final;
@@ -410,12 +410,12 @@ JacoGotoThread::_check_final()
     default: //TARGET_CARTESIAN
       //logger->log_debug(name(), "check final for TARGET CARTESIAN");
       //final = __arm->arm->final();
-      final &= (std::abs(angle_distance(__target->pos.at(0) , __arm->iface->x())) < 0.01);
-      final &= (std::abs(angle_distance(__target->pos.at(1) , __arm->iface->y())) < 0.01);
-      final &= (std::abs(angle_distance(__target->pos.at(2) , __arm->iface->z())) < 0.01);
-      final &= (std::abs(angle_distance(__target->pos.at(3) , __arm->iface->euler1())) < 0.1);
-      final &= (std::abs(angle_distance(__target->pos.at(4) , __arm->iface->euler2())) < 0.1);
-      final &= (std::abs(angle_distance(__target->pos.at(5) , __arm->iface->euler3())) < 0.1);
+      final &= (angle_distance(__target->pos.at(0) , __arm->iface->x()) < 0.01);
+      final &= (angle_distance(__target->pos.at(1) , __arm->iface->y()) < 0.01);
+      final &= (angle_distance(__target->pos.at(2) , __arm->iface->z()) < 0.01);
+      final &= (angle_distance(__target->pos.at(3) , __arm->iface->euler1()) < 0.1);
+      final &= (angle_distance(__target->pos.at(4) , __arm->iface->euler2()) < 0.1);
+      final &= (angle_distance(__target->pos.at(5) , __arm->iface->euler3()) < 0.1);
       __final_mutex->lock();
       __final = final;
       __final_mutex->unlock();

- *commit* 636d76b2640a90582006efe163e47c08ed5cc919 - - - - - - - - - -
Author:  Tobias Neumann <t.neumann at fh-aachen.de>
Date:    Tue Feb 9 23:43:12 2016 +0100
Subject: navgraph: removed call of abs around angle_distance

 src/plugins/navgraph/navgraph_thread.cpp |    4 ++--
 1 files changed, 2 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/navgraph/navgraph_thread.cpp b/src/plugins/navgraph/navgraph_thread.cpp
index 3d528f6..4e96587 100644
--- a/src/plugins/navgraph/navgraph_thread.cpp
+++ b/src/plugins/navgraph/navgraph_thread.cpp
@@ -879,8 +879,8 @@ NavGraphThread::node_ori_reached(const NavGraphNode &node)
   if (node.has_property("orientation")) {
     float ori_tolerance = node.property_as_float("orientation_tolerance");
     float ori_diff  =
-      fabs( angle_distance( normalize_rad(tf::get_yaw(pose_.getRotation())),
-			    normalize_rad(node.property_as_float("orientation"))));
+      angle_distance( normalize_rad(tf::get_yaw(pose_.getRotation())),
+			    normalize_rad(node.property_as_float("orientation")));
 
     //logger->log_info(name(), "Ori=%f Rot=%f Diff=%f Tol=%f Dist=%f Tol=%f", cur_target.property_as_float("orientation"), tf::get_yaw(pose_.getRotation() ), ori_diff, ori_tolerance, dist, tolerance);
     return (ori_diff <= ori_tolerance);




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list