Skip to content

Commit b9ee82d

Browse files
nim65snim65s
committed
WIP sot-core
1 parent 93b2125 commit b9ee82d

File tree

6 files changed

+382
-2
lines changed

6 files changed

+382
-2
lines changed

‎py-sot-core-v3/Makefile‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
ORG= stack-of-tasks
66
NAME= sot-core
77
VERSION= 4.11.1
8-
PKGREVISION= 1
8+
PKGREVISION= 2
99

1010
PKGNAME= ${PKGTAG.python-}${NAME}-v3-${VERSION}
1111

‎py-sot-core-v3/distinfo‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
SHA1 (sot-core-4.11.1.tar.gz) = 6a339dbecbc5f8b6bcfdfda76c71f2e358b869ca
22
RMD160 (sot-core-4.11.1.tar.gz) = 790b91727c7bcd52efe4f463c107d8ccb7bd99e9
33
Size (sot-core-4.11.1.tar.gz) = 1125565 bytes
4+
SHA1 (patch-167) = c76162af4852012a5609778e1bdfdb7cb68734e5

‎py-sot-core-v3/patches/patch-167‎

Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
From 438a79cb6fed77983b98ebe9175d364764ea803c Mon Sep 17 00:00:00 2001
2+
From: Joseph Mirabel <jmirabel@laas.fr>
3+
Date: Tue, 29 Sep 2020 19:18:07 +0200
4+
Subject: [PATCH 1/2] [Python] Add constructor of Flags from list or tuple.
5+
6+
---
7+
include/sot/core/flags.hh | 2 +-
8+
src/python-module.cc | 11 ++++++++++-
9+
src/sot/flags.cpp | 2 +-
10+
3 files changed, 12 insertions(+), 3 deletions(-)
11+
12+
diff --git a/include/sot/core/flags.hh b/include/sot/core/flags.hh
13+
index c4d79627..e233969d 100644
14+
--- include/sot/core/flags.hh
15+
+++ include/sot/core/flags.hh
16+
@@ -37,7 +37,7 @@ protected:
17+
public:
18+
Flags(const bool &b = false);
19+
Flags(const char *flags);
20+
- Flags(std::vector<bool> &&flags);
21+
+ Flags(const std::vector<bool> &flags);
22+
23+
void add(const bool &b);
24+
25+
diff --git a/src/python-module.cc b/src/python-module.cc
26+
index da50ed29..baabaaa4 100644
27+
--- src/python-module.cc
28+
+++ src/python-module.cc
29+
@@ -50,7 +50,16 @@ BOOST_PYTHON_MODULE(wrap)
30+
using dgs::Flags;
31+
bp::class_<Flags>("Flags", bp::init<>())
32+
.def(bp::init<const char*>())
33+
- //TODO .def(bp::init<std::vector<bool>&& >())
34+
+ .def("__init__", bp::make_constructor(+[](bp::list bools) {
35+
+ std::vector<bool> flags (bp::len(bools));
36+
+ for (std::size_t i = 0; i < flags.size(); ++i) flags[i] = bp::extract<bool>(bools[i]);
37+
+ return new Flags(flags);
38+
+ }))
39+
+ .def("__init__", bp::make_constructor(+[](bp::tuple bools) {
40+
+ std::vector<bool> flags (bp::len(bools));
41+
+ for (std::size_t i = 0; i < flags.size(); ++i) flags[i] = bp::extract<bool>(bools[i]);
42+
+ return new Flags(flags);
43+
+ }))
44+
.def("add", &Flags::add)
45+
.def("set", &Flags::set)
46+
.def("unset", &Flags::unset)
47+
diff --git a/src/sot/flags.cpp b/src/sot/flags.cpp
48+
index c32bba73..29a5642d 100644
49+
--- src/sot/flags.cpp
50+
+++ src/sot/flags.cpp
51+
@@ -47,7 +47,7 @@ Flags::Flags(const char *_flags)
52+
}
53+
}
54+
55+
-Flags::Flags(std::vector<bool> &&_flags)
56+
+Flags::Flags(const std::vector<bool> &_flags)
57+
: flags(_flags), outOfRangeFlag(false) {}
58+
59+
Flags::operator bool(void) const {
60+
61+
From 9e900f79a2e5540c22310e1645d158586b321979 Mon Sep 17 00:00:00 2001
62+
From: Joseph Mirabel <jmirabel@laas.fr>
63+
Date: Tue, 29 Sep 2020 19:27:04 +0200
64+
Subject: [PATCH 2/2] [Python] Make toFlags deprecated.
65+
66+
---
67+
src/dynamic_graph/sot/core/meta_task_6d.py | 17 +++--------------
68+
src/dynamic_graph/sot/core/meta_task_posture.py | 8 +++++---
69+
src/dynamic_graph/sot/core/meta_tasks.py | 9 ++++-----
70+
.../sot/core/meta_tasks_kine_relative.py | 9 ++++-----
71+
4 files changed, 16 insertions(+), 27 deletions(-)
72+
73+
diff --git a/src/dynamic_graph/sot/core/meta_task_6d.py b/src/dynamic_graph/sot/core/meta_task_6d.py
74+
index e8e3de53..7dc23f0f 100644
75+
--- src/dynamic_graph/sot/core/meta_task_6d.py
76+
+++ src/dynamic_graph/sot/core/meta_task_6d.py
77+
@@ -7,20 +7,9 @@
78+
79+
80+
def toFlags(arr):
81+
- """
82+
- Convert an array of boolean to a /flag/ format, type 001010110,
83+
- in little indian
84+
- (reverse order, first bool of the list will be the [01] of extrem right).
85+
- """
86+
- lres = [0] * (max(arr) + 1)
87+
- for i in arr:
88+
- lres[i] = 1
89+
- lres.reverse()
90+
- res = ''
91+
- for i in lres:
92+
- res += str(i)
93+
- return Flags(res)
94+
-
95+
+ from warnings import warn
96+
+ warn("This function is deprecated. Please, use Flags directly.")
97+
+ return Flags(arr)
98+
99+
class MetaTask6d(object):
100+
name = ''
101+
diff --git a/src/dynamic_graph/sot/core/meta_task_posture.py b/src/dynamic_graph/sot/core/meta_task_posture.py
102+
index 8e21c11a..4f515809 100644
103+
--- src/dynamic_graph/sot/core/meta_task_posture.py
104+
+++ src/dynamic_graph/sot/core/meta_task_posture.py
105+
@@ -2,7 +2,7 @@
106+
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
107+
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
108+
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
109+
-from dynamic_graph.sot.core.meta_task_6d import toFlags
110+
+from dynamic_graph.sot.core.meta_task_6d import toFlags # kept for backward compatibility
111+
from dynamic_graph.sot.core.meta_tasks import setGain
112+
from dynamic_graph.sot.core.sot import Task
113+
from numpy import identity, matrix, zeros
114+
@@ -67,9 +67,11 @@ def gotoq(self, gain=None, qdes=None, **kwargs):
115+
MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
116+
qdes = zeros((MetaTaskPosture.nbDof, 1))
117+
118+
+ act = [ False, ] * MetaTaskPosture.nbDof
119+
for limbName, jointValues in kwargs.items():
120+
limbRange = self.postureRange[limbName]
121+
- act += limbRange
122+
+ for i in limbRange:
123+
+ act[i] = True
124+
if jointValues != []:
125+
if isinstance(jointValues, matrix):
126+
qdes[limbRange, 0] = vectorToTuple(jointValues)
127+
@@ -77,7 +79,7 @@ def gotoq(self, gain=None, qdes=None, **kwargs):
128+
qdes[limbRange, 0] = jointValues
129+
self.ref = vectorToTuple(qdes)
130+
if len(act) > 0:
131+
- self.feature.selec.value = toFlags(act)
132+
+ self.feature.selec.value = Flags(act)
133+
setGain(self.gain, gain)
134+
135+
136+
diff --git a/src/dynamic_graph/sot/core/meta_tasks.py b/src/dynamic_graph/sot/core/meta_tasks.py
137+
index 95123dab..b5912699 100644
138+
--- src/dynamic_graph/sot/core/meta_tasks.py
139+
+++ src/dynamic_graph/sot/core/meta_tasks.py
140+
@@ -5,7 +5,7 @@
141+
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
142+
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
143+
from dynamic_graph.sot.core.matrix_util import matrixToTuple, rpy2tr
144+
-from dynamic_graph.sot.core.meta_task_6d import toFlags
145+
+from dynamic_graph.sot.core.meta_task_6d import toFlags # kept for backward compatibility
146+
147+
148+
class MetaTaskCom(object):
149+
@@ -79,10 +79,9 @@ def goto6d(task, position, gain=None, resetJacobian=True):
150+
def gotoNd(task, position, selec=None, gain=None, resetJacobian=True):
151+
M = generic6dReference(position)
152+
if selec is not None:
153+
- if isinstance(selec, str):
154+
- task.feature.selec.value = Flags(selec)
155+
- else:
156+
- task.feature.selec.value = toFlags(selec)
157+
+ if not isinstance(selec, Flags):
158+
+ selec = Flags(selec)
159+
+ task.feature.selec.value = selec
160+
task.featureDes.position.value = array(M)
161+
setGain(task.gain, gain)
162+
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
163+
diff --git a/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py b/src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
164+
index e1e538c5..713e1459 100644
165+
--- src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
166+
+++ src/dynamic_graph/sot/core/meta_tasks_kine_relative.py
167+
@@ -2,7 +2,7 @@
168+
from dynamic_graph.sot.core import Flags
169+
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
170+
from dynamic_graph.sot.core.matrix_util import matrixToTuple
171+
-from dynamic_graph.sot.core.meta_task_6d import MetaTask6d, toFlags
172+
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d, toFlags # kept for backward compatibility
173+
from dynamic_graph.sot.core.meta_tasks import generic6dReference, setGain
174+
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
175+
176+
@@ -94,10 +94,9 @@ def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=
177+
M = generic6dReference(position)
178+
MRef = generic6dReference(positionRef)
179+
if selec is not None:
180+
- if isinstance(selec, str):
181+
- task.feature.selec.value = selec
182+
- else:
183+
- task.feature.selec.value = toFlags(selec)
184+
+ if not isinstance(selec, Flags):
185+
+ selec = Flags(selec)
186+
+ task.feature.selec.value = selec
187+
task.featureDes.position.value = matrixToTuple(M)
188+
task.featureDes.positionRef.value = matrixToTuple(MRef)
189+
setGain(task.gain, gain)

‎sot-core-v3/Makefile‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
ORG= stack-of-tasks
66
NAME= sot-core
77
VERSION= 4.11.1
8-
PKGREVISION= 1
8+
PKGREVISION= 2
99

1010
PKGNAME= ${NAME}-v3-${VERSION}
1111

‎sot-core-v3/distinfo‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
SHA1 (sot-core-4.11.1.tar.gz) = 6a339dbecbc5f8b6bcfdfda76c71f2e358b869ca
22
RMD160 (sot-core-4.11.1.tar.gz) = 790b91727c7bcd52efe4f463c107d8ccb7bd99e9
33
Size (sot-core-4.11.1.tar.gz) = 1125565 bytes
4+
SHA1 (patch-167) = c76162af4852012a5609778e1bdfdb7cb68734e5

0 commit comments

Comments
 (0)