...
 
Commits (66)
......@@ -147,6 +147,7 @@ If you want to learn about Crocoddyl, take a look at the Jupyter notebooks. Star
- [examples/notebooks/unicycle_towards_origin.ipynb](https://github.com/loco-3d/crocoddyl/blob/devel/examples/notebooks/unicycle_towards_origin.ipynb)
- [examples/notebooks/cartpole_swing_up.ipynb](https://github.com/loco-3d/crocoddyl/blob/devel/examples/notebooks/cartpole_swing_up.py)
- [examples/notebooks/arm_manipulation.ipynb](https://github.com/loco-3d/crocoddyl/blob/devel/examples/notebooks/arm_manipulation.ipynb)
- [examples/notebooks/whole_body_manipulation.ipynb](https://github.com/loco-3d/crocoddyl/blob/devel/examples/notebooks/whole_body_manipulation.ipynb)
- [examples/notebooks/bipedal_walking.ipynb](https://github.com/loco-3d/crocoddyl/blob/devel/examples/notebooks/bipedal_walking.ipynb)
- [examples/notebooks/introduction_to_crocoddyl.ipynb](https://github.com/loco-3d/crocoddyl/blob/devel/examples/notebooks/introduction_to_crocoddyl.ipynb)
......
......@@ -124,7 +124,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calcDiff [us]:\t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -147,7 +148,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calc [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -175,7 +177,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded diff calc [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -211,7 +214,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded aba [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -245,7 +249,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded aba-derivs [us]:\t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -261,8 +266,9 @@ int main(int argc, char* argv[]) {
}
double avg_bp = AVG(duration);
double stddev_bp = STDDEV(duration);
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (per nodes: " << avg_bp / N << " +- "
<< stddev_bp / N << ")" << std::endl;
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_bp / N << " +- " << stddev_bp / N << ")"
<< std::endl;
// Forward pass timings
duration.setZero();
......@@ -273,8 +279,9 @@ int main(int argc, char* argv[]) {
}
double avg_fp = AVG(duration);
double stddev_fp = STDDEV(duration);
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (per nodes: " << avg_fp / N << " +- "
<< stddev_fp / N << ")" << std::endl;
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_fp / N << " +- " << stddev_fp / N << ")"
<< std::endl;
/*******************************************************************************/
/*************************** CODE GENERATION TIMINGS ***************************/
......@@ -303,7 +310,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calcDiff [us]:\t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -326,7 +334,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calc [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -341,8 +350,9 @@ int main(int argc, char* argv[]) {
}
avg_bp = AVG(duration);
stddev_bp = STDDEV(duration);
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (per nodes: " << avg_bp / N << " +- "
<< stddev_bp / N << ")" << std::endl;
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_bp / N << " +- " << stddev_bp / N << ")"
<< std::endl;
// Forward pass timings
for (unsigned int i = 0; i < T; ++i) {
......@@ -352,6 +362,7 @@ int main(int argc, char* argv[]) {
}
avg_fp = AVG(duration);
stddev_fp = STDDEV(duration);
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (per nodes: " << avg_fp / N << " +- "
<< stddev_fp / N << ")" << std::endl;
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_fp / N << " +- " << stddev_fp / N << ")"
<< std::endl;
}
This diff is collapsed.
This diff is collapsed.
......@@ -123,7 +123,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calcDiff [us]:\t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -146,7 +147,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calc [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -167,7 +169,6 @@ int main(int argc, char* argv[]) {
boost::static_pointer_cast<crocoddyl::IntegratedActionModelEuler>(problem->get_runningModels()[j]);
boost::shared_ptr<crocoddyl::IntegratedActionDataEuler> d =
boost::static_pointer_cast<crocoddyl::IntegratedActionDataEuler>(problem->get_runningDatas()[j]);
m->get_differential()->calc(d->differential, xs[j], us[j]);
}
duration[i] = timer.get_us_duration();
......@@ -175,7 +176,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded diff calc [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -191,8 +193,9 @@ int main(int argc, char* argv[]) {
}
double avg_bp = AVG(duration);
double stddev_bp = STDDEV(duration);
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (per nodes: " << avg_bp / N << " +- "
<< stddev_bp / N << ")" << std::endl;
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_bp / N << " +- " << stddev_bp / N << ")"
<< std::endl;
// Forward pass timings
duration.setZero();
......@@ -203,8 +206,9 @@ int main(int argc, char* argv[]) {
}
double avg_fp = AVG(duration);
double stddev_fp = STDDEV(duration);
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (per nodes: " << avg_fp / N << " +- "
<< stddev_fp / N << ")" << std::endl;
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_fp / N << " +- " << stddev_fp / N << ")"
<< std::endl;
/*******************************************************************************/
/*************************** CODE GENERATION TIMINGS ***************************/
......@@ -233,7 +237,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calcDiff [us]:\t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -256,7 +261,8 @@ int main(int argc, char* argv[]) {
avg[ithread] = AVG(duration);
stddev[ithread] = STDDEV(duration);
std::cout << ithread + 1 << " threaded calc [us]: \t" << avg[ithread] << " +- " << stddev[ithread]
<< " (per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< " (max: " << duration.maxCoeff() << ", min: " << duration.minCoeff()
<< ", per nodes: " << avg[ithread] * (ithread + 1) / N << " +- " << stddev[ithread] * (ithread + 1) / N
<< ")" << std::endl;
}
......@@ -271,8 +277,9 @@ int main(int argc, char* argv[]) {
}
avg_bp = AVG(duration);
stddev_bp = STDDEV(duration);
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (per nodes: " << avg_bp / N << " +- "
<< stddev_bp / N << ")" << std::endl;
std::cout << "backwardPass [us]:\t\t" << avg_bp << " +- " << stddev_bp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_bp / N << " +- " << stddev_bp / N << ")"
<< std::endl;
// Forward pass timings
for (unsigned int i = 0; i < T; ++i) {
......@@ -282,6 +289,7 @@ int main(int argc, char* argv[]) {
}
avg_fp = AVG(duration);
stddev_fp = STDDEV(duration);
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (per nodes: " << avg_fp / N << " +- "
<< stddev_fp / N << ")" << std::endl;
std::cout << "forwardPass [us]: \t\t" << avg_fp << " +- " << stddev_fp << " (max: " << duration.maxCoeff()
<< ", min: " << duration.minCoeff() << ", per nodes: " << avg_fp / N << " +- " << stddev_fp / N << ")"
<< std::endl;
}
......@@ -175,7 +175,9 @@ class GepettoDisplay(DisplayAbstract):
for key, contact in data.differential.multibody.contacts.contacts.items():
if model.differential.contacts.contacts[key].active:
oMf = contact.pinocchio.oMi[contact.joint] * contact.jMf
force = contact.jMf.actInv(contact.f)
fiMo = pinocchio.SE3(contact.pinocchio.oMi[contact.joint].rotation.T,
contact.jMf.translation)
force = fiMo.actInv(contact.f)
nsurf = np.array([0., 0., 1.])
mu = 0.7
for k, c in model.differential.costs.costs.items():
......@@ -191,7 +193,8 @@ class GepettoDisplay(DisplayAbstract):
for key, impulse in data.multibody.impulses.impulses.items():
if model.impulses.impulses[key].active:
oMf = impulse.pinocchio.oMi[impulse.joint] * impulse.jMf
force = impulse.jMf.actInv(impulse.f)
fiMo = pinocchio.SE3(impulse.pinocchio.oMi[impulse.joint].rotation.T, impulse.jMf.translation)
force = fiMo.actInv(impulse.f)
nsurf = np.array([0., 0., 1.])
mu = 0.7
for k, c in model.costs.costs.items():
......
......@@ -7,7 +7,6 @@
///////////////////////////////////////////////////////////////////////////////
#include "python/crocoddyl/core/core.hpp"
#include "crocoddyl/core/actuation/actuation-squashing.hpp"
#include "crocoddyl/core/utils/exception.hpp"
......
......@@ -31,14 +31,9 @@ void exposeDifferentialActionAbstract() {
":param nu: dimension of control vector\n"
":param nr: dimension of cost-residual vector (default 1)"))
.def("calc", pure_virtual(&DifferentialActionModelAbstract_wrap::calc), bp::args("self", "data", "x", "u"),
"Compute the state evolution and cost value.\n\n"
"First, it describes the time-continuous evolution of our dynamical system\n"
"in which along predefined integrated action self we might obtain the\n"
"next discrete state. Indeed it computes the time derivatives of the\n"
"state from a predefined dynamical system. Additionally it computes the\n"
"cost value associated to this state and control pair.\n"
"Compute the system acceleration and cost value.\n\n"
":param data: differential action data\n"
":param x: state vector\n"
":param x: state tuple\n"
":param u: control input")
.def<void (DifferentialActionModelAbstract::*)(const boost::shared_ptr<DifferentialActionDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
......
......@@ -116,8 +116,9 @@ void exposeShootingProblem() {
.add_property("ndx",
bp::make_function(&ShootingProblem::get_ndx, bp::return_value_policy<bp::return_by_value>()),
"dimension of the tangent space of the state manifold")
.add_property("nu", bp::make_function(&ShootingProblem::get_nu, bp::return_value_policy<bp::return_by_value>()),
"dimension of the control vector");
.add_property("nu_max",
bp::make_function(&ShootingProblem::get_nu_max, bp::return_value_policy<bp::return_by_value>()),
"dimension of the maximun control vector");
}
} // namespace python
......
......@@ -6,6 +6,7 @@
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include "python/crocoddyl/core/core.hpp"
#include "python/crocoddyl/core/state-base.hpp"
namespace crocoddyl {
......
......@@ -48,6 +48,12 @@ void exposeContactAbstract() {
":param data: contact data\n"
":param df_dx: Jacobian of the force with respect to the state\n"
":param df_du: Jacobian of the force with respect to the control")
.def("setZeroForce", &ContactModelAbstract_wrap::setZeroForce, bp::args("self", "data"),
"Set zero the spatial force.\n\n"
":param data: contact data")
.def("setZeroForceDiff", &ContactModelAbstract_wrap::setZeroForceDiff, bp::args("self", "data"),
"Set zero the derivatives of the spatial force.\n\n"
":param data: contact data")
.def("createData", &ContactModelAbstract_wrap::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args("self", "data"),
"Create the contact data.\n\n"
......
......@@ -69,27 +69,31 @@ void exposeContactMultiple() {
":param name: contact name\n"
":param active: contact status (true for active and false for inactive)")
.def("calc", &ContactModelMultiple::calc, bp::args("self", "data", "x"),
"Compute the total contact Jacobian and drift.\n\n"
"Compute the contact Jacobian and contact acceleration.\n\n"
"The rigid contact model throught acceleration-base holonomic constraint\n"
"of the contact frame placement.\n"
":param data: contact data\n"
":param x: state vector")
.def("calcDiff", &ContactModelMultiple::calcDiff, bp::args("self", "data", "x"),
"Compute the derivatives of the total contact holonomic constraint.\n\n"
"Compute the derivatives of the contact holonomic constraint.\n\n"
"The rigid contact model throught acceleration-base holonomic constraint\n"
"of the contact frame placement.\n"
":param data: contact data\n"
":param x: state vector\n")
.def("updateAcceleration", &ContactModelMultiple::updateAcceleration, bp::args("self", "data", "dv"),
"Update the constrained acceleration.\n\n"
"Update the constrained system acceleration.\n\n"
":param data: contact data\n"
":param dv: constrained acceleration (dimension nv)")
.def("updateForce", &ContactModelMultiple::updateForce, bp::args("self", "data", "force"),
"Convert the force into a stack of spatial forces.\n\n"
"Update the spatial force in frame coordinate.\n\n"
":param data: contact data\n"
":param force: force vector (dimension nc)")
.def("updateAccelerationDiff", &ContactModelMultiple::updateAccelerationDiff, bp::args("self", "data", "ddv_dx"),
"Update the Jacobian of the constrained system acceleration.\n\n"
":param data: contact data\n"
":param ddv_dx: Jacobian of the system acceleration in generalized coordinates (dimension nv*ndx)")
.def("updateForceDiff", &ContactModelMultiple::updateForceDiff, bp::args("self", "data", "df_dx", "df_du"),
"Update the Jacobians of the force.\n\n"
"Update the Jacobians of the spatial force defined in frame coordinates.\n\n"
":param data: contact data\n"
":param df_dx: Jacobian of the force with respect to the state (dimension nc*ndx)\n"
":param df_du: Jacobian of the force with respect to the control (dimension nc*nu)")
......@@ -137,22 +141,27 @@ void exposeContactMultiple() {
":param model: multicontact model\n"
":param data: Pinocchio data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
.add_property("Jc", bp::make_getter(&ContactDataMultiple::Jc, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::Jc), "Jacobian for all contacts (active and inactive)")
.add_property("a0", bp::make_getter(&ContactDataMultiple::a0, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::a0),
"desired acceleration for all contacts (active and inactive)")
.add_property("da0_dx", bp::make_getter(&ContactDataMultiple::da0_dx, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::da0_dx),
"Jacobian of the desired acceleration for all contacts (active and inactive)")
bp::make_setter(&ContactDataMultiple::Jc),
"contact Jacobian in frame coordinate (memory defined for active and inactive contacts)")
.add_property(
"a0", bp::make_getter(&ContactDataMultiple::a0, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::a0),
"desired spatial contact acceleration in frame coordinate (memory defined for active and inactive contacts)")
.add_property(
"da0_dx", bp::make_getter(&ContactDataMultiple::da0_dx, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::da0_dx),
"Jacobian of the desired spatial contact acceleration in frame coordinate (memory defined for active and "
"inactive contacts)")
.add_property("dv", bp::make_getter(&ContactDataMultiple::dv, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::dv), "constrained acceleration in generalized coordinates")
bp::make_setter(&ContactDataMultiple::dv),
"constrained system acceleration in generalized coordinates")
.add_property("ddv_dx", bp::make_getter(&ContactDataMultiple::ddv_dx, bp::return_internal_reference<>()),
bp::make_setter(&ContactDataMultiple::ddv_dx),
"Jacobian of the constrained acceleration in generalized coordinates")
"Jacobian of the constrained system acceleration in generalized coordinates")
.add_property("contacts",
bp::make_getter(&ContactDataMultiple::contacts, bp::return_value_policy<bp::return_by_value>()),
"stack of contacts data")
.def_readwrite("fext", &ContactDataMultiple::fext, "external spatial forces");
.def_readwrite("fext", &ContactDataMultiple::fext, "external spatial forces in join coordinates");
}
} // namespace python
......
......@@ -19,40 +19,53 @@ void exposeCostContactForce() {
bp::init<boost::shared_ptr<StateMultibody>, boost::shared_ptr<ActivationModelAbstract>, FrameForce, int>(
bp::args("self", "state", "activation", "fref", "nu"),
"Initialize the contact force cost model.\n\n"
"Note that the activation.nr is lower / equals than 6.\n"
":param state: state of the multibody system\n"
":param activation: activation model\n"
":param fref: reference contact force\n"
":param fref: reference spatial contact force in the contact coordinates\n"
":param nu: dimension of control vector"))
.def(bp::init<boost::shared_ptr<StateMultibody>, boost::shared_ptr<ActivationModelAbstract>, FrameForce>(
bp::args("self", "state", "activation", "fref"),
"Initialize the contact force cost model.\n\n"
"For this case the default nu is equals to model.nv.\n"
"For this case the default nu is equals to state.nv.\n"
"Note that the activation.nr is lower / equals than 6.\n"
":param state: state of the multibody system\n"
":param activation: activation model\n"
":param fref: reference contact force\n"))
.def(bp::init<boost::shared_ptr<StateMultibody>, FrameForce, int>(
bp::args("self", "state", "fref", "nu"),
":param fref: reference spatial contact force in the contact coordinates\n"))
.def(bp::init<boost::shared_ptr<StateMultibody>, FrameForce, int, int>(
bp::args("self", "state", "fref", "nr", "nu"),
"Initialize the contact force cost model.\n\n"
"For this case the default activation model is quadratic, i.e.\n"
"crocoddyl.ActivationModelQuad(6).\n"
"crocoddyl.ActivationModelQuad(nr).\n"
"Note that the nr is lower / equals than 6.\n"
":param state: state of the multibody system\n"
":param fref: reference contact force\n"
":param fref: reference spatial contact force in the contact coordinates\n"
":param nr: dimension of force vector (>= 6)\n"
":param nu: dimension of control vector"))
.def(bp::init<boost::shared_ptr<StateMultibody>, FrameForce, int>(
bp::args("self", "state", "fref", "nr"),
"Initialize the contact force cost model.\n\n"
"For this case the default activation model is quadratic, i.e.\n"
"crocoddyl.ActivationModelQuad(nr), and nu is equals to state.nv.\n"
"Note that the nr is lower / equals than 6.\n"
":param state: state of the multibody system\n"
":param fref: reference spatial contact force in the contact coordinates\n"
":param nr: dimension of force vector (>= 6)"))
.def(bp::init<boost::shared_ptr<StateMultibody>, FrameForce>(
bp::args("self", "state", "fref"),
"Initialize the contact force cost model.\n\n"
"For this case the default activation model is quadratic, i.e.\n"
"crocoddyl.ActivationModelQuad(6), and nu is equals to model.nv.\n"
"crocoddyl.ActivationModelQuad(6), and nu is equals to state.nv.\n"
":param state: state of the multibody system\n"
":param fref: reference force\n"))
":param fref: reference spatial contact force in the contact coordinates"))
.def<void (CostModelContactForce::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calc", &CostModelContactForce::calc, bp::args("self", "data", "x", "u"),
"Compute the contact force cost.\n\n"
":param data: cost data\n"
":param x: time-discrete state vector\n"
":param u: time-discrete control input")
":param x: state vector\n"
":param u: control input")
.def<void (CostModelContactForce::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>("calc", &CostModelAbstract::calc,
bp::args("self", "data", "x"))
......@@ -62,8 +75,8 @@ void exposeCostContactForce() {
"calcDiff", &CostModelContactForce::calcDiff, bp::args("self", "data", "x", "u"),
"Compute the derivatives of the contact force cost.\n\n"
":param data: action data\n"
":param x: time-discrete state vector\n"
":param u: time-discrete control input\n")
":param x: state vector\n"
":param u: control input\n")
.def<void (CostModelContactForce::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calcDiff", &CostModelAbstract::calcDiff, bp::args("self", "data", "x"))
......@@ -76,9 +89,10 @@ void exposeCostContactForce() {
":return cost data.")
.add_property("reference",
bp::make_function(&CostModelContactForce::get_fref, bp::return_internal_reference<>()),
&CostModelContactForce::set_reference<FrameForce>, "reference frame force")
&CostModelContactForce::set_reference<FrameForce>,
"reference spatial contact force in the contact coordinates")
.add_property("fref", bp::make_function(&CostModelContactForce::get_fref, bp::return_internal_reference<>()),
&CostModelContactForce::set_fref, "reference contact force");
&CostModelContactForce::set_fref, "reference spatial contact force in the contact coordinates");
bp::register_ptr_to_python<boost::shared_ptr<CostDataContactForce> >();
......
///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2020-2022, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include "python/crocoddyl/multibody/multibody.hpp"
#include "python/crocoddyl/multibody/cost-base.hpp"
#include "crocoddyl/multibody/costs/contact-impulse.hpp"
namespace crocoddyl {
namespace python {
void exposeCostContactImpulse() {
bp::class_<CostModelContactImpulse, bp::bases<CostModelAbstract> >(
"CostModelContactImpulse",
bp::init<boost::shared_ptr<StateMultibody>, boost::shared_ptr<ActivationModelAbstract>, FrameForce>(
bp::args("self", "state", "activation", "fref"),
"Initialize the contact impulse cost model.\n\n"
"Note that the activation.nr is lower / equals than 6.\n"
":param state: state of the multibody system\n"
":param activation: activation model\n"
":param fref: reference spatial contact impulse in the contact coordinates"))
.def(bp::init<boost::shared_ptr<StateMultibody>, FrameForce, int>(
bp::args("self", "state", "fref", "nr"),
"Initialize the contact impulse cost model.\n\n"
"For this case the default activation model is quadratic, i.e.\n"
"crocoddyl.ActivationModelQuad(nr).\n"
"Note that the nr is lower / equals than 6.\n"
":param state: state of the multibody system\n"
":param fref: reference spatial contact impulse in the contact coordinates\n"
":param nr: dimension of impulse vector (>= 6)"))
.def(bp::init<boost::shared_ptr<StateMultibody>, FrameForce>(
bp::args("self", "state", "fref"),
"Initialize the contact impulse cost model.\n\n"
"For this case the default activation model is quadratic, i.e.\n"
"crocoddyl.ActivationModelQuad(6).\n"
":param state: state of the multibody system\n"
":param fref: reference spatial contact impulse in the contact coordinates"))
.def<void (CostModelContactImpulse::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calc", &CostModelContactImpulse::calc, bp::args("self", "data", "x", "u"),
"Compute the contact impulse cost.\n\n"
":param data: cost data\n"
":param x: state vector\n"
":param u: control input")
.def<void (CostModelContactImpulse::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calc", &CostModelAbstract::calc, bp::args("self", "data", "x"))
.def<void (CostModelContactImpulse::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calcDiff", &CostModelContactImpulse::calcDiff, bp::args("self", "data", "x", "u"),
"Compute the derivatives of the contact impulse cost.\n\n"
":param data: action data\n"
":param x: state vector\n"
":param u: control input\n")
.def<void (CostModelContactImpulse::*)(const boost::shared_ptr<CostDataAbstract>&,
const Eigen::Ref<const Eigen::VectorXd>&)>(
"calcDiff", &CostModelAbstract::calcDiff, bp::args("self", "data", "x"))
.def("createData", &CostModelContactImpulse::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args("self", "data"),
"Create the contact impulse cost data.\n\n"
"Each cost model has its own data that needs to be allocated. This function\n"
"returns the allocated data for a predefined cost.\n"
":param data: shared data\n"
":return cost data.")
.add_property("reference",
bp::make_function(&CostModelContactImpulse::get_fref, bp::return_internal_reference<>()),
&CostModelContactImpulse::set_reference<FrameForce>,
"reference spatial contact impulse in the contact coordinates")
.add_property("fref", bp::make_function(&CostModelContactImpulse::get_fref, bp::return_internal_reference<>()),
&CostModelContactImpulse::set_fref,
"reference spatial contact impulse in the contact coordinates");
bp::register_ptr_to_python<boost::shared_ptr<CostDataContactImpulse> >();
bp::class_<CostDataContactImpulse, bp::bases<CostDataAbstract> >(
"CostDataContactImpulse", "Data for contact impulse cost.\n\n",
bp::init<CostModelContactImpulse*, DataCollectorAbstract*>(
bp::args("self", "model", "data"),
"Create contact impulse cost data.\n\n"
":param model: contact impulse cost model\n"
":param data: shared data")[bp::with_custodian_and_ward<1, 2, bp::with_custodian_and_ward<1, 3> >()])
.add_property(
"impulse", bp::make_getter(&CostDataContactImpulse::impulse, bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&CostDataContactImpulse::impulse), "impulse data associated with the current cost");
}
} // namespace python
} // namespace crocoddyl
......@@ -41,6 +41,12 @@ void exposeImpulseAbstract() {
"Update the Jacobian of the impulse force.\n\n"
":param data: impulse data\n"
":param df_dx: Jacobian of the impulse force (dimension ni*ndx)")
.def("setZeroForce", &ImpulseModelAbstract_wrap::setZeroForce, bp::args("self", "data"),
"Set zero the spatial force.\n\n"
":param data: contact data")
.def("setZeroForceDiff", &ImpulseModelAbstract_wrap::setZeroForceDiff, bp::args("self", "data"),
"Set zero the derivatives of the spatial force.\n\n"
":param data: contact data")
.def("createData", &ImpulseModelAbstract_wrap::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
bp::args("self", "data"),
"Create the impulse data.\n\n"
......
......@@ -68,31 +68,31 @@ void exposeImpulseMultiple() {
":param name: impulse name\n"
":param active: impulse status (true for active and false for inactive)")
.def("calc", &ImpulseModelMultiple::calc, bp::args("self", "data", "x"),
"Compute the total impulse Jacobian and drift.\n\n"
"Compute the impulse Jacobian and drift.\n\n"
"The rigid impulse model throught acceleration-base holonomic constraint\n"
"of the impulse frame placement.\n"
":param data: impulse data\n"
":param x: state vector")
.def("calcDiff", &ImpulseModelMultiple::calcDiff, bp::args("self", "data", "x"),
"Compute the derivatives of the total impulse holonomic constraint.\n\n"
"Compute the derivatives of the impulse holonomic constraint.\n\n"
"The rigid impulse model throught acceleration-base holonomic constraint\n"
"of the impulse frame placement.\n"
":param data: impulse data\n"
":param x: state vector\n")
.def("updateVelocity", &ImpulseModelMultiple::updateVelocity, bp::args("self", "data", "vnext"),
"Update the velocity after impulse.\n\n"
"Update the system velocity after impulse.\n\n"
":param data: impulse data\n"
":param vnext: velocity after impulse (dimension nv)")
.def("updateForce", &ImpulseModelMultiple::updateForce, bp::args("self", "data", "lambda"),
"Convert the force into a stack of spatial forces.\n\n"
"Update the spatial impulse defined in frame coordinate.\n\n"
":param data: impulse data\n"
":param force: force vector (dimension ni)")
.def("updateVelocityDiff", &ImpulseModelMultiple::updateVelocityDiff, bp::args("self", "data", "dvnext_dx"),
"Update the velocity after impulse.\n\n"
"Update the Jacobian of the system velocity after impulse.\n\n"
":param data: impulse data\n"
":param dvnext_dx: Jacobian of the impulse velocity (dimension nv*ndx)")
.def("updateForceDiff", &ImpulseModelMultiple::updateForceDiff, bp::args("self", "data", "df_dx"),
"Update the Jacobian of the impulse force.\n\n"
"Update the Jacobian of the spatial impulse defined in frame coordinate.\n\n"
":param data: impulse data\n"
":param df_dx: Jacobian of the impulse force (dimension ni*ndx)")
.def("createData", &ImpulseModelMultiple::createData, bp::with_custodian_and_ward_postcall<0, 2>(),
......@@ -141,9 +141,9 @@ void exposeImpulseMultiple() {
bp::make_setter(&ImpulseDataMultiple::dv0_dq),
"Jacobian of the previous impulse velocity (active and inactive)")
.add_property("vnext", bp::make_getter(&ImpulseDataMultiple::vnext, bp::return_internal_reference<>()),
bp::make_setter(&ImpulseDataMultiple::vnext), "impulse velocity")
bp::make_setter(&ImpulseDataMultiple::vnext), "impulse system velocity")
.add_property("dvnext_dx", bp::make_getter(&ImpulseDataMultiple::dvnext_dx, bp::return_internal_reference<>()),
bp::make_setter(&ImpulseDataMultiple::dvnext_dx), "Jacobian of the impulse velocity")
bp::make_setter(&ImpulseDataMultiple::dvnext_dx), "Jacobian of the impulse system velocity")
.add_property("impulses",
bp::make_getter(&ImpulseDataMultiple::impulses, bp::return_value_policy<bp::return_by_value>()),
"stack of impulses data")
......
......@@ -39,6 +39,7 @@ void exposeMultibody() {
exposeCostFrameRotation();
exposeCostFrameVelocity();
exposeCostContactForce();
exposeCostContactImpulse();
exposeCostContactFrictionCone();
exposeCostImpulseCoM();
exposeCostImpulseFrictionCone();
......
......@@ -43,6 +43,7 @@ void exposeCostFrameRotation();
void exposeCostFrameVelocity();
void exposeCostContactForce();
void exposeCostContactFrictionCone();
void exposeCostContactImpulse();
void exposeCostImpulseFrictionCone();
void exposeCostImpulseCoM();
void exposeContact3D();
......
......@@ -49,11 +49,11 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"SimpleBipedGaitProblem class builds action models for each locomotion phase:\n",
" - createSwingFootModel: defines an action model for the swing phase\n",
" - createFootSwitchModel: defines an action model for switch knots between phases\n",
"`SimpleBipedGaitProblem` class builds action models for each locomotion phase:\n",
" - `createSwingFootModel`: defines an action model for the swing phase\n",
" - `createFootSwitchModel`: defines an action model for switch knots between phases\n",
" \n",
"Then we build a walking by combining a set of contact phases and their contact switches. This is defined by createFootstepModel\n",
"Then we build a walking by combining a set of contact phases and their contact switches. This is defined by `createFootstepModel`\n",
"\n",
"Now let's create a walking OC problem for the Talos legs."
]
......@@ -82,7 +82,7 @@
"# Create the initial state\n",
"q0 = talos_legs.q0.copy()\n",
"v0 = pinocchio.utils.zero(talos_legs.model.nv)\n",
"x0 = np.concatenate([q0,v0])\n",
"x0 = np.concatenate([q0, v0])\n",
"\n",
"\n",
"# Creating the walking problem\n",
......@@ -135,7 +135,7 @@
"metadata": {},
"source": [
"With the following commands we can plot \n",
" - the state and control trajectories, and\n",
" - the state and control trajectories\n",
" - the DDP performance"
]
},
......@@ -145,19 +145,17 @@
"metadata": {},
"outputs": [],
"source": [
"%matplotlib inline\n",
"\n",
"# Plotting the solution and the DDP convergence\n",
"log = ddp.getCallbacks()[0]\n",
"crocoddyl.plotOCSolution(log.xs, log.us)\n",
"crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.grads, log.stops, log.steps)\n"
"crocoddyl.plotConvergence(log.costs, log.u_regs, log.x_regs, log.grads, log.stops, log.steps)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally we can visualize the solution using gepetto-viewer."
"Finally we can visualize the solution."
]
},
{
......@@ -166,9 +164,10 @@
"metadata": {},
"outputs": [],
"source": [
"# Visualization of the DDP solution in gepetto-viewer\n",
"rate = 2.\n",
"display.displayFromSolver(ddp, rate)"
"# Visualization of the DDP solution in meshcat\n",
"display.rate = -1\n",
"display.freq = 1\n",
"display.displayFromSolver(ddp)"
]
},
{
......@@ -183,32 +182,25 @@
" 2. What happens when do we change $dt$ (e.g. 2e-2 secs)?\n",
" 3. What happens when do we change the number of step knots (e.g. 10)?\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 2",
"display_name": "Python 3",
"language": "python",
"name": "python2"
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.17"
"pygments_lexer": "ipython3",
"version": "3.8.3"
}
},
"nbformat": 4,
......
......@@ -51,23 +51,27 @@
"import crocoddyl\n",
"import pinocchio\n",
"import numpy as np\n",
"from IPython.display import HTML\n",
"from cartpole_utils import animateCartpole\n",
"\n",
"\n",
"class DifferentialActionModelCartpole(crocoddyl.DifferentialActionModelAbstract):\n",
" def __init__(self):\n",
" crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(4), 1, 6) # nu = 1; nr = 6\n",
" crocoddyl.DifferentialActionModelAbstract.__init__(self, crocoddyl.StateVector(4), 1, 6) # nu = 1; nr = 6\n",
" self.unone = np.zeros(self.nu)\n",
"\n",
" self.m1 = 1.\n",
" self.m2 = .1\n",
" self.l = .5\n",
" self.g = 9.81\n",
" self.costWeights = [ 1., 1., 0.1, 0.001, 0.001, 1. ] # sin, 1-cos, x, xdot, thdot, f\n",
" self.costWeights = [1., 1., 0.1, 0.001, 0.001, 1.] # sin, 1-cos, x, xdot, thdot, f\n",
" \n",
" def calc(self, data, x, u=None):\n",
" if u is None: u=model.unone\n",
" if u is None: \n",
" u = model.unone\n",
" # Getting the state and control variables\n",
" y, th, ydot, thdot = np.asscalar(x[0]), np.asscalar(x[1]), np.asscalar(x[2]), np.asscalar(x[3])\n",
" f = np.asscalar(u[0])\n",
" y, th, ydot, thdot = x[0].item(), x[1].item(), x[2].item(), x[3].item()\n",
" f = u[0].item()\n",
"\n",
" # Shortname for system parameters\n",
" m1, m2, l, g = self.m1, self.m2, self.l, self.g\n",
......@@ -80,15 +84,15 @@
" # You don't need to implement integration rules for your dynamic system.\n",
" # Remember that DAM implemented action models in continuous-time.\n",
" m = m1 + m2\n",
" mu = m1 + m2 * s**2\n",
" xddot, thddot = cartpole_dynamics(self, data, x, u) ### Write the cartpole dynamics here\n",
" mu = m1 + m2 * s ** 2\n",
" xddot, thddot = cartpole_dynamics(self, data, x, u) # Write the cartpole dynamics here\n",
" data.xout = np.matrix([ xddot, thddot ]).T\n",
" \n",
" # Computing the cost residual and value\n",
" data.r = np.matrix(self.costWeights * np.array([ s, 1-c, y, ydot, thdot, f ])).T\n",
" data.cost = .5* np.asscalar(sum(np.asarray(data.r)**2))\n",
" data.r = np.matrix(self.costWeights * np.array([ s, 1 - c, y, ydot, thdot, f ])).T\n",
" data.cost = .5 * sum(np.asarray(data.r) ** 2).item()\n",
"\n",
" def calcDiff(model,data,x,u=None):\n",
" def calcDiff(model, data, x, u=None):\n",
" # Advance user might implement the derivatives\n",
" pass"
]
......@@ -124,7 +128,7 @@
"source": [
"cartpoleDAM = DifferentialActionModelCartpole()\n",
"cartpoleData = cartpoleDAM.createData()\n",
"x = cartpoleDAM.State.rand()\n",
"x = cartpoleDAM.state.rand()\n",
"u = np.zeros(1)\n",
"cartpoleDAM.calc(cartpoleData, x, u)"
]
......@@ -213,9 +217,9 @@
"outputs": [],
"source": [
"# Fill the number of knots (T) and the time step (dt)\n",
"x0 = np.matrix([ 0., 3.14, 0., 0. ]).T\n",
"T = 50\n",
"problem = crocoddyl.ShootingProblem(x0, [ cartpoleIAM ]*T, cartpoleIAM)"
"x0 = np.matrix([0., 3.14, 0., 0.]).T\n",
"T = 50\n",
"problem = crocoddyl.ShootingProblem(x0, [cartpoleIAM] * T, cartpoleIAM)"
]
},
{
......@@ -231,7 +235,7 @@
"metadata": {},
"outputs": [],
"source": [
"us = [ pinocchio.utils.zero(cartpoleIAM.differential.nu) ]*T\n",
"us = [pinocchio.utils.zero(cartpoleIAM.differential.nu)] * T\n",
"xs = problem.rollout(us)"
]
},
......@@ -239,31 +243,7 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"In cartpole_utils, we provite a plotCartpole and a animateCartpole methods."
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%%capture\n",
"%matplotlib inline\n",
"from cartpole_utils import animateCartpole\n",
"anim = animateCartpole(xs)\n",
"\n",
"# If you encounter problems probably you need to install ffmpeg/libav-tools:\n",
"# sudo apt-get install ffmpeg\n",
"# or\n",
"# sudo apt-get install libav-tools"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"And let's display this rollout!\n",
"In cartpole_utils, we provite a plotCartpole and a animateCartpole methods. Let's display this rollout!\n",
"\n",
"Note that to_jshtml spawns the video control commands."
]
......@@ -274,16 +254,7 @@
"metadata": {},
"outputs": [],
"source": [
"from IPython.display import HTML\n",
"# HTML(anim.to_jshtml())\n",
"HTML(anim.to_html5_video())"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now we want to create the solver (SolverDDP class) and run it. Display the result. **Do you like it?**"
"HTML(animateCartpole(xs).to_jshtml())"
]
},
{
......@@ -293,22 +264,9 @@
"outputs": [],
"source": [
"# %load solutions/cartpole_ddp.py\n",
"##########################################################################\n",
"################## TODO: Create the DDP solver and run it ###############\n",
"###########################################################################"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%%capture\n",
"%matplotlib inline\n",
"\n",
"# Create animation\n",
"anim = animateCartpole(xs)"
"# #########################################################################\n",
"# ################# TODO: Create the DDP solver and run it ###############\n",
"# ##########################################################################"
]
},
{
......@@ -317,8 +275,7 @@
"metadata": {},
"outputs": [],
"source": [
"# HTML(anim.to_jshtml())\n",
"HTML(anim.to_html5_video())"
"HTML(animateCartpole(ddp.xs).to_jshtml())"
]
},
{
......@@ -345,9 +302,9 @@
"outputs": [],
"source": [
"# %load solutions/cartpole_tuning.py\n",
"###########################################################################\n",
"################## TODO: Tune the weights for each cost ###################\n",
"###########################################################################\n",
"# ##########################################################################\n",
"# ################# TODO: Tune the weights for each cost ###################\n",
"# ##########################################################################\n",
"terminalCartpole = DifferentialActionModelCartpole()\n",
"terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(terminalCartpole, True)\n",
"terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)\n",
......@@ -358,7 +315,7 @@
"terminalCartpole.costWeights[3] = 0 # fix me :)\n",
"terminalCartpole.costWeights[4] = 0 # fix me :)\n",
"terminalCartpole.costWeights[5] = 0 # fix me :)\n",
"problem = crocoddyl.ShootingProblem(x0, [ cartpoleIAM ]*T, terminalCartpoleIAM)"
"problem = crocoddyl.ShootingProblem(x0, [cartpoleIAM] * T, terminalCartpoleIAM)"
]
},
{
......@@ -369,24 +326,11 @@
"source": [
"# Creating the DDP solver\n",
"ddp = crocoddyl.SolverDDP(problem)\n",
"ddp.setCallbacks([ crocoddyl.CallbackVerbose() ])\n",
"ddp.setCallbacks([crocoddyl.CallbackVerbose()])\n",
"\n",
"# Solving this problem\n",
"done = ddp.solve([], [], 300)\n",
"print done"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"%%capture\n",
"%matplotlib inline\n",
"\n",
"# Create animation\n",
"anim = animateCartpole(xs)"
"print(done)"
]
},
{
......@@ -395,28 +339,27 @@
"metadata": {},
"outputs": [],
"source": [
"# HTML(anim.to_jshtml())\n",
"HTML(anim.to_html5_video())"
"HTML(animateCartpole(ddp.xs).to_jshtml())"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 2",
"display_name": "Python 3",
"language": "python",
"name": "python2"
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.12"
"pygments_lexer": "ipython3",
"version": "3.8.3"
}
},
"nbformat": 4,
......
......@@ -35,5 +35,4 @@ def animateCartpole(xs, sleep=50):
anim = animation.FuncAnimation(fig, animate, init_func=init, frames=len(xs), interval=sleep, blit=True)
print("... processing done")
plt.show()
return anim
##########################################################################
################## TODO: Create the DDP solver and run it ###############
###########################################################################
# #########################################################################
# ################# TODO: Create the DDP solver and run it ###############
# ##########################################################################
# Creating the DDP solver
ddp = crocoddyl.SolverDDP(problem)
ddp.setCallbacks([crocoddyl.CallbackDDPVerbose()])
ddp.setCallbacks([crocoddyl.CallbackVerbose()])
# Solving this problem
done = ddp.solve(1000)
print done
done = ddp.solve()
print(done)
# Use this function inside DifferentialActionModel.calc by setting:
# xddot, thddot = cartpole_dynamics(self, data, x, u)
def cartpole_dynamics(model, data, x, u):
# Getting the state and control variables
th, thdot = np.asscalar(x[1]), np.asscalar(x[3])
f = np.asscalar(u[0])
th, thdot = x[1].item(), x[3].item()
f = u[0].item()
# Shortname for system parameters
m1, m2, l, g = model.m1, model.m2, model.l, model.g
......
###########################################################################
################## TODO: Tune the weights for each cost ###################
###########################################################################
# ##########################################################################
# ################# TODO: Tune the weights for each cost ###################
# ##########################################################################
terminalCartpole = DifferentialActionModelCartpole()
terminalCartpoleDAM = DifferentialActionModelNumDiff(terminalCartpole, withGaussApprox=True)
terminalCartpoleIAM = IntegratedActionModelEuler(terminalCartpoleDAM)
terminalCartpoleDAM = crocoddyl.DifferentialActionModelNumDiff(terminalCartpole, True)
terminalCartpoleIAM = crocoddyl.IntegratedActionModelEuler(terminalCartpoleDAM)
terminalCartpole.costWeights[0] = 100
terminalCartpole.costWeights[1] = 100
......@@ -11,4 +11,4 @@ terminalCartpole.costWeights[2] = 1.
terminalCartpole.costWeights[3] = 0.1
terminalCartpole.costWeights[4] = 0.01
terminalCartpole.costWeights[5] = 0.0001
problem = ShootingProblem(x0, [cartpoleIAM] * T, terminalCartpoleIAM)
problem = crocoddyl.ShootingProblem(x0, [cartpoleIAM] * T, terminalCartpoleIAM)
......@@ -34,28 +34,30 @@
"outputs": [],
"source": [
"import numpy as np\n",
"import matplotlib.pylab as plt\n",
"\n",
"x = np.random.rand(3)\n",
"u = np.random.rand(2)\n",
"\n",
"# Unicycle dynamical model\n",
"v,w = u\n",
"c,s = np.cos(x[2]),np.sin(x[2])\n",
"v, w = u\n",
"c, s = np.cos(x[2]), np.sin(x[2])\n",
"dt = 1e-2\n",
"dx = np.array([ v*c, v*s, w ])\n",
"xnext = x + dx*dt\n",
"dx = np.array([v * c, v * s, w])\n",
"xnext = x + dx * dt\n",
"\n",
"# Cost function: driving to origin (state) and reducing speed (control)\n",
"stateWeight = 1\n",
"ctrlWeight = 1\n",
"costResiduals = np.concatenate([stateWeight*x,ctrlWeight*u])\n",
"cost = .5* sum(costResiduals**2)"
"costResiduals = np.concatenate([stateWeight * x, ctrlWeight * u])\n",
"cost = .5 * sum(costResiduals ** 2)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"For this basic example, the unicycle model is coded in the library. We will just load it and use it. If you are very curious, have a look! It is in crocoddyl/unicycle.py. "
"For this basic example, the unicycle model is coded in the library. We will just load it and use it. If you are very curious, have a look! It is in `crocoddyl/unicycle.py`. "
]
},
{
......@@ -73,7 +75,7 @@
"source": [
"import crocoddyl\n",
"model = crocoddyl.ActionModelUnicycle()\n",
"data = model.createData()"
"data = model.createData()"
]
},
{
......@@ -118,9 +120,9 @@
"metadata": {},
"outputs": [],
"source": [
"x0 = np.matrix([ -1., -1., 1. ]).T #x,y,theta\n",
"T = 20\n",
"problem = crocoddyl.ShootingProblem(x0, [ model ] * T, model)"
"x0 = np.matrix([-1., -1., 1.]).T # x, y, theta\n",
"T = 20\n",
"problem = crocoddyl.ShootingProblem(x0, [model] * T, model)"
]
},
{
......@@ -138,7 +140,7 @@
"metadata": {},
"outputs": [],
"source": [
"us = [ np.matrix([1., 1.]).T for t in range(T)]\n",
"us = [np.matrix([1., 1.]).T for _ in range(T)]\n",
"xs = problem.rollout(us)"
]
},
......@@ -156,10 +158,10 @@
"outputs": [],
"source": [
"%matplotlib inline\n",
"import matplotlib.pylab as plt\n",
"from unicycle_utils import plotUnicycle\n",
"for x in xs: plotUnicycle(x)\n",
"plt.axis([-2,2.,-2.,2.])"
"for x in xs: \n",
" plotUnicycle(x)\n",
"plt.axis([-2, 2., -2., 2.])"
]
},
{
......@@ -178,7 +180,7 @@
"source": [
"ddp = crocoddyl.SolverDDP(problem)\n",
"done = ddp.solve()\n",
"assert(done)"
"assert done"
]
},
{
......@@ -188,8 +190,9 @@
"outputs": [],
"source": [
"plt.clf()\n",
"for x in ddp.xs: plotUnicycle(x)\n",
"plt.axis([-2,2,-2,2])"
"for x in ddp.xs: \n",
" plotUnicycle(x)\n",
"plt.axis([-2, 2, -2, 2])"
]
},
{
......@@ -205,7 +208,7 @@
"metadata": {},
"outputs": [],
"source": [
"print ddp.xs[-1]"
"print(ddp.xs[-1])"
]
},
{
......@@ -224,21 +227,21 @@
],
"metadata": {
"kernelspec": {
"display_name": "Python 2",
"display_name": "Python 3",
"language": "python",
"name": "python2"
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.17"
"pygments_lexer": "ipython3",
"version": "3.8.3"
}
},
"nbformat": 4,
......
......@@ -14,7 +14,6 @@ def plotUnicycle(x):
def plotUnicycleSolution(xs, figIndex=1, show=True):
import matplotlib.pylab as plt
plt.figure(figIndex, figsize=(6.4, 6.4))
for x in xs:
plotUnicycle(x)
......
This diff is collapsed.
......@@ -16,6 +16,8 @@
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/core/activation-base.hpp"
#include <pinocchio/utils/static-if.hpp>
namespace crocoddyl {
template <typename _Scalar>
......@@ -34,12 +36,13 @@ struct ActivationBoundsTpl {
<< "The lower and upper bounds don't have the same dimension (lb,ub dimensions equal to " +
std::to_string(lb.size()) + "," + std::to_string(ub.size()) + ", respectively)");
}
if (beta < 0 || beta > 1.) {
if (beta < Scalar(0) || beta > Scalar(1.)) {
throw_pretty("Invalid argument: "
<< "The range of beta is between 0 and 1");