From 0432c12e60b206a1ecf797523137f84c01d8da9e Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 25 Sep 2020 11:50:51 +0200
Subject: [PATCH] Fix tutorial_3.py.
Redefine q_init and q_goal in such a way that box is on the table.
Fix warning in construction of constraint graph.
---
script/tutorial_3.py | 18 +++++++++---------
1 file changed, 9 insertions(+), 9 deletions(-)
diff --git a/script/tutorial_3.py b/script/tutorial_3.py
index 577459b..aaa6421 100644
--- a/script/tutorial_3.py
+++ b/script/tutorial_3.py
@@ -1,6 +1,7 @@
# Import libraries and load robots. {{{1
# Import. {{{2
+from math import sqrt
from hpp.gepetto import PathPlayer
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule, \
@@ -62,12 +63,15 @@ q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
rank = robot.rankInConfiguration ['box/root_joint']
-q_init [rank:rank+3] = [-2.5, -4, 0.8]
+q_init [rank:rank+3] = [-2.5, -4, 0.746]
+
+# Put box in right orientation
+q_init[rank+3:rank+7] = [0, -sqrt(2)/2, 0, sqrt(2)/2]
q_goal = q_init [::]
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['box/root_joint']
-q_goal [rank:rank+3] = [-3.5, -4, 0.8]
+q_goal [rank:rank+3] = [-2.5, -4.5, 0.746]
# 2}}}
# Create the constraints. {{{2
@@ -99,17 +103,13 @@ factory.setObjects (objects, handlesPerObject, contactSurfacesPerObject)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints = Constraints \
- (lockedJoints = locklhand))
+ (numConstraints = locklhand))
cg.initialize ()
# 2}}}
-res, q_init_proj, err = cg.applyNodeConstraints("free", q_init)
-res, q_goal_proj, err = cg.applyNodeConstraints("free", q_goal)
-
-ps.setInitialConfig (q_init_proj)
-
-ps.addGoalConfig (q_goal_proj)
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
# print ps.solve()
--
GitLab