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
39a3db9c
Commit
39a3db9c
authored
Jul 28, 2016
by
Rohan Budhiraja
Browse files
remove stuff dependent on urdf
parent
95f043a4
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/sot-dynamic/dynamic.h
View file @
39a3db9c
...
...
@@ -47,7 +47,6 @@
/* PINOCCHIO */
#include
<pinocchio/multibody/model.hpp>
#include
<pinocchio/multibody/joint/joint-variant.hpp>
#include
<pinocchio/parsers/urdf.hpp>
#include
<pinocchio/algorithm/rnea.hpp>
#include
<pinocchio/algorithm/jacobian.hpp>
#include
<pinocchio/algorithm/frames.hpp>
...
...
src/python-module-py.cpp
View file @
39a3db9c
...
...
@@ -86,10 +86,6 @@ namespace dynamicgraph{
pointer1
=
PyCObject_AsVoidPtr
(
object
);
Dynamic
*
dyn_entity
=
(
Dynamic
*
)
pointer1
;
std
::
string
msg
(
"Error in obtaining pinocchio model"
);
PyObject
*
dgpyError
=
PyErr_NewException
(
const_cast
<
char
*>
(
msg
.
c_str
()),
NULL
,
NULL
);
try
{
se3
::
python
::
ModelHandler
cppModelHandle
=
boost
::
python
::
extract
<
se3
::
python
::
ModelHandler
>
(
pyPinocchioObject
);
...
...
src/sot-dynamic.cpp
View file @
39a3db9c
...
...
@@ -286,21 +286,29 @@ dg::Vector Dynamic::getPinocchioPos(int time)
if
(
freeFlyerPositionSIN
)
{
dg
::
Vector
qFF
=
freeFlyerPositionSIN
.
access
(
time
);
q
.
resize
(
qJoints
.
size
()
+
7
);
urdf
::
Rotation
rot
;
rot
.
setFromRPY
(
qFF
(
3
),
qFF
(
4
),
qFF
(
5
));
double
x
,
y
,
z
,
w
;
rot
.
getQuaternion
(
x
,
y
,
z
,
w
);
q
<<
qFF
(
0
),
qFF
(
1
),
qFF
(
2
),
x
,
y
,
z
,
w
,
qJoints
;
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
>
();
urdf
::
Rotation
rot
;
rot
.
setFromRPY
(
qFF
(
3
),
qFF
(
4
),
qFF
(
5
));
double
x
,
y
,
z
,
w
;
rot
.
getQuaternion
(
x
,
y
,
z
,
w
);
q
.
resize
(
qJoints
.
size
()
+
1
);
q
<<
qFF
(
0
),
qFF
(
1
),
qFF
(
2
),
x
,
y
,
z
,
w
,
qJoints
.
segment
(
6
,
qJoints
.
size
()
-
6
);
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
);
}
else
{
...
...
unitTesting/kineromeo.py
View file @
39a3db9c
...
...
@@ -5,6 +5,7 @@
#
# ______________________________________________________________________________
# ******************************************************************************
import
pinocchio
as
se3
from
dynamic_graph
import
plug
from
dynamic_graph.sot.core
import
*
from
dynamic_graph.sot.dynamics
import
*
...
...
@@ -24,7 +25,7 @@ set_printoptions(suppress=True, precision=7)
#Define robotName, urdfpath and initialConfig
robotName
=
'romeo'
urdfpath
=
"
/local/rbudhira/git/proyan/sot-pinocchio/unitTesting/
romeoNoToes.urdf"
urdfpath
=
"romeoNoToes.urdf"
initialConfig
=
(
0
,
0
,
.
840252
,
0
,
0
,
0
,
# FF
0
,
0
,
-
0.3490658
,
0.6981317
,
-
0.3490658
,
0
,
# LLEG
...
...
@@ -35,13 +36,24 @@ initialConfig = (0, 0, .840252, 0, 0, 0, # FF
1.5
,
-
0.6
,
0.5
,
1.05
,
-
0.4
,
-
0.3
,
-
0.2
,
# RARM
)
#-----------------------------------------------------------------------------
#---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
#-----------------------------------------------------------------------------
pinocchioModel
=
se3
.
buildModelFromUrdf
(
urdfpath
,
se3
.
JointModelFreeFlyer
())
pinocchioData
=
pinocchioModel
.
createData
()
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
dyn
=
Dynamic
(
"dyn"
)
dyn
.
setModel
(
pinocchioModel
)
dyn
.
setData
(
pinocchioData
)
dyn
.
displayModel
()
robotDim
=
dyn
.
getDimension
()
inertiaRotor
=
(
0
,)
*
6
+
(
5e-4
,)
*
31
gearRatio
=
(
0
,)
*
6
+
(
200
,)
*
31
dyn
.
inertiaRotor
.
value
=
inertiaRotor
...
...
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