From 2471ebd6e21ba7612f3f7329fdf498242fbc085f Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Mon, 31 Mar 2014 08:22:30 +0000
Subject: [PATCH] Tweaked GraspRRT demo to avoid collisions with other arm.
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@546 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../examples/GraspRRT/GraspRrtDemo.cpp | 4 +-
.../examples/GraspRRT/GraspRrtWindow.cpp | 4 +-
.../data/robots/ArmarIII/ArmarIII-LeftArm.xml | 6 +-
.../robots/ArmarIII/ArmarIII-RightArm.xml | 7 +-
.../scenes/examples/GraspRrt/planning.xml | 64 +++++++++++++++----
5 files changed, 62 insertions(+), 23 deletions(-)
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp
index 2b21723f4..56d419369 100644
--- a/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtDemo.cpp
@@ -36,13 +36,13 @@ int main (int argc, char** argv)
std::string goalObject("Can");
std::string rnsName("Planning Left");
std::string colModel1("ColModel Robot Moving Left");
- std::string colModel2("ColModel Robot Body");
+ std::string colModel2("ColModel Robot Body1");
std::string colModel3("ColModel Obstacles");
std::string eefName("Hand L");
std::string rnsNameB("Planning Right");
std::string colModel1B("ColModel Robot Moving Right");
- std::string colModel2B("ColModel Robot Body");
+ std::string colModel2B("ColModel Robot Body2");
std::string eefNameB("Hand R");
VirtualRobot::RuntimeEnvironment::considerKey("scene");
diff --git a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
index cded146bf..6ff75733f 100644
--- a/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
+++ b/MotionPlanning/examples/GraspRRT/GraspRrtWindow.cpp
@@ -610,7 +610,7 @@ void GraspRrtWindow::testInit()
if (colModelEnv)
cdm->addCollisionModel(colModelEnv);
- test_cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns));
+ test_cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns,500000));
}
float sampl = (float)UI.doubleSpinBoxCSpaceSampling->value();
float samplDCD = (float)UI.doubleSpinBoxColChecking->value();
@@ -685,7 +685,7 @@ void GraspRrtWindow::plan()
if (colModelEnv)
cdm->addCollisionModel(colModelEnv);
cdm->addCollisionModel(targetObject);
- cspace.reset(new Saba::CSpaceSampled(robot,cdm,rns));
+ cspace.reset(new Saba::CSpaceSampled(robot, cdm, rns, 500000));
float sampl = (float)UI.doubleSpinBoxCSpaceSampling->value();
float samplDCD = (float)UI.doubleSpinBoxColChecking->value();
cspace->setSamplingSize(sampl);
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
index 35263ecb0..9362a8d07 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-LeftArm.xml
@@ -16,7 +16,8 @@
<Joint type="revolute">
<!--DH a="0" d="0" theta="90" alpha="90" unitsangle="degree" unitslength="mm"/-->
<axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-106.91" hi="42.8"/>
+ <Limits unit="degree" lo="-106.91" hi="80"/>
+ <!--42.8-->
</Joint>
<Physics>
<CoM location="VisualizationBBoxCenter"/>
@@ -86,7 +87,8 @@
<Joint type="revolute">
<!--DH a="0" d="-7.5" theta="90" alpha="-90" unitsangle="degree" unitslength="mm"/-->
<axis x="0" y="0" z="1"/>
- <Limits unit="degree" lo="-122.96" hi="19.14"/>
+ <Limits unit="degree" lo="-122.96" hi="55"/>
+ <!--19.14-->
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
diff --git a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
index d76ef795d..94ffcf479 100644
--- a/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
+++ b/VirtualRobot/data/robots/ArmarIII/ArmarIII-RightArm.xml
@@ -18,8 +18,9 @@
</Transform>
<Joint type="revolute">
<!--DH a="0" d="0" theta="90" alpha="90" unitsangle="degree" unitslength="mm"/-->
- <Limits unit="degree" lo="-106.94" hi="41.67"/>
- </Joint>
+ <Limits unit="degree" lo="-106.94" hi="80"/>
+ <!--41.67-->
+ </Joint>
<Physics>
<CoM location="VisualizationBBoxCenter"/>
<Mass value="3.65" units="kg" />
@@ -85,7 +86,7 @@
</Transform>
<Joint type="revolute">
<!--DH a="0" d="7.5" theta="90" alpha="-90" unitsangle="degree" unitslength="mm"/-->
- <Limits unit="degree" lo="-114.58" hi="39.7"/> <!-- ?! -->
+ <Limits unit="degree" lo="-114.58" hi="55"/> <!-- 39.7 -->
</Joint>
<Visualization enable="true">
<CoordinateAxis type="Inventor" enable="false" scaling="8"/>
diff --git a/VirtualRobot/data/scenes/examples/GraspRrt/planning.xml b/VirtualRobot/data/scenes/examples/GraspRrt/planning.xml
index 9bd163e8f..0dc07014f 100644
--- a/VirtualRobot/data/scenes/examples/GraspRrt/planning.xml
+++ b/VirtualRobot/data/scenes/examples/GraspRrt/planning.xml
@@ -8,17 +8,19 @@
<Node name="Hip Pitch" unit="radian" value="0"/>
<Node name="Hip Roll" unit="radian" value="0"/>
<Node name="Hip Yaw" unit="radian" value="0"/>
- <Node name="Shoulder 1 L" unit="radian" value="0.26"/>
- <Node name="Shoulder 2 L" unit="radian" value="0"/>
- <Node name="Upperarm L" unit="radian" value="0.46"/>
- <Node name="Elbow L" unit="radian" value="0.2"/>
- <Node name="Underarm L" unit="radian" value="0"/>
+ <!--Node name="Shoulder 1 L" unit="radian" value="0.26"/-->
+ <Node name="Shoulder 1 L" unit="radian" value="0.85"/>
+ <Node name="Shoulder 2 L" unit="radian" value="0"/>
+ <Node name="Upperarm L" unit="radian" value="0.16"/>
+ <!--Node name="Elbow L" unit="radian" value="0.2"/-->
+ <Node name="Elbow L" unit="radian" value="0.55"/>
+ <Node name="Underarm L" unit="radian" value="0"/>
<Node name="Wrist 1 L" unit="radian" value="0"/>
<Node name="Wrist 2 L" unit="radian" value="0"/>
- <Node name="Shoulder 1 R" unit="radian" value="0.26"/>
+ <Node name="Shoulder 1 R" unit="radian" value="0.85"/>
<Node name="Shoulder 2 R" unit="radian" value="0"/>
- <Node name="Upperarm R" unit="radian" value="-0.46"/>
- <Node name="Elbow R" unit="radian" value="0.2"/>
+ <Node name="Upperarm R" unit="radian" value="-0.16"/>
+ <Node name="Elbow R" unit="radian" value="0.55"/>
<Node name="Underarm R" unit="radian" value="0"/>
<Node name="Wrist 1 R" unit="radian" value="0"/>
<Node name="Wrist 2 R" unit="radian" value="0"/>
@@ -87,13 +89,47 @@
</RobotNodeSet>
<!-- The second collision model (torso, head and platform)-->
- <RobotNodeSet name="ColModel Robot Body">
- <Node name="Platform"/>
- <Node name="Hip Yaw"/>
- <Node name="Head_Tilt"/>
- </RobotNodeSet>
+ <RobotNodeSet name="ColModel Robot Body1">
+ <Node name="Platform"/>
+ <Node name="Hip Yaw"/>
+ <Node name="Head_Tilt"/>
+ <Node name="Upperarm R"/>
+ <Node name="Underarm R"/>
+ <Node name="Hand Palm 1 R"/>
+ <Node name="Hand Palm 2 R"/>
+ <Node name="Thumb R J0"/>
+ <Node name="Thumb R J1"/>
+ <Node name="Index R J0"/>
+ <Node name="Index R J1"/>
+ <Node name="Middle R J0"/>
+ <Node name="Middle R J1"/>
+ <Node name="Ring R J0"/>
+ <Node name="Ring R J1"/>
+ <Node name="Pinky R J0"/>
+ <Node name="Pinky R J1"/>
+ </RobotNodeSet>
+
+ <RobotNodeSet name="ColModel Robot Body2">
+ <Node name="Platform"/>
+ <Node name="Hip Yaw"/>
+ <Node name="Head_Tilt"/>
+ <Node name="Upperarm L"/>
+ <Node name="Underarm L"/>
+ <Node name="Hand Palm 1 L"/>
+ <Node name="Hand Palm 2 L"/>
+ <Node name="Thumb L J0"/>
+ <Node name="Thumb L J1"/>
+ <Node name="Index L J0"/>
+ <Node name="Index L J1"/>
+ <Node name="Middle L J0"/>
+ <Node name="Middle L J1"/>
+ <Node name="Ring L J0"/>
+ <Node name="Ring L J1"/>
+ <Node name="Pinky L J0"/>
+ <Node name="Pinky L J1"/>
+ </RobotNodeSet>
- <GlobalPose>
+ <GlobalPose>
<Transform>
<Translation x="-6115.0" y="3580.0" z="0"/>
<rollpitchyaw units="degree" roll="0" pitch="0" yaw="0"/>
--
GitLab