Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-dynamic-pinocchio
Commits
cff464ee
Commit
cff464ee
authored
Sep 08, 2016
by
Rohan Budhiraja
Browse files
[c++] Deal with Spherical Joints in JointLimits.
update signal lowerJl and upperJl
parent
daade23f
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/sot-dynamic/dynamic.h
View file @
cff464ee
...
...
@@ -249,6 +249,8 @@ class SOTDYNAMIC_EXPORT Dynamic
dg
::
Vector
getPinocchioPos
(
int
);
dg
::
Vector
getPinocchioVel
(
int
);
dg
::
Vector
getPinocchioAcc
(
int
);
//\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
std
::
vector
<
int
>
sphericalJoints
;
};
...
...
src/sot-dynamic.cpp
View file @
cff464ee
...
...
@@ -247,13 +247,38 @@ Dynamic::setData(se3::Data* dataPtr){
dg
::
Vector
&
Dynamic
::
getLowerPositionLimits
(
dg
::
Vector
&
res
,
const
int
&
)
{
sotDEBUGIN
(
15
);
assert
(
m_model
);
res
.
resize
(
m_model
->
nq
);
res
=
m_model
->
lowerPositionLimit
;
res
.
resize
(
m_model
->
nv
);
if
(
!
sphericalJoints
.
empty
())
{
int
fillingIndex
=
0
;
//SoTValue
int
origIndex
=
0
;
//PinocchioValue
for
(
std
::
vector
<
int
>::
const_iterator
it
=
sphericalJoints
.
begin
();
it
<
sphericalJoints
.
end
();
it
++
){
if
(
*
it
-
fillingIndex
>
0
){
res
.
segment
(
fillingIndex
,
*
it
-
fillingIndex
)
=
m_model
->
lowerPositionLimit
.
segment
(
origIndex
,
*
it
-
fillingIndex
);
//Don't Change this order
origIndex
+=
*
it
-
fillingIndex
;
fillingIndex
+=
*
it
-
fillingIndex
;
}
//Found a Spherical Joint.
//Assuming that spherical joint limits are unset
res
(
fillingIndex
)
=
std
::
numeric_limits
<
double
>::
min
();
res
(
fillingIndex
+
1
)
=
std
::
numeric_limits
<
double
>::
min
();
res
(
fillingIndex
+
2
)
=
std
::
numeric_limits
<
double
>::
min
();
fillingIndex
+=
3
;
origIndex
+=
4
;
}
assert
(
m_model
->
nv
-
fillingIndex
==
m_model
->
nq
-
origIndex
);
if
(
m_model
->
nv
>
fillingIndex
)
res
.
segment
(
fillingIndex
,
m_model
->
nv
-
fillingIndex
)
=
m_model
->
lowerPositionLimit
.
segment
(
origIndex
,
m_model
->
nv
-
fillingIndex
);
}
else
{
res
=
m_model
->
lowerPositionLimit
;
}
sotDEBUG
(
15
)
<<
"lowerLimit ("
<<
res
<<
")="
<<
std
::
endl
;
sotDEBUGOUT
(
15
);
return
res
;
...
...
@@ -265,10 +290,36 @@ getUpperPositionLimits(dg::Vector& res, const int&)
sotDEBUGIN
(
15
);
assert
(
m_model
);
res
.
resize
(
m_model
->
nq
);
res
=
m_model
->
upperPositionLimit
;
sotDEBUG
(
15
)
<<
"upperLimit ("
<<
res
<<
")="
<<
std
::
endl
;
res
.
resize
(
m_model
->
nv
);
if
(
!
sphericalJoints
.
empty
())
{
int
fillingIndex
=
0
;
//SoTValue
int
origIndex
=
0
;
//PinocchioValue
for
(
std
::
vector
<
int
>::
const_iterator
it
=
sphericalJoints
.
begin
();
it
<
sphericalJoints
.
end
();
it
++
){
if
(
*
it
-
fillingIndex
>
0
){
res
.
segment
(
fillingIndex
,
*
it
-
fillingIndex
)
=
m_model
->
upperPositionLimit
.
segment
(
origIndex
,
*
it
-
fillingIndex
);
//Don't Change this order
origIndex
+=
*
it
-
fillingIndex
;
fillingIndex
+=
*
it
-
fillingIndex
;
}
//Found a Spherical Joint.
//Assuming that spherical joint limits are unset
res
(
fillingIndex
)
=
std
::
numeric_limits
<
double
>::
max
();
res
(
fillingIndex
+
1
)
=
std
::
numeric_limits
<
double
>::
max
();
res
(
fillingIndex
+
2
)
=
std
::
numeric_limits
<
double
>::
max
();
fillingIndex
+=
3
;
origIndex
+=
4
;
}
assert
(
m_model
->
nv
-
fillingIndex
==
m_model
->
nq
-
origIndex
);
if
(
m_model
->
nv
>
fillingIndex
)
res
.
segment
(
fillingIndex
,
m_model
->
nv
-
fillingIndex
)
=
m_model
->
upperPositionLimit
.
segment
(
origIndex
,
m_model
->
nv
-
fillingIndex
);
}
else
{
res
=
m_model
->
upperPositionLimit
;
}
sotDEBUG
(
15
)
<<
"upperLimit ("
<<
res
<<
")="
<<
std
::
endl
;
sotDEBUGOUT
(
15
);
return
res
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment