Add missing script flip_box.py.

talos/manipulate_boxes/flip_box.py

@@ -0,0 +1,41 @@
+# Copyright 2018, 2019, 2020 CNRS - Airbus SAS
+# Author: Florent Lamiraux, Joseph Mirabel, Alexis Nicolin
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are
+# met:
+
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+
+# 2. 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.
+
+# 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
+# HOLDER 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.
+
+from hpp import Quaternion
+# Plan a path to flip the box starting at the end of pathId.
+def flipBox (ps, pathId = None):
+    if pathId is None: pathId = ps.numberPaths () - 1
+    q1 = ps.configAtParam (0, ps.pathLength (pathId))
+    q2 = q1 [::]
+    rank = ps.robot.rankInConfiguration["box/root_joint"]
+    q2 [rank + 3 : rank + 7] = (
+        Quaternion([0, 1, 0, 0]) * Quaternion(q1[rank + 3 : rank + 7])).\
+        toTuple()
+    ps.resetGoalConfigs ()
+    ps.setInitialConfig (q1)
+    ps.addGoalConfig (q2)
+    ps.setMaxIterPathPlanning (1000)
+    return ps.solve ()