|
From: <hsu...@us...> - 2008-09-25 16:25:23
|
Revision: 4675
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4675&view=rev
Author: hsujohnhsu
Date: 2008-09-25 16:25:17 +0000 (Thu, 25 Sep 2008)
Log Message:
-----------
testslide now passes.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py 2008-09-25 05:54:46 UTC (rev 4674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py 2008-09-25 16:25:17 UTC (rev 4675)
@@ -49,9 +49,9 @@
from std_msgs.msg import *
from robot_msgs.msg import *
-TARGET_X = -5.4
-TARGET_Y = 0.0
-TARGET_Z = 2.6
+TARGET_X = -5.4 + 25.65 #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
+TARGET_Y = 0.0 + 25.65 #contains offset specified in P3D for base, alternatively, use the gripper roll ground truths
+TARGET_Z = 3.8
TARGET_RAD = 4.5
class TestSlide(unittest.TestCase):
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world 2008-09-25 05:54:46 UTC (rev 4674)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world 2008-09-25 16:25:17 UTC (rev 4675)
@@ -29,10 +29,6 @@
<erp>0.2</erp>
</physics:ode>
- <geo:origin>
- <lat>37.4270909558</lat><lon>-122.077919338</lon>
- </geo:origin>
-
<rendering:gui>
<type>fltk</type>
<size>1024 800</size>
@@ -219,5 +215,27 @@
</light>
</model:renderable>
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+ <model:physical name="robot_model1">
+
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <xyz>0.0 0.0 0.0 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2_xml.model" />
+ </include>
+
+ </model:physical>
+
+
</gazebo:world>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world 2008-09-25 05:54:46 UTC (rev 4674)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world 2008-09-25 16:25:17 UTC (rev 4675)
@@ -29,10 +29,6 @@
<erp>0.2</erp>
</physics:ode>
- <geo:origin>
- <lat>37.4270909558</lat><lon>-122.077919338</lon>
- </geo:origin>
-
<rendering:gui>
<type>fltk</type>
<size>1024 800</size>
@@ -199,12 +195,19 @@
</model:renderable>
<model:physical name="robot_model1">
- <xyz>0.0 0.0 0.02</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
- <rpy>0.0 0.0 0.0 </rpy>
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <xyz>0.0 0.0 0.0 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+
<!-- base, torso and arms -->
<include embedded="true">
- <xi:include href="pr2_xml_test.model" />
+ <xi:include href="pr2_xml.model" />
</include>
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world 2008-09-25 05:54:46 UTC (rev 4674)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world 2008-09-25 16:25:17 UTC (rev 4675)
@@ -112,7 +112,9 @@
<geom:trimesh name="endbin_geom">
<static>true</static>
<kp>10000000000.0</kp>
- <kd>9000</kd>
+ <kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<scale> 4 2 4</scale>
<mesh>cup.mesh</mesh>
@@ -142,16 +144,16 @@
<xyz> 0.0 0.0 0.00</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<body:box name="slide1_legs_body">
- <kp>10</kp>
- <kd>0.1</kd>
<geom:box name="slide_base_geom">
<kp>1000000000000000</kp>
<kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz> 0.0 5.0 13</xyz>
<rpy> 45.0 0.0 0.00</rpy>
<size> 2.0 14.14 1.0</size>
- <mass> 10.0</mass>
+ <mass> 1000.0</mass>
<visual>
<size>2.0 14.14 1.0</size>
<material>Gazebo/Rocky</material>
@@ -163,10 +165,12 @@
<geom:box name="slide_side1_geom">
<kp>100000</kp>
<kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz> 1.0 5.0 13.7</xyz>
<rpy> 135.0 0.0 0.00</rpy>
<size> 1.0 1.0 14.14</size>
- <mass> 10.0</mass>
+ <mass> 1000.0</mass>
<visual>
<size>0.1 1.0 14.14</size>
<material>Gazebo/Rocky</material>
@@ -176,10 +180,12 @@
<geom:box name="slide_side2_geom">
<kp>100000</kp>
<kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz> -1.0 7.0 15.5</xyz>
<rpy> 135.0 0.0 0.00</rpy>
<size> 1.0 1.0 10.0</size>
- <mass> 10.0</mass>
+ <mass> 1000.0</mass>
<visual>
<size>0.1 1.0 9.0</size>
<material>Gazebo/Rocky</material>
@@ -191,7 +197,9 @@
<geom:sphere name="slide_end_geom">
<kp>1000000.0</kp>
- <kd>0.1</kd>
+ <kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<mesh>default</mesh>
<xyz> 1.0 -1.0 9.5</xyz>
<size> 2.5</size>
@@ -216,7 +224,9 @@
<geom:box name="support_1">
<kp>10000000.0</kp>
- <kd>0.1</kd>
+ <kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz>1000.0 1000.0 1.00</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<mesh>default</mesh>
@@ -230,7 +240,9 @@
</geom:box>
<geom:box name="support_2">
<kp>10000000.0</kp>
- <kd>0.1</kd>
+ <kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz>1000.0 -1000.0 1.00</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<mesh>default</mesh>
@@ -244,7 +256,9 @@
</geom:box>
<geom:box name="support_3">
<kp>10000000.0</kp>
- <kd>0.1</kd>
+ <kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz>-1000.0 -1000.0 1.00</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<mesh>default</mesh>
@@ -258,7 +272,9 @@
</geom:box>
<geom:box name="support_4">
<kp>10000000.0</kp>
- <kd>0.1</kd>
+ <kd>1</kd>
+ <mu1>0.0</mu1>
+ <mu2>0.0</mu2>
<xyz>-1000.0 1000.0 1.00</xyz>
<rpy> 0.0 0.0 0.00</rpy>
<mesh>default</mesh>
@@ -282,6 +298,8 @@
<geom:sphere name="ball_geom">
<kp>100000.0</kp>
<kd>1.0</kd>
+ <mu1>5.0</mu1>
+ <mu2>5.0</mu2>
<mesh>default</mesh>
<size> 0.25</size>
<mass> 10.0</mass>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|