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
e82f73f6
Commit
e82f73f6
authored
Aug 17, 2016
by
Rohan Budhiraja
Browse files
[c++] Dealing with Spherical Joints
parent
9fe5912d
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/sot-dynamic/dynamic.h
View file @
e82f73f6
...
...
@@ -138,7 +138,7 @@ class SOTDYNAMIC_EXPORT Dynamic
dg
::
SignalPtr
<
dg
::
Vector
,
int
>
freeFlyerAccelerationSIN
;
dg
::
SignalTimeDependent
<
Dummy
,
int
>
newtonEulerSINTERN
;
int
&
computeNewtonEuler
(
int
&
dummy
,
int
time
);
dg
::
SignalTimeDependent
<
dg
::
Vector
,
int
>
zmpSOUT
;
...
...
@@ -177,6 +177,9 @@ class SOTDYNAMIC_EXPORT Dynamic
void
displayModel
()
const
{
assert
(
m_model
);
std
::
cout
<<
(
*
m_model
)
<<
std
::
endl
;
};
void
setModel
(
se3
::
Model
*
);
void
setData
(
se3
::
Data
*
);
/* --- GETTERS --- */
...
...
@@ -246,6 +249,7 @@ class SOTDYNAMIC_EXPORT Dynamic
dg
::
Vector
getPinocchioPos
(
int
);
dg
::
Vector
getPinocchioVel
(
int
);
dg
::
Vector
getPinocchioAcc
(
int
);
std
::
vector
<
int
>
sphericalJoints
;
};
...
...
src/python-module-py.cpp
View file @
e82f73f6
...
...
@@ -89,7 +89,8 @@ namespace dynamicgraph{
try
{
se3
::
python
::
ModelHandler
cppModelHandle
=
boost
::
python
::
extract
<
se3
::
python
::
ModelHandler
>
(
pyPinocchioObject
);
dyn_entity
->
m_model
=
cppModelHandle
.
ptr
();
dyn_entity
->
setModel
(
cppModelHandle
.
ptr
());
//dyn_entity->m_model = cppModelHandle.ptr();
}
catch
(
const
std
::
exception
&
exc
)
{
//PyErr_SetString(dgpyError, exc.what());
...
...
@@ -128,7 +129,8 @@ namespace dynamicgraph{
try
{
se3
::
python
::
DataHandler
cppDataHandle
=
boost
::
python
::
extract
<
se3
::
python
::
DataHandler
>
(
pyPinocchioObject
);
dyn_entity
->
m_data
=
cppDataHandle
.
ptr
();
dyn_entity
->
setData
(
cppDataHandle
.
ptr
());
//dyn_entity->m_data = cppDataHandle.ptr();
}
catch
(
const
std
::
exception
&
exc
)
{
// PyErr_SetString(dgpyError, exc.what());
...
...
src/sot-dynamic.cpp
View file @
e82f73f6
...
...
@@ -196,6 +196,9 @@ Dynamic( const std::string & name)
addCommand
(
"createPosition"
,
makeCommandVoid2
(
*
this
,
&
Dynamic
::
cmd_createPositionSignal
,
docstring
));
}
sphericalJoints
.
clear
();
sotDEBUG
(
10
)
<<
"Dynamic class_name address"
<<&
CLASS_NAME
<<
std
::
endl
;
sotDEBUGOUT
(
5
);
...
...
@@ -218,6 +221,27 @@ Dynamic::~Dynamic( void ) {
sotDEBUGOUT
(
15
);
}
void
Dynamic
::
setModel
(
se3
::
Model
*
modelPtr
){
this
->
m_model
=
modelPtr
;
if
(
this
->
m_model
->
nq
>
m_model
->
nv
)
{
if
(
se3
::
nv
(
this
->
m_model
->
joints
[
1
])
==
6
)
sphericalJoints
.
push_back
(
3
);
//FreeFlyer Orientation
for
(
int
i
=
1
;
i
<
this
->
m_model
->
njoint
;
i
++
)
//0: universe
if
(
se3
::
nq
(
this
->
m_model
->
joints
[
i
])
==
4
)
//Spherical Joint Only
sphericalJoints
.
push_back
(
se3
::
idx_v
(
this
->
m_model
->
joints
[
i
]));
}
}
void
Dynamic
::
setData
(
se3
::
Data
*
dataPtr
){
this
->
m_data
=
dataPtr
;
}
/*--------------------------------GETTERS-------------------------------------------*/
dg
::
Vector
&
Dynamic
::
...
...
@@ -283,32 +307,35 @@ dg::Vector Dynamic::getPinocchioPos(int time)
sotDEBUGIN
(
15
);
dg
::
Vector
qJoints
=
jointPositionSIN
.
access
(
time
);
dg
::
Vector
q
;
assert
(
m_model
);
if
(
freeFlyerPositionSIN
)
{
dg
::
Vector
qFF
=
freeFlyerPositionSIN
.
access
(
time
);
q
.
resize
(
qJoints
.
size
()
+
7
);
Eigen
::
Quaternion
<
double
>
quat
=
Eigen
::
AngleAxisd
(
qFF
(
5
),
Eigen
::
Vector3d
::
UnitZ
())
*
Eigen
::
AngleAxisd
(
qFF
(
4
),
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
qFF
(
3
),
Eigen
::
Vector3d
::
UnitX
());
q
<<
qFF
(
0
),
qFF
(
1
),
qFF
(
2
),
quat
.
x
(),
quat
.
y
(),
quat
.
z
(),
quat
.
w
(),
qJoints
;
}
else
if
(
se3
::
nv
(
m_model
->
joints
[
1
])
==
6
){
dg
::
Vector
qFF
=
qJoints
.
head
<
6
>
();
q
.
resize
(
qJoints
.
size
()
+
1
);
Eigen
::
Quaternion
<
double
>
quat
=
Eigen
::
AngleAxisd
(
qFF
(
5
),
Eigen
::
Vector3d
::
UnitZ
())
*
Eigen
::
AngleAxisd
(
qFF
(
4
),
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
qFF
(
3
),
Eigen
::
Vector3d
::
UnitX
());
q
<<
qFF
(
0
),
qFF
(
1
),
qFF
(
2
),
quat
.
x
(),
quat
.
y
(),
quat
.
z
(),
quat
.
w
(),
qJoints
.
segment
(
6
,
qJoints
.
size
()
-
6
);
assert
(
q
.
size
()
==
m_model
->
nq
);
if
(
!
sphericalJoints
.
empty
()){
if
(
freeFlyerPositionSIN
)
{
dg
::
Vector
qFF
=
freeFlyerPositionSIN
.
access
(
time
);
qJoints
.
head
<
6
>
()
=
qFF
;
//Overwrite qJoints ff with ffposition value
assert
(
sphericalJoints
[
0
]
==
3
);
// FreeFlyer should ideally be present.
}
q
.
resize
(
qJoints
.
size
()
+
sphericalJoints
.
size
());
int
fillingIndex
=
0
;
int
origIndex
=
0
;
for
(
std
::
vector
<
int
>::
const_iterator
it
=
sphericalJoints
.
begin
();
it
<
sphericalJoints
.
end
();
it
++
){
if
(
*
it
-
origIndex
>
0
){
q
.
segment
(
fillingIndex
,
*
it
-
origIndex
)
=
qJoints
.
segment
(
origIndex
,
*
it
-
origIndex
);
fillingIndex
+=
*
it
-
origIndex
;
origIndex
+=
*
it
-
origIndex
;
}
assert
(
*
it
==
origIndex
);
Eigen
::
Quaternion
<
double
>
temp
=
Eigen
::
AngleAxisd
(
qJoints
(
origIndex
+
2
),
Eigen
::
Vector3d
::
UnitZ
())
*
Eigen
::
AngleAxisd
(
qJoints
(
origIndex
+
1
),
Eigen
::
Vector3d
::
UnitY
())
*
Eigen
::
AngleAxisd
(
qJoints
(
origIndex
),
Eigen
::
Vector3d
::
UnitX
());
q
(
fillingIndex
)
=
temp
.
x
();
q
(
fillingIndex
+
1
)
=
temp
.
y
();
q
(
fillingIndex
+
2
)
=
temp
.
z
();
q
(
fillingIndex
+
3
)
=
temp
.
w
();
it
++
;
fillingIndex
+=
4
;
origIndex
+=
3
;
}
if
(
qJoints
.
size
()
>
origIndex
)
q
.
segment
(
fillingIndex
,
qJoints
.
size
()
-
origIndex
)
=
qJoints
.
tail
(
qJoints
.
size
()
-
origIndex
);
}
else
{
q
.
resize
(
qJoints
.
size
());
...
...
@@ -316,7 +343,7 @@ dg::Vector Dynamic::getPinocchioPos(int time)
}
sotDEBUG
(
15
)
<<
"Position out"
<<
q
<<
std
::
endl
;
sotDEBUGOUT
(
15
);
return
q
;
return
q
;
}
Eigen
::
VectorXd
Dynamic
::
getPinocchioVel
(
int
time
)
...
...
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