unittest/python/crocoddyl/action.py
def _assertStableStateFD(model, x):
        """ Make sure that when we finite difference the Action Model, the user does
        not face unknown behaviour because of the finite differencing of a quaternion around pi.
        This behaviour might occur if CostModelState and FloatingInContact differential model are used
        together.

        For full discussions see issue https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
        """
        if hasattr(model.model0, "differential"):
            md = model.model0.differential
            if isinstance(md, DifferentialActionModelFloatingInContact):
                if hasattr(md, "costs"):
                    mc = md.costs
                    if isinstance(mc, CostModelState):
                        assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
                        assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
                    elif isinstance(mc, CostModelSum):
                        for (key, cost) in mc.costs.items():
                            if isinstance(cost.cost, CostModelState):
                                assert (~np.isclose(
                                    model.State.diff(cost.cost.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
                                assert (~np.isclose(
                                    model.State.diff(cost.cost.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
 Make sure that when we finite difference the Action Model, the user does
        not face unknown behaviour because of the finite differencing of a quaternion around pi.
        This behaviour might occur if CostModelState and FloatingInContact differential model are used
        together.

        For full discussions see issue https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
        
Make sure that when we finite difference the Action Model, the user does
not face unknown behaviour because of the finite differencing of a quaternion around pi.
This behaviour might occur if CostModelState and FloatingInContact differential model are used
together.


Make sure that when we finite difference the Action Model the user does not face unknown behaviour because of the finite differencing of a quaternion around pi . This behaviour might occur if CostModelState and FloatingInContact differential model are used together .
def _assertStableStateFD(model, x):
        if hasattr(model.model0, "differential"):
            md = model.model0.differential
            if isinstance(md, DifferentialActionModelFloatingInContact):
                if hasattr(md, "costs"):
                    mc = md.costs
                    if isinstance(mc, CostModelState):
                        assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
                        assert (~np.isclose(model.State.diff(mc.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
                    elif isinstance(mc, CostModelSum):
                        for (key, cost) in mc.costs.items():
                            if isinstance(cost.cost, CostModelState):
                                assert (~np.isclose(
                                    model.State.diff(cost.cost.ref, x)[3:6], np.ones(3) * np.pi, atol=1e-6).any())
                                assert (~np.isclose(
                                    model.State.diff(cost.cost.ref, x)[3:6], -np.ones(3) * np.pi, atol=1e-6).any())
def _assertStableStateFD ( model , x ) : if hasattr ( model . model0 , "differential" ) : md = model . model0 . differential if isinstance ( md , DifferentialActionModelFloatingInContact ) : if hasattr ( md , "costs" ) : mc = md . costs if isinstance ( mc , CostModelState ) : assert ( ~ np . isclose ( model . State . diff ( mc . ref , x ) [ 3 : 6 ] , np . ones ( 3 ) * np . pi , atol = 1e-6 ) . any ( ) ) assert ( ~ np . isclose ( model . State . diff ( mc . ref , x ) [ 3 : 6 ] , - np . ones ( 3 ) * np . pi , atol = 1e-6 ) . any ( ) ) elif isinstance ( mc , CostModelSum ) : for ( key , cost ) in mc . costs . items ( ) : if isinstance ( cost . cost , CostModelState ) : assert ( ~ np . isclose ( model . State . diff ( cost . cost . ref , x ) [ 3 : 6 ] , np . ones ( 3 ) * np . pi , atol = 1e-6 ) . any ( ) ) assert ( ~ np . isclose ( model . State . diff ( cost . cost . ref , x ) [ 3 : 6 ] , - np . ones ( 3 ) * np . pi , atol = 1e-6 ) . any ( ) )
Make sure that when we finite difference the Action Model, the user does
not face unknown behaviour because of the finite differencing of a quaternion around pi.
Make sure that when we finite difference the Action Model the user does not face unknown behaviour because of the finite differencing of a quaternion around pi .
""" Make sure that when we finite difference the Action Model, the user does
        not face unknown behaviour because of the finite differencing of a quaternion around pi.
        This behaviour might occur if CostModelState and FloatingInContact differential model are used
        together.

        For full discussions see issue https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
        """
model
x