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
Gabriele Buondonno
pinocchio
Commits
3c047ae3
Commit
3c047ae3
authored
Mar 16, 2016
by
jcarpent
Browse files
[Python] Expose forward dynamics with contact
parent
86cc0b49
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/python/algorithms.hpp
View file @
3c047ae3
...
...
@@ -28,6 +28,7 @@
#include
"pinocchio/algorithm/non-linear-effects.hpp"
#include
"pinocchio/algorithm/crba.hpp"
#include
"pinocchio/algorithm/aba.hpp"
#include
"pinocchio/algorithm/dynamics.hpp"
#include
"pinocchio/algorithm/kinematics.hpp"
#include
"pinocchio/algorithm/jacobian.hpp"
#include
"pinocchio/algorithm/operational-frames.hpp"
...
...
@@ -94,6 +95,19 @@ namespace se3
aba
(
*
model
,
*
data
,
q
,
v
,
tau
);
return
data
->
ddq
;
}
static
Eigen
::
MatrixXd
fd_llt_proxy
(
const
ModelHandler
&
model
,
DataHandler
&
data
,
const
VectorXd_fx
&
q
,
const
VectorXd_fx
&
v
,
const
VectorXd_fx
&
tau
,
const
eigenpy
::
MatrixXd_fx
&
J
,
const
VectorXd_fx
&
gamma
,
const
bool
update_kinematics
=
true
)
{
forwardDynamics
(
*
model
,
*
data
,
q
,
v
,
tau
,
J
,
gamma
,
update_kinematics
);
return
data
->
ddq
;
}
static
SE3
::
Vector3
com_0_proxy
(
const
ModelHandler
&
model
,
...
...
@@ -327,6 +341,16 @@ namespace se3
"Joint torque tau (size Model::nv)"
),
"Compute ABA, put the result in Data::ddq and return it."
);
bp
::
def
(
"forwardDynamics"
,
fd_llt_proxy
,
bp
::
args
(
"Model"
,
"Data"
,
"Joint configuration q (size Model::nq)"
,
"Joint velocity v (size Model::nv)"
,
"Joint torque tau (size Model::nv)"
,
"Contact Jacobian J (size nb_constraint * Model::nv)"
,
"Contact drift gamma (size nb_constraint)"
,
"Update kinematics (if true, it updates the dynamic variable according to the current state)"
),
"Solve the forward dynamics problem with contacts, put the result in Data::ddq and return it."
);
bp
::
def
(
"centerOfMass"
,
com_0_proxy
,
bp
::
args
(
"Model"
,
"Data"
,
"Configuration q (size Model::nq)"
,
...
...
src/python/data.hpp
View file @
3c047ae3
...
...
@@ -112,6 +112,8 @@ namespace se3
.
ADD_DATA_PROPERTY_CONST
(
double
,
kinetic_energy
,
"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)"
)
.
ADD_DATA_PROPERTY_CONST
(
double
,
potential_energy
,
"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)"
)
.
ADD_DATA_PROPERTY_CONST
(
Eigen
::
VectorXd
,
lambda
,
"Lagrange Multipliers linked to contact forces"
)
;
}
...
...
@@ -159,6 +161,8 @@ namespace se3
IMPL_DATA_PROPERTY_CONST
(
double
,
kinetic_energy
,
"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)"
)
IMPL_DATA_PROPERTY_CONST
(
double
,
potential_energy
,
"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)"
)
IMPL_DATA_PROPERTY_CONST
(
Eigen
::
VectorXd
,
lambda
,
"Lagrange Multipliers linked to contact forces"
)
/* --- Expose --------------------------------------------------------- */
static
void
expose
()
{
...
...
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