Revision: 6615
http://playerstage.svn.sourceforge.net/playerstage/?rev=6615&view=rev
Author: thjc
Date: 2008-06-16 20:35:12 -0700 (Mon, 16 Jun 2008)
Log Message:
-----------
merged p2os kinematics fix from trunk [6579,6584]
Modified Paths:
--------------
code/player/branches/release-2-1-patches/server/drivers/mixed/p2os/kinecalc.cc
Modified:
code/player/branches/release-2-1-patches/server/drivers/mixed/p2os/kinecalc.cc
===================================================================
---
code/player/branches/release-2-1-patches/server/drivers/mixed/p2os/kinecalc.cc
2008-06-17 03:24:37 UTC (rev 6614)
+++
code/player/branches/release-2-1-patches/server/drivers/mixed/p2os/kinecalc.cc
2008-06-17 03:35:12 UTC (rev 6615)
@@ -28,6 +28,7 @@
*/
#include <libplayercore/playercommon.h>
+#include <libplayercore/error.h>
#include "kinecalc.h"
#include <math.h>
@@ -129,9 +130,19 @@
{
KineVector result;
double length = sqrt (vector.x * vector.x + vector.y * vector.y + vector.z *
vector.z);
- result.x = vector.x / length;
- result.y = vector.y / length;
- result.z = vector.z / length;
+ if (length != 0)
+ {
+ result.x = vector.x / length;
+ result.y = vector.y / length;
+ result.z = vector.z / length;
+ }
+ else
+ {
+ PLAYER_WARN ("P2OS: Tried to normalise a vector of zero length.");
+ result.x = 0;
+ result.y = 0;
+ result.z = 0;
+ }
return result;
}
@@ -141,6 +152,27 @@
result.x = pose.o.y * pose.a.z - pose.a.y * pose.o.z;
result.y = pose.o.z * pose.a.x - pose.a.z * pose.o.x;
result.z = pose.o.x * pose.a.y - pose.a.x * pose.o.y;
+ if (result.x == 0 && result.y == 0 && result.z == 0)
+ {
+ PLAYER_WARN ("P2OS: Approach and orientation cannot be the same vector -
their cross product cannot be zero.");
+ // Ensures that a different orientation vector is created
+ KineVector orient;
+ if (pose.a.y == 0 && pose.a.z == 0)
+ {
+ orient.x = 0;
+ orient.y = 1;
+ orient.z = 0;
+ }
+ else
+ {
+ orient.x = 1;
+ orient.y = 0;
+ orient.z = 0;
+ }
+ result.x = orient.y * pose.a.z - pose.a.y * orient.z;
+ result.y = orient.z * pose.a.x - pose.a.z * orient.x;
+ result.z = orient.x * pose.a.y - pose.a.x * orient.y;
+ }
return Normalise (result);
}
@@ -202,7 +234,7 @@
// Next, using theta1_a, calculate thetas 2 and 3 (a and b)
// First up is calculating r and rz
double r = 0.0f, rz = 0.0f;
- if (sin (solutions[0][0]) < 0.1f || sin(solutions[0][0]) > -0.1f)
+ if (sin (solutions[0][0]) < 0.1f && sin(solutions[0][0]) > -0.1f)
{
r = ((p.x - (link5 * a.x)) / cos (solutions[0][0])) - link1;
}
@@ -258,7 +290,7 @@
// First up is calculating r and rz
r = 0.0f;
rz = 0.0f;
- if (sin (solutions[2][0]) < 0.1f || sin(solutions[2][0]) > -0.1f)
+ if (sin (solutions[2][0]) < 0.1f && sin(solutions[2][0]) > -0.1f)
{
r = (p.x - link5 * a.x) / cos (solutions[2][0]) - link1;
}
@@ -477,7 +509,7 @@
{
for (int ii = 0; ii < 5; ii++)
{
- if (angles[ii] < jointMin[ii] || angles[ii] > jointMax[ii])
+ if (angles[ii] < jointMin[ii] || angles[ii] > jointMax[ii] ||
isnan(angles[ii]))
return false;
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Check out the new SourceForge.net Marketplace.
It's the best place to buy or sell services for
just about anything Open Source.
http://sourceforge.net/services/buy/index.php
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
