Commit 43080940 authored by Côme Perrot's avatar Côme Perrot
Browse files

Added python unittest

parent 2f24fad0
Pipeline #20902 failed with stage
in 30 minutes and 32 seconds
......@@ -60,7 +60,7 @@ void ResidualModelAnticipatedStateTpl<Scalar>::calcDiff(
<< "x has wrong dimension (it should be " +
std::to_string(state_->get_nx()) + ")");
}
// Data* d = static_cast<Data*>(data.get());
Data* d = static_cast<Data*>(data.get());
const std::size_t nv = state_->get_nv();
MatrixXs Jx = MatrixXs::Zero(state_->get_ndx(), state_->get_ndx());
......
......@@ -77,7 +77,8 @@ void exposeResidualAnticipatedState() {
boost::shared_ptr<ResidualDataAnticipatedState> >();
bp::class_<ResidualDataAnticipatedState, bp::bases<ResidualDataAbstract> >(
"ResidualDataAnticipatedState", "Data for anticipated state residual.\n\n",
"ResidualDataAnticipatedState",
"Data for anticipated state residual.\n\n",
bp::init<ResidualModelAnticipatedState*, DataCollectorAbstract*>(
bp::args("self", "model", "data"),
"Create anticipated state residual data.\n\n"
......
......@@ -10,6 +10,7 @@ BOOST_PYTHON_MODULE(sobec_pywrap) {
eigenpy::enableEigenPy();
ENABLE_SPECIFIC_MATRIX_TYPE(Eigen::VectorXi);
sobec::python::exposeStdContainers();
sobec::python::exposeResidualAnticipatedState();
sobec::python::exposeResidualVelCollision();
sobec::python::exposeResidualCoMVelocity();
sobec::python::exposeResidualCenterOfPressure();
......
......@@ -120,9 +120,9 @@ boost::shared_ptr<crocoddyl::CostModelAbstract> CostModelFactory::create(
// break;
case CostModelTypes::CostModelAnticipatedState:
cost = boost::make_shared<crocoddyl::CostModelResidual>(
state, activation_factory.create(activation_type, state->get_ndx()),
state, activation_factory.create(activation_type, state->get_nv()),
boost::make_shared<sobec::ResidualModelAnticipatedState>(
state, nu, Eigen::VectorXd::Random(1)[0]));
state, state->get_nv(), Eigen::VectorXd::Random(1)[0]));
break;
case CostModelTypes::CostModelResidualControl:
cost = boost::make_shared<crocoddyl::CostModelResidual>(
......
#ADD_PYTHON_UNIT_TEST("py-walk-with-traj" "tests/python/test_walk_with_traj.py" "python")
ADD_PYTHON_UNIT_TEST("py-vel-collision" "tests/python/test_vel_collision.py" "python")
ADD_PYTHON_UNIT_TEST("py-cop" "tests/python/test_cop.py" "python")
ADD_PYTHON_UNIT_TEST("py-anticipated-state" "tests/python/test_anticipated_state.py" "python")
ADD_PYTHON_UNIT_TEST("py-fly-high" "tests/python/test_fly_high.py" "python")
ADD_PYTHON_UNIT_TEST("py-test-collision" "tests/python/test_feet_collision.py" "python")
ADD_PYTHON_UNIT_TEST("py-test-mpc-walk" "tests/python/test_mpc_walk.py" "python")
......
"""
Simple numdiff test of the COP cost.
COP is a complex function as it depends on the contact forces.
Hence, build a complete DAM with contact, and assert its gradient WRT numdiff.
"""
from turtle import shape
import unittest
import pinocchio as pin
import crocoddyl as croc
import numpy as np
import example_robot_data as robex
from numpy.linalg import norm
# Local imports
import sobec
class TestAnticipatedState(unittest.TestCase):
def test_anticipated_state(self):
np.random.seed(0)
dt = np.random.random()
robot = robex.load("talos_arm")
# The pinocchio model is what we are really interested by.
model = robot.model
model.q0 = robot.q0
# Initial config, also used for warm start
x0 = np.concatenate([model.q0, np.random.random(model.nv) * 20 - 10])
# #############################################################################
state = croc.StateMultibody(model)
actuation = croc.ActuationModelFull(state)
# Costs
costs = croc.CostModelSum(state, actuation.nu)
anticipatedStateResidual = sobec.ResidualModelAnticipatedState(state, state.nq, dt)
anticipatedStateCost = croc.CostModelResidual(state, anticipatedStateResidual)
costs.addCost("anticipatedState", anticipatedStateCost, 1)
# Action
damodel = croc.DifferentialActionModelFreeFwdDynamics(state, actuation, costs)
# #############################################################################
# jid = model.frames[cid].parent
# ### For easier manipulation at debug time
dadata = damodel.createData()
u0 = np.random.rand(actuation.nu) * 20 - 10
q0 = x0[: state.nq]
v0 = x0[state.nq :]
try:
cosname = "anticipatedState"
cosdata = dadata.costs.costs[cosname]
# cosmodel = damodel.costs.costs[cosname].cost
data = cosdata.shared.pinocchio
except KeyError:
pass
damodel.calc(dadata, x0, u0)
damodel.calcDiff(dadata, x0, u0)
# ## MANUAL CHECK
np.set_printoptions(precision=3, linewidth=300, suppress=True, threshold=10000)
self.assertGreater(norm(cosdata.residual.r), 0)
r = q0 + dt * v0
self.assertLess(norm(cosdata.residual.r - r), 1e-6)
Rx = np.concatenate((np.diag(np.ones(state.nq)), dt * np.diag(np.ones(state.nq))), axis=1)
self.assertLess(norm(cosdata.residual.Rx - Rx), 1e-6)
# ### NUMDIFF TEST
damnd = croc.DifferentialActionModelNumDiff(damodel, gaussApprox=True)
dadnd = damnd.createData()
damnd.calc(dadnd, x0, u0)
damnd.calcDiff(dadnd, x0, u0)
self.assertLess(norm(dadnd.Lx - dadata.Lx) / norm(dadata.Lx), 1e-5)
self.assertLess(norm(dadnd.Lu - dadata.Lu) / norm(dadata.Lx), 1e-5)
if __name__ == "__main__":
unittest.main()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment