From d80c8938c3fa970cec1b73bde56e80caf1716b8a Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 3 Jul 2020 15:00:36 +0000 Subject: [PATCH] Add missing script flip_box.py. --- talos/manipulate_boxes/flip_box.py | 41 ++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 talos/manipulate_boxes/flip_box.py diff --git a/talos/manipulate_boxes/flip_box.py b/talos/manipulate_boxes/flip_box.py new file mode 100644 index 0000000..c65ae1a --- /dev/null +++ b/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 ()