Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7485] pkg/trunk
|
From: <hsu...@us...> - 2008-12-03 01:10:36
|
Revision: 7485
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7485&view=rev
Author: hsujohnhsu
Date: 2008-12-03 01:10:34 +0000 (Wed, 03 Dec 2008)
Log Message:
-----------
* remove deprecated math expression file.
* added pr2_empty_no_x.launch for tests. updated testbase and testslide to run without x.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomw_gt.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomxyw_gt.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml
Added Paths:
-----------
pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
Removed Paths:
-------------
pkg/trunk/util/angles/src/MathExpression.cpp
Added: pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2008-12-03 01:10:34 UTC (rev 7485)
@@ -0,0 +1,22 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
+ <group name="wg">
+ <!-- send pr2.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
+ <!-- start gazebo -->
+ <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+ </group>
+</launch>
+
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml 2008-12-03 01:09:41 UTC (rev 7484)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml 2008-12-03 01:10:34 UTC (rev 7485)
@@ -2,7 +2,7 @@
<master auto="start" />
<!-- send single_link.xml to param server -->
- <include file="$(find pr2_gazebo)/pr2_empty.launch" />
+ <include file="$(find pr2_gazebo)/pr2_empty_no_x.launch" />
<!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
<test test-name="gazebo_plugin_testbase_vw1" pkg="gazebo_plugin" type="testbase_vw_gt.py" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomw_gt.py 2008-12-03 01:09:41 UTC (rev 7484)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomw_gt.py 2008-12-03 01:10:34 UTC (rev 7485)
@@ -65,18 +65,39 @@
self.y = y
self.z = z
+ def shortest_euler_distance(self, e_from, e_to):
+ # takes two sets of euler angles, finds minimum transform between the two, FIXME: return some hacked norm for now
+ # start from the euler-from, and apply reverse euler-to transform, see how far we are from 0,0,0
+ r0 = e_from.x
+ p0 = e_from.y
+ y0 = e_from.z
+
+ r1 = math.cos(e_to.z)*r0 + math.sin(e_to.z)*p0
+ p1 = -math.sin(e_to.z)*r0 + math.cos(e_to.z)*p0
+ y1 = y0
+
+ r2 = math.cos(e_to.y)*r1 - math.sin(e_to.y)*y1
+ p2 = p1
+ y2 = math.sin(e_to.y)*r1 + math.cos(e_to.y)*y1
+
+ self.x = r2
+ self.y = math.cos(e_to.x)*p1 + math.sin(e_to.x)*y1
+ self.z = -math.sin(e_to.x)*p1 + math.cos(e_to.x)*y1
+
class Q:
def __init__(self,x,y,z,u):
self.x = x
self.y = y
self.z = z
self.u = u
+
def normalize(self):
s = math.sqrt(self.u * self.u + self.x * self.x + self.y * self.y + self.z * self.z)
self.u /= s
self.x /= s
self.y /= s
self.z /= s
+
def getEuler(self):
self.normalize()
squ = self.u
@@ -104,23 +125,37 @@
self.odom_xi = 0;
self.odom_yi = 0;
- self.odom_ti = 0;
+ self.odom_ei = E(0,0,0)
self.odom_initialized = False;
self.odom_x = 0;
self.odom_y = 0;
- self.odom_t = 0;
+ self.odom_e = E(0,0,0)
self.p3d_xi = 0;
self.p3d_yi = 0;
- self.p3d_ti = 0;
+ self.p3d_ei = E(0,0,0)
self.p3d_initialized = False;
self.p3d_x = 0;
self.p3d_y = 0;
- self.p3d_t = 0;
+ self.p3d_e = E(0,0,0)
+ def normalize_angle_positive(self, angle):
+ return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
+ def normalize_angle(self, angle):
+ anorm = self.normalize_angle_positive(angle)
+ if anorm > math.pi:
+ anorm -= 2*math.pi
+ return anorm
+
+ def shortest_angular_distance(self, angle_from, angle_to):
+ angle_diff = self.normalize_angle_positive(angle_to) - self.normalize_angle_positive(angle_from)
+ if angle_diff > math.pi:
+ angle_diff = -(2*math.pi - angle_diff)
+ return self.normalize_angle(angle_diff)
+
def printBaseOdom(self, odom):
print "odom received"
print "odom pos " + "x: " + str(odom.pos.x)
@@ -146,16 +181,18 @@
print " " + "y: " + str(p3d.vel.ang_vel.vy)
print " " + "z: " + str(p3d.vel.ang_vel.vz)
+
+
def odomInput(self, odom):
#self.printBaseOdom(odom)
if self.odom_initialized == False:
self.odom_initialized = True
self.odom_xi = odom.pos.x
self.odom_yi = odom.pos.y
- self.odom_ti = odom.pos.th
+ self.odom_ei = E(0,0,odom.pos.th)
self.odom_x = odom.pos.x - self.odom_xi
self.odom_y = odom.pos.y - self.odom_yi
- self.odom_t = odom.pos.th - self.odom_ti
+ self.odom_e.shortest_euler_distance(self.odom_ei, E(0,0,odom.pos.th))
def p3dInput(self, p3d):
@@ -167,11 +204,13 @@
self.p3d_initialized = True
self.p3d_xi = p3d.pos.position.x
self.p3d_yi = p3d.pos.position.y
- self.p3d_ti = v.z
+ self.p3d_ei = E(v.x,v.y,v.z)
self.p3d_x = p3d.pos.position.x - self.p3d_xi
self.p3d_y = p3d.pos.position.y - self.p3d_yi
- self.p3d_t = v.z - self.p3d_ti
+ self.p3d_e.shortest_euler_distance(self.p3d_ei,E(v.x,v.y,v.z))
+ #print "p3d initial:" + str(self.p3d_ei.x) + "," + str(self.p3d_ei.y) + "," + str(self.p3d_ei.z) \
+ # + " p3d final:" + str(v.x) + "," + str(v.y) + "," + str(v.z)
def test_base(self):
@@ -185,14 +224,20 @@
pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
time.sleep(0.1)
# display what odom thinks
- #print " odom " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
+ #print " odom " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
# display what ground truth is
- #print " p3d " + " x: " + str(self.p3d_x) + " y: " + str(self.p3d_y) + " t: " + str(self.p3d_t)
+ #print " p3d " + " x: " + str(self.p3d_e.x) + " y: " + str(self.p3d_e.y) + " t: " + str(self.p3d_e.t)
# display what the odom error is
- print " error " + " x: " + str(self.odom_x - self.p3d_x) + " y: " + str(self.odom_y - self.p3d_y) + " t: " + str(self.odom_t - self.p3d_t)
+ error = E(0,0,0)
+ error.shortest_euler_distance(self.p3d_e,self.odom_e)
+ print " error " + " x:" + str(self.odom_x - self.p3d_x) \
+ + " y:" + str(self.odom_y - self.p3d_y) \
+ + " e:" + str(error.x) + "," + str(error.y) + "," + str(error.z) \
+ + " t_odom:" + str(self.odom_e.z) + " t_p3d:" + str(self.p3d_e.z)
# check total error
- total_error = abs(self.odom_x - self.p3d_x) + abs(self.odom_y - self.p3d_y) + abs(self.odom_t - self.p3d_t)
+ total_error = abs(self.odom_x - self.p3d_x) + abs(self.odom_y - self.p3d_y) + abs(error.x) + abs(error.y) + abs(error.z)
+ print "total error:" + str(total_error) + " tol:" + str(TARGET_TOL)
if total_error < TARGET_TOL:
self.success = True
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomxyw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomxyw_gt.py 2008-12-03 01:09:41 UTC (rev 7484)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase_odomxyw_gt.py 2008-12-03 01:10:34 UTC (rev 7485)
@@ -35,7 +35,7 @@
## Gazebo test base controller vw
PKG = 'gazebo_plugin'
-NAME = 'testbase_odomxyw_gt'
+NAME = 'testbase_odomw_gt'
import math
import rostools
@@ -57,7 +57,7 @@
TARGET_VY = 0.5
TARGET_VW = 0.5
TARGET_DURATION = 2.0
-TARGET_TOL = 1.0 #empirical test result john - 20081124
+TARGET_TOL = 0.2 #empirical test result john - 20081124
class E:
def __init__(self,x,y,z):
@@ -65,18 +65,39 @@
self.y = y
self.z = z
+ def shortest_euler_distance(self, e_from, e_to):
+ # takes two sets of euler angles, finds minimum transform between the two, FIXME: return some hacked norm for now
+ # start from the euler-from, and apply reverse euler-to transform, see how far we are from 0,0,0
+ r0 = e_from.x
+ p0 = e_from.y
+ y0 = e_from.z
+
+ r1 = math.cos(e_to.z)*r0 + math.sin(e_to.z)*p0
+ p1 = -math.sin(e_to.z)*r0 + math.cos(e_to.z)*p0
+ y1 = y0
+
+ r2 = math.cos(e_to.y)*r1 - math.sin(e_to.y)*y1
+ p2 = p1
+ y2 = math.sin(e_to.y)*r1 + math.cos(e_to.y)*y1
+
+ self.x = r2
+ self.y = math.cos(e_to.x)*p1 + math.sin(e_to.x)*y1
+ self.z = -math.sin(e_to.x)*p1 + math.cos(e_to.x)*y1
+
class Q:
def __init__(self,x,y,z,u):
self.x = x
self.y = y
self.z = z
self.u = u
+
def normalize(self):
s = math.sqrt(self.u * self.u + self.x * self.x + self.y * self.y + self.z * self.z)
self.u /= s
self.x /= s
self.y /= s
self.z /= s
+
def getEuler(self):
self.normalize()
squ = self.u
@@ -104,23 +125,37 @@
self.odom_xi = 0;
self.odom_yi = 0;
- self.odom_ti = 0;
+ self.odom_ei = E(0,0,0)
self.odom_initialized = False;
self.odom_x = 0;
self.odom_y = 0;
- self.odom_t = 0;
+ self.odom_e = E(0,0,0)
self.p3d_xi = 0;
self.p3d_yi = 0;
- self.p3d_ti = 0;
+ self.p3d_ei = E(0,0,0)
self.p3d_initialized = False;
self.p3d_x = 0;
self.p3d_y = 0;
- self.p3d_t = 0;
+ self.p3d_e = E(0,0,0)
+ def normalize_angle_positive(self, angle):
+ return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi)
+ def normalize_angle(self, angle):
+ anorm = self.normalize_angle_positive(angle)
+ if anorm > math.pi:
+ anorm -= 2*math.pi
+ return anorm
+
+ def shortest_angular_distance(self, angle_from, angle_to):
+ angle_diff = self.normalize_angle_positive(angle_to) - self.normalize_angle_positive(angle_from)
+ if angle_diff > math.pi:
+ angle_diff = -(2*math.pi - angle_diff)
+ return self.normalize_angle(angle_diff)
+
def printBaseOdom(self, odom):
print "odom received"
print "odom pos " + "x: " + str(odom.pos.x)
@@ -146,16 +181,18 @@
print " " + "y: " + str(p3d.vel.ang_vel.vy)
print " " + "z: " + str(p3d.vel.ang_vel.vz)
+
+
def odomInput(self, odom):
#self.printBaseOdom(odom)
if self.odom_initialized == False:
self.odom_initialized = True
self.odom_xi = odom.pos.x
self.odom_yi = odom.pos.y
- self.odom_ti = odom.pos.th
+ self.odom_ei = E(0,0,odom.pos.th)
self.odom_x = odom.pos.x - self.odom_xi
self.odom_y = odom.pos.y - self.odom_yi
- self.odom_t = odom.pos.th - self.odom_ti
+ self.odom_e.shortest_euler_distance(self.odom_ei, E(0,0,odom.pos.th))
def p3dInput(self, p3d):
@@ -167,11 +204,13 @@
self.p3d_initialized = True
self.p3d_xi = p3d.pos.position.x
self.p3d_yi = p3d.pos.position.y
- self.p3d_ti = v.z
+ self.p3d_ei = E(v.x,v.y,v.z)
self.p3d_x = p3d.pos.position.x - self.p3d_xi
self.p3d_y = p3d.pos.position.y - self.p3d_yi
- self.p3d_t = v.z - self.p3d_ti
+ self.p3d_e.shortest_euler_distance(self.p3d_ei,E(v.x,v.y,v.z))
+ #print "p3d initial:" + str(self.p3d_ei.x) + "," + str(self.p3d_ei.y) + "," + str(self.p3d_ei.z) \
+ # + " p3d final:" + str(v.x) + "," + str(v.y) + "," + str(v.z)
def test_base(self):
@@ -185,14 +224,20 @@
pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
time.sleep(0.1)
# display what odom thinks
- #print " odom " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
+ #print " odom " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
# display what ground truth is
- #print " p3d " + " x: " + str(self.p3d_x) + " y: " + str(self.p3d_y) + " t: " + str(self.p3d_t)
+ #print " p3d " + " x: " + str(self.p3d_e.x) + " y: " + str(self.p3d_e.y) + " t: " + str(self.p3d_e.t)
# display what the odom error is
- print " error " + " x: " + str(self.odom_x - self.p3d_x) + " y: " + str(self.odom_y - self.p3d_y) + " t: " + str(self.odom_t - self.p3d_t)
+ error = E(0,0,0)
+ error.shortest_euler_distance(self.p3d_e,self.odom_e)
+ print " error " + " x:" + str(self.odom_x - self.p3d_x) \
+ + " y:" + str(self.odom_y - self.p3d_y) \
+ + " e:" + str(error.x) + "," + str(error.y) + "," + str(error.z) \
+ + " t_odom:" + str(self.odom_e.z) + " t_p3d:" + str(self.p3d_e.z)
# check total error
- total_error = abs(self.odom_x - self.p3d_x) + abs(self.odom_y - self.p3d_y) + abs(self.odom_t - self.p3d_t)
+ total_error = abs(self.odom_x - self.p3d_x) + abs(self.odom_y - self.p3d_y) + abs(error.x) + abs(error.y) + abs(error.z)
+ print "total error:" + str(total_error) + " tol:" + str(TARGET_TOL)
if total_error < TARGET_TOL:
self.success = True
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml 2008-12-03 01:09:41 UTC (rev 7484)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml 2008-12-03 01:10:34 UTC (rev 7485)
@@ -1,7 +1,7 @@
<launch>
<master auto="start" />
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/test/testslide.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/test/testslide.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
Deleted: pkg/trunk/util/angles/src/MathExpression.cpp
===================================================================
--- pkg/trunk/util/angles/src/MathExpression.cpp 2008-12-03 01:09:41 UTC (rev 7484)
+++ pkg/trunk/util/angles/src/MathExpression.cpp 2008-12-03 01:10:34 UTC (rev 7485)
@@ -1,167 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/** \author Ioan Sucan */
-
-#include "math_utils/MathExpression.h"
-#include <cstdlib>
-#include <cstring>
-#include <cmath>
-#include <deque>
-#include <cstdlib>
-
-bool meval::ContainsOperators(const char *expression)
-{
- return ContainsOperators(std::string(expression));
-}
-
-bool meval::ContainsOperators(const std::string &expression)
-{
- return expression.find_first_of("+-*/") != std::string::npos;
-}
-
-double meval::EvaluateMathExpression(const char *expression, ExpressionVariableFn var, void *data)
-{
- return EvaluateMathExpression(std::string(expression), var, data);
-}
-
-double meval::EvaluateMathExpression(const std::string &expression, ExpressionVariableFn var, void *data)
-{
- std::string exp = expression;
- while (!exp.empty())
- {
- std::string::size_type pos = exp.find_first_of("\n\t ");
- if (pos != std::string::npos)
- exp.erase(pos, 1);
- else
- break;
- }
-
- /* remove brackets, if needed */
- while (exp.size() > 0 && exp[0] == '(' && exp[exp.size() - 1] == ')')
- {
- int depth = 1;
- bool done = false;
- for (unsigned int i = 1 ; i < exp.size() - 1; ++i)
- {
- if (exp[i] == '(')
- depth++;
- if (exp[i] == ')')
- depth--;
- if (depth == 0)
- {
- done = true;
- break;
- }
- }
- if (done)
- break;
- else
- {
- exp.erase(exp.size() - 1);
- exp.erase(0, 1);
- }
- }
-
- /* find possible operations */
- int depth = 0;
- std::deque<unsigned int> ops;
- for (unsigned int i = 0 ; i < exp.size() ; ++i)
- {
- if (depth == 0 && (exp[i] == '+' || exp[i] == '-'))
- ops.push_front(i);
- if (depth == 0 && (exp[i] == '*' || exp[i] == '/'))
- ops.push_back(i);
- if (exp[i] == '(')
- depth++;
- if (exp[i] == ')')
- depth--;
- }
-
- if (ops.empty())
- {
- if (!exp.empty())
- {
- bool variable = false;
- for (unsigned int i = 0 ; i < exp.size() ; ++i)
- {
- if (!((exp[i] <= '9' && exp[i] >= '0') || exp[i] == '+' || exp[i] == '-' || exp[i] == '.'))
- {
- variable = true;
- break;
- }
- }
-
- if (variable)
- {
- if (var)
- return var(data, exp);
- return NAN;
- }
- else
- {
- return atof(exp.c_str());
- }
- }
- }
- else
- {
- unsigned int pos = ops[0];
- std::string exp1 = exp.substr(0, pos);
- std::string exp2 = exp.substr(pos + 1);
- double val1, val2;
- val1 = EvaluateMathExpression(exp1, var, data);
- val2 = EvaluateMathExpression(exp2, var, data);
-
- // Hack: handles unary minus
- if (exp1.size() == 0)
- val1 = 0.0;
-
- switch (exp[pos])
- {
- case '+':
- return val1 + val2;
- case '-':
- return val1 - val2;
- case '*':
- return val1 * val2;
- case '/':
- return val1 / val2;
- default:
- return NAN;
- }
- }
-
- return NAN;
-}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
