diff --git a/.pylintrc b/.pylintrc index a8239b7ec..1a3ffd901 100644 --- a/.pylintrc +++ b/.pylintrc @@ -71,6 +71,7 @@ disable = useless-parent-delegation, use-dict-literal, unspecified-encoding, + undefined-loop-variable, cyclic-import # Enable the message, report, category or checker with the given id(s) diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h index 4148a8e78..62488012d 100644 --- a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h +++ b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h @@ -46,9 +46,9 @@ namespace jiminy::pinocchio_overload pinocchio::DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, - bool update_kinematics = true) + bool updateKinematics = true) { - if (update_kinematics) + if (updateKinematics) { pinocchio::forwardKinematics(model, data, q, v); } diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index 9dcd311c7..c7d5adb59 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -793,6 +793,7 @@ namespace jiminy void computeExtraTerms( std::shared_ptr & robot, const RobotData & robotData, ForceVector & fExt) { + // Define some proxies for convenience const pinocchio::Model & model = robot->pinocchioModel_; pinocchio::Data & data = robot->pinocchioData_; @@ -801,7 +802,7 @@ namespace jiminy model, data, robotData.state.q, robotData.state.v, false); pinocchio::computePotentialEnergy(model, data); - /* Update manually the subtree (apparent) inertia, since it is only computed by crba, which + /* Update manually the subtree (apparent) inertia, since it is only computed by CRBA, which is doing more computation than necessary. It will be used here for computing the centroidal kinematics, and used later for joint bounds dynamics. Note that, by doing all the computations here instead of 'computeForwardKinematics', we are doing the assumption @@ -823,13 +824,25 @@ namespace jiminy } } - /* Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint - accelerations, joint forces and body forces, so it must be done separately: - - 1st step: computing the accelerations based on ForwardKinematic algorithm - - 2nd step: computing the forces based on RNEA algorithm */ + /* The objective here is to compute the actual joint accelerations and the joint internal + forces. The latter are not involved in dynamic computations, but are useful for analysis + of the mechanical design. Indeed, it brings information about stresses and strains + applied to the mechanical structure, which may cause unexpected fatigue wear. In + addition, the body external forces are also evaluated, as an intermediate quantity for + computing the centroidal dynamics, ie the spatial momentum of the whole robot at the + Center of Mass along with its temporal derivative. + + Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint + accelerations, the joint internal forces and the body external forces. Hence, it is done + manually, following a two computation steps procedure: + * joint accelerations based on ForwardKinematic algorithm + * joint internal forces and body external forces based on RNEA algorithm */ /* Compute the true joint acceleration and the one due to the lone gravity field, then - the spatial momenta and the total internal and external forces acting on each body. */ + the spatial momenta and the total internal and external forces acting on each body. + * `fExt` is used as a buffer for storing the total body external forces. It serves + no purpose other than being an intermediate quantity for other computations. + * `data.f` stores the joint internal forces */ data.h[0].setZero(); fExt[0].setZero(); data.f[0].setZero(); diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 3ac8081c2..2c03270f9 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -592,7 +592,7 @@ namespace jiminy std::string backlashName = jointName; backlashName += BACKLASH_JOINT_SUFFIX; - // Check if constraint a joint bounds constraint exist + // Check if the corresponding joint bound constraint already exists const bool hasConstraint = constraints_.exist(backlashName, ConstraintRegistryType::BOUNDS_JOINTS); @@ -605,10 +605,13 @@ namespace jiminy { removeConstraint(backlashName, ConstraintRegistryType::BOUNDS_JOINTS); } - continue; } + // Add backlash joint to the model + addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName); + backlashJointNames_.push_back(backlashName); + // Add joint bounds constraint if necessary if (!hasConstraint) { @@ -617,10 +620,6 @@ namespace jiminy addConstraint(backlashName, constraint, ConstraintRegistryType::BOUNDS_JOINTS); } - // Add backlash joint to the model - addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName); - backlashJointNames_.push_back(backlashName); - // Update position limits in model const Eigen::Index positionIndex = getJointPositionFirstIndex(pinocchioModel_, backlashName); diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py b/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py index f2b525787..37876ae65 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py @@ -296,6 +296,14 @@ class and all their constructor keyword-arguments name: cls(env, self, **kwargs) for name, (cls, kwargs) in requirements.items()} + # Update the state explicitly if available but auto-refresh not enabled + self._state: Optional[StateQuantity] = None + if isinstance(self, AbstractQuantity): + quantity = self.requirements["state"] + if not quantity.auto_refresh: + assert isinstance(quantity, StateQuantity) + self._state = quantity + # Shared cache handling self._cache: Optional[SharedCache[ValueT]] = None self.has_cache = False @@ -423,10 +431,17 @@ def get(self) -> ValueT: # Evaluate quantity try: + # Initialize quantity if not self._is_initialized: self.initialize() assert (self._is_initialized and self._is_active) # type: ignore[unreachable] + + # Make sure that the state has been refreshed + if self._state is not None: + self._state.get() + + # Refresh quantity value = self.refresh() except RecursionError as e: raise LookupError( @@ -611,13 +626,23 @@ class and all their constructor keyword-arguments # Make sure that no user-specified requirement is named 'trajectory' requirement_names = requirements.keys() - if any(name in requirement_names for name in ("state", "trajectory")): + if "trajectory" in requirement_names: raise ValueError( - "No user-specified requirement can be named 'state' nor " - "'trajectory' as these keys are reserved.") - - # Add state quantity as requirement - requirements["state"] = (StateQuantity, dict(mode=mode)) + "Key 'trajectory' is reserved and cannot be used for " + "user-specified requirements.") + + # Make sure that state requirement is valid if any or use default + quantity = requirements.get("state") + if quantity is not None: + cls, kwargs = quantity + if (not issubclass(cls, StateQuantity) or + kwargs.setdefault("mode", mode) != mode): + raise ValueError( + "Key 'state' is reserved and can only be used to specify " + "a `StateQuantity` requirement, as a way to give the " + "opportunity to overwrite 'update_*' default arguments.") + else: + requirements["state"] = (StateQuantity, dict(mode=mode)) # Call base implementation super().__init__(env, parent, requirements, auto_refresh=auto_refresh) @@ -905,7 +930,12 @@ def __init__(self, env: InterfaceJiminyEnv, parent: Optional[InterfaceQuantity], *, - mode: QuantityEvalMode = QuantityEvalMode.TRUE) -> None: + mode: QuantityEvalMode = QuantityEvalMode.TRUE, + update_kinematics: bool = True, + update_dynamics: bool = False, + update_centroidal: bool = False, + update_energy: bool = False, + update_jacobian: bool = False) -> None: """ :param env: Base or wrapped jiminy environment. :param parent: Higher-level quantity from which this quantity is a @@ -917,12 +947,66 @@ def __init__(self, some reference trajectory at the current simulation time will be used instead. Optional: 'QuantityEvalMode.TRUE' by default. + :param update_kinematics: Whether to update body and frame transforms, + spatial velocities and accelerations stored + in `self.pinocchio_data` if necessary to be + consistent with the current state of the + robot. This argument has no effect if mode is + set to `QuantityEvalMode.TRUE` because this + property is already guarantee. + Optional: False by default. + :param update_dynamics: Whether to update the non-linear effects and + the joint internal forces stored in + `self.pinocchio_data` if necessary. + Optional: False by default. + :param update_centroidal: Whether to update the centroidal dynamics + (incl. CoM) stored in `self.pinocchio_data` + if necessary. + Optional: True by default. + :param update_energy: Whether to update the potential and kinematic + energy stored in `self.pinocchio_data` if + necessary. + Optional: False by default. + :param update_jacobian: Whether to update the joint Jacobian matrices + stored in `self.pinocchio_data` if necessary. + Optional: False by default. """ + # Make sure that the input arguments are valid + update_kinematics = ( + update_kinematics or update_dynamics or update_centroidal or + update_energy or update_jacobian) + # Backup user argument(s) self.mode = mode + self.update_kinematics = update_kinematics + self.update_dynamics = update_dynamics + self.update_centroidal = update_centroidal + self.update_energy = update_energy + self.update_jacobian = update_jacobian + + # Enable auto-refresh based on the evaluation mode + # Note that it is necessary to auto-refresh this quantity, as it is the + # one responsible for making sure that dynamics quantities are always + # up-to-date when refreshing quantities. The latter are involved one + # way of the other in the computation of any quantity, which means that + # pre-computing it does not induce any unnecessary computations as long + # as the user fetches the value of at least one quantity. Although this + # assumption is very likely to be true at the step update period, it is + # not the case at the observer update period. It sounds more efficient + # refresh to the state the first time any quantity gets computed. + # However, systematically checking if the state must be refreshed for + # all quantities adds overheat and may be fairly costly overall. The + # optimal trade-off is to rely on auto-refresh if the evaluation mode + # is TRUE, since refreshing the state only consists in copying some + # data, which is very cheap. On the contrary, it is more efficient to + # only refresh the state when needed if the evaluation mode is TRAJ. + # * Update state: 500ns (TRUE) | 5.0us (TRAJ) + # * Check cache state: 70ns + auto_refresh = mode == QuantityEvalMode.TRUE - # Call base implementation - super().__init__(env, parent, requirements={}, auto_refresh=True) + # Call base implementation. + super().__init__( + env, parent, requirements={}, auto_refresh=auto_refresh) # Create empty trajectory database, manually added as a requirement. # Note that it must be done after base initialization, otherwise a @@ -936,12 +1020,16 @@ def __init__(self, self.pinocchio_data = env.robot.pinocchio_data # State for which the quantity must be evaluated - self._f_external_slices: List[np.ndarray] = [] - self._f_external_list: List[np.ndarray] = [] self.state = State(t=np.nan, q=np.array([])) + # Persistent buffer for storing body external forces if necessary + self._f_external_vec = pin.StdVec_Force() + self._f_external_list: List[np.ndarray] = [] + self._f_external_batch = np.array([]) + self._f_external_slices: List[np.ndarray] = [] + # Persistent buffer storing all lambda multipliers for efficiency - self._constraint_lambda_all = np.array({}) + self._constraint_lambda_batch = np.array([]) # Slices in stacked lambda multiplier flat vector self._constraint_lambda_slices: List[np.ndarray] = [] @@ -949,7 +1037,30 @@ def __init__(self, # Lambda multipliers of all the constraints individually self._constraint_lambda_list: List[np.ndarray] = [] + # Whether to update kinematic and dynamic data to be consistent with + # the current state of the robot, based on the requirement of all the + # co-owners of shared cache. + self._update_kinematics = False + self._update_dynamics = False + self._update_centroidal = False + self._update_energy = False + self._update_jacobian = False + def initialize(self) -> None: + # Determine which data must be update based on shared cache co-owners + owners = self.cache.owners if self.has_cache else (self,) + self._update_kinematics = False + self._update_dynamics = False + self._update_centroidal = False + self._update_energy = False + self._update_jacobian = False + for owner in owners: + self._update_kinematics |= owner.update_kinematics + self._update_dynamics |= owner.update_dynamics + self._update_centroidal |= owner.update_centroidal + self._update_energy |= owner.update_energy + self._update_jacobian |= owner.update_jacobian + # Refresh robot and pinocchio proxies for co-owners of shared cache. # Note that automatic refresh is not sufficient to guarantee that # `initialize` will be called unconditionally, because it will be @@ -959,7 +1070,6 @@ def initialize(self) -> None: # instance to found the cache empty. Only the necessary bits are # synchronized instead of the whole method, to avoid messing up with # computation graph tracking. - owners = self.cache.owners if self.has_cache else (self,) for owner in owners: assert isinstance(owner, StateQuantity) if owner._is_initialized: @@ -981,18 +1091,53 @@ def initialize(self) -> None: # The quantity will be considered initialized and active at this point. super().initialize() - # Define the state for which the quantity must be evaluated + # Refresh proxies and allocate memory for storing external forces + if self.mode == QuantityEvalMode.TRUE: + self._f_external_vec = self.env.robot_state.f_external + else: + self._f_external_vec = pin.StdVec_Force() + self._f_external_vec.extend([ + pin.Force() for _ in range(self.pinocchio_model.njoints)]) + self._f_external_list = [ + f_ext.vector for f_ext in self._f_external_vec] + self._f_external_batch = np.zeros((self.pinocchio_model.njoints, 6)) + self._f_external_slices = list(self._f_external_batch) + + # Allocate memory for lambda vector + self._constraint_lambda_batch = np.zeros( + (len(self.robot.log_constraint_fieldnames),)) + + # Refresh mapping from lambda multipliers to corresponding slice + self._constraint_lambda_list.clear() + self._constraint_lambda_slices.clear() + constraint_lookup_pairs = tuple( + (f"Constraint{registry_type}", registry) + for registry_type, registry in ( + ("BoundJoints", self.robot.constraints.bounds_joints), + ("ContactFrames", self.robot.constraints.contact_frames), + ("CollisionBodies", { + name: constraint for constraints in ( + self.robot.constraints.collision_bodies) + for name, constraint in constraints.items()}), + ("User", self.robot.constraints.user))) + i = 0 + while i < len(self.robot.log_constraint_fieldnames): + fieldname = self.robot.log_constraint_fieldnames[i] + for registry_type, registry in constraint_lookup_pairs: + if fieldname.startswith(registry_type): + break + constraint_name = fieldname[len(registry_type):-1] + constraint = registry[constraint_name] + self._constraint_lambda_list.append(constraint.lambda_c) + self._constraint_lambda_slices.append( + self._constraint_lambda_batch[i:(i + constraint.size)]) + i += constraint.size + + # Allocate state for which the quantity must be evaluated if needed if self.mode == QuantityEvalMode.TRUE: - # Refresh mapping from robot state to quantity buffer if not self.env.is_simulation_running: raise RuntimeError("No simulation running. Impossible to " "initialize this quantity.") - self._f_external_list = [ - f_ext.vector for f_ext in self.env.robot_state.f_external] - if self._f_external_list: - f_external_batch = np.stack(self._f_external_list, axis=0) - else: - f_external_batch = np.array([]) self.state = State( 0.0, self.env.robot_state.q, @@ -1000,38 +1145,7 @@ def initialize(self) -> None: self.env.robot_state.a, self.env.robot_state.u, self.env.robot_state.command, - f_external_batch) - self._f_external_slices = list(f_external_batch) - else: - # Allocate memory for lambda vector - self._constraint_lambda_all = np.zeros( - (len(self.robot.log_constraint_fieldnames),)) - - # Refresh mapping from lambda multipliers to corresponding slice - self._constraint_lambda_list.clear() - self._constraint_lambda_slices.clear() - constraint_lookup_pairs = tuple( - (f"Constraint{registry_type}", registry) - for registry_type, registry in ( - ("BoundJoints", self.robot.constraints.bounds_joints), - ("ContactFrames", self.robot.constraints.contact_frames), - ("CollisionBodies", { - name: constraint for constraints in ( - self.robot.constraints.collision_bodies) - for name, constraint in constraints.items()}), - ("User", self.robot.constraints.user))) - i = 0 - while i < len(self.robot.log_constraint_fieldnames): - fieldname = self.robot.log_constraint_fieldnames[i] - for registry_type, registry in constraint_lookup_pairs: - if fieldname.startswith(registry_type): - break - constraint_name = fieldname[len(registry_type):-1] - constraint = registry[constraint_name] - self._constraint_lambda_list.append(constraint.lambda_c) - self._constraint_lambda_slices.append( - self._constraint_lambda_all[i:(i + constraint.size)]) - i += constraint.size + self._f_external_batch) def refresh(self) -> State: """Compute the current state depending on the mode of evaluation, and @@ -1039,29 +1153,47 @@ def refresh(self) -> State: """ # Update state at which the quantity must be evaluated if self.mode == QuantityEvalMode.TRUE: + # Update the current simulation time self.state.t = self.env.stepper_state.t + + # Update external forces and constraint multipliers in state buffer multi_array_copyto(self._f_external_slices, self._f_external_list) + multi_array_copyto( + self._constraint_lambda_slices, self._constraint_lambda_list) else: self.state = self.trajectory.get() if self.mode == QuantityEvalMode.REFERENCE: + # Copy body external forces from stacked buffer to force vector + has_forces = self.state.f_external is not None + if has_forces: + array_copyto(self._f_external_batch, self.state.f_external) + multi_array_copyto(self._f_external_list, + self._f_external_slices) + # Update all dynamical quantities that can be given available data - update_quantities( - self.robot, - self.state.q, - self.state.v, - self.state.a, - update_physics=True, - update_centroidal=True, - update_energy=True, - update_jacobian=False, - update_collisions=True, - use_theoretical_model=self.trajectory.use_theoretical_model) - - # Restore lagrangian multipliers of the constraints - array_copyto(self._constraint_lambda_all, self.state.lambda_c) - multi_array_copyto( - self._constraint_lambda_list, self._constraint_lambda_slices) + if self.update_kinematics: + update_quantities( + self.robot, + self.state.q, + self.state.v, + self.state.a, + self._f_external_vec if has_forces else None, + update_dynamics=self._update_dynamics, + update_centroidal=self._update_centroidal, + update_energy=self._update_energy, + update_jacobian=self._update_jacobian, + update_collisions=False, + use_theoretical_model=( + self.trajectory.use_theoretical_model)) + + # Restore lagrangian multipliers of the constraints if available + has_constraints = self.state.lambda_c is not None + if has_constraints: + array_copyto( + self._constraint_lambda_batch, self.state.lambda_c) + multi_array_copyto(self._constraint_lambda_list, + self._constraint_lambda_slices) # Return state return self.state diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py index 03bf129b7..7e3db84a7 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py @@ -904,7 +904,7 @@ def step(self, # type: ignore[override] if terminated or truncated: self._num_steps_beyond_terminate = 0 else: - if not self.is_training and self._num_steps_beyond_terminate == 0: + if self.is_training and self._num_steps_beyond_terminate == 0: LOGGER.error( "Calling `step` after termination causes the reward to be " "'nan' systematically and is strongly discouraged in " diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py index d22168ed0..416a5836c 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py @@ -13,7 +13,7 @@ MultiFramesPosition, MultiFramesXYZQuat, MultiFramesMeanXYZQuat, - AverageFramePose, + AverageFrameXYZQuat, AverageFrameRollPitch, AverageFrameSpatialVelocity, ActuatedJointsPosition) @@ -24,10 +24,10 @@ AverageBaseMomentum, MultiFootMeanXYZQuat, MultiFootRelativeXYZQuat, + MultiContactRelativeForceTangential, CenterOfMass, CapturePoint, ZeroMomentPoint, - MultiContactRelativeForceTangential, translate_position_odom) @@ -48,7 +48,8 @@ 'MultiFootMeanXYZQuat', 'MultiFootRelativeXYZQuat', 'MultiFootMeanOdometryPose', - 'AverageFramePose', + 'MultiContactRelativeForceTangential', + 'AverageFrameXYZQuat', 'AverageFrameRollPitch', 'AverageFrameSpatialVelocity', 'AverageBaseSpatialVelocity', @@ -59,6 +60,5 @@ 'CenterOfMass', 'CapturePoint', 'ZeroMomentPoint', - 'MultiContactRelativeForceTangential', 'translate_position_odom' ] diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py index 73e94c2d9..6970034b9 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py @@ -1126,8 +1126,9 @@ def refresh(self) -> np.ndarray: @dataclass(unsafe_hash=True) -class AverageFramePose(InterfaceQuantity[np.ndarray]): - """Average pose of a given frame over the whole agent step. +class AverageFrameXYZQuat(InterfaceQuantity[np.ndarray]): + """Spatial vector representation (X, Y, Z, QuatX, QuatY, QuatZ, QuatW) of + the average pose of a given frame over the whole agent step. The average frame pose is obtained by integration of the average velocity over the whole agent step, backward in time from the state at the end of @@ -1203,7 +1204,7 @@ class AverageFrameRollPitch(InterfaceQuantity[np.ndarray]): Roll-Pitch_yaw decomposition of a given frame over the whole agent step. .. seealso:: - See `remove_yaw_from_quat` and `AverageFramePose` for details about + See `remove_yaw_from_quat` and `AverageFrameXYZQuat` for details about the Roll-Pitch-Yaw decomposition and how the average frame pose is defined respectively. """ @@ -1245,7 +1246,7 @@ def __init__(self, parent, requirements=dict( quat_mean=(MaskedQuantity, dict( - quantity=(AverageFramePose, dict( + quantity=(AverageFrameXYZQuat, dict( frame_name=frame_name, mode=mode)), axis=0, @@ -1340,7 +1341,7 @@ def __init__(self, frame_name=frame_name, mode=mode)), quat_mean=(MaskedQuantity, dict( - quantity=(AverageFramePose, dict( + quantity=(AverageFrameXYZQuat, dict( frame_name=frame_name, mode=mode)), axis=0, diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py index 493122016..0ba1d3b20 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py @@ -14,7 +14,8 @@ import pinocchio as pin from ..bases import ( - InterfaceJiminyEnv, InterfaceQuantity, AbstractQuantity, QuantityEvalMode) + InterfaceJiminyEnv, InterfaceQuantity, AbstractQuantity, StateQuantity, + QuantityEvalMode) from ..utils import ( matrix_to_yaw, quat_to_yaw, quat_to_matrix, quat_multiply, quat_apply) @@ -660,7 +661,13 @@ def __init__( # Call base implementation super().__init__( - env, parent, requirements={}, mode=mode, auto_refresh=False) + env, + parent, + requirements=dict( + state=(StateQuantity, dict( + update_centroidal=True))), + mode=mode, + auto_refresh=False) # Pre-allocate memory for the CoM quantity self._com_data: np.ndarray = np.array([]) @@ -747,8 +754,7 @@ def __init__(self, com_position=(CenterOfMass, dict( kinematic_level=pin.POSITION, mode=mode)), - odom_pose=(BaseOdometryPose, dict(mode=mode)) - ), + odom_pose=(BaseOdometryPose, dict(mode=mode))), mode=mode, auto_refresh=False) @@ -855,8 +861,7 @@ def __init__(self, com_velocity=(CenterOfMass, dict( kinematic_level=pin.VELOCITY, mode=mode)), - odom_pose=(BaseOdometryPose, dict(mode=mode)) - ), + odom_pose=(BaseOdometryPose, dict(mode=mode))), mode=mode, auto_refresh=False) @@ -881,7 +886,7 @@ def initialize(self) -> None: update_quantities( self.robot, pin.neutral(self.robot.pinocchio_model_th), - update_physics=True, + update_dynamics=False, update_centroidal=True, update_energy=False, update_jacobian=False, @@ -933,10 +938,7 @@ def __init__(self, """ # Call base implementation super().__init__( - env, - parent, - requirements={}, - auto_refresh=False) + env, parent, requirements={}, auto_refresh=False) # Weight of the robot self._robot_weight: float = -1 diff --git a/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py b/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py index 6499d0c4b..594c0a113 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py +++ b/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py @@ -198,7 +198,7 @@ def _setup(self) -> None: if (step_ratio // frame_ratio) * frame_ratio != step_ratio: LOGGER.warning( "Beware `step_dt // observe_dt` is not a multiple of " - "`skip_frame_ratio + 1`.") + "`skip_frames_ratio + 1`.") # Copy observe and control update periods from wrapped environment self.observe_dt = self.env.observe_dt diff --git a/python/gym_jiminy/unit_py/data/anymal_pipeline.json b/python/gym_jiminy/unit_py/data/anymal_pipeline.json index 6ca97568d..641c5940f 100644 --- a/python/gym_jiminy/unit_py/data/anymal_pipeline.json +++ b/python/gym_jiminy/unit_py/data/anymal_pipeline.json @@ -58,8 +58,8 @@ ["measurements", "ImuSensor"], ["actions"] ], - "num_stack": 3, - "skip_frames_ratio": 2 + "num_stack": 4, + "skip_frames_ratio": 3 } } } diff --git a/python/gym_jiminy/unit_py/data/anymal_pipeline.toml b/python/gym_jiminy/unit_py/data/anymal_pipeline.toml index 111118f55..053d2beef 100644 --- a/python/gym_jiminy/unit_py/data/anymal_pipeline.toml +++ b/python/gym_jiminy/unit_py/data/anymal_pipeline.toml @@ -54,5 +54,5 @@ nested_filter_keys = [ ["measurements", "ImuSensor"], ["actions"], ] -num_stack = 3 -skip_frames_ratio = 2 +num_stack = 4 +skip_frames_ratio = 3 diff --git a/python/gym_jiminy/unit_py/test_misc.py b/python/gym_jiminy/unit_py/test_misc.py index 8baac24de..14dd440de 100644 --- a/python/gym_jiminy/unit_py/test_misc.py +++ b/python/gym_jiminy/unit_py/test_misc.py @@ -29,7 +29,8 @@ def test_play_log_data(self): """ TODO: Write documentation. """ # Instantiate the environment - env = gym.make("gym_jiminy.envs:atlas", debug=True) + env = gym.make("gym_jiminy.envs:atlas", debug=False) + env.eval() # Run a few steps env.reset(seed=0) @@ -50,7 +51,8 @@ def test_play_log_data(self): backend="panda3d-sync", record_video_path=video_path, display_contacts=True, - display_f_external=True) + display_f_external=True, + verbose=False) Viewer.close() # Check the external forces has been updated diff --git a/python/gym_jiminy/unit_py/test_pipeline_design.py b/python/gym_jiminy/unit_py/test_pipeline_design.py index 973a18112..4ea10cc24 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -6,7 +6,6 @@ import unittest import numpy as np -import gymnasium as gym from jiminy_py.robot import _gcd from jiminy_py.log import extract_trajectory_from_log @@ -16,87 +15,69 @@ from gym_jiminy.common.blocks import PDController +STEP_DT = 0.04 +PID_KP = np.full((12,), fill_value=1500) +PID_KD = np.full((12,), fill_value=0.01) + TOLERANCE = 1.0e-6 -class PipelineDesign(unittest.TestCase): +def _create_pipeline(num_stack: int, + skip_frames_ratio: int) -> InterfaceJiminyEnv: """ TODO: Write documentation """ - def setUp(self): - """ TODO: Write documentation - """ - self.step_dt = 0.04 - self.pid_kp = np.full((12,), fill_value=1500) - self.pid_kd = np.full((12,), fill_value=0.01) - self.num_stack = 3 - self.skip_frames_ratio = 2 - - self.ANYmalPipelineEnv = build_pipeline( - env_config=dict( - cls='gym_jiminy.envs.ANYmalJiminyEnv', - kwargs=dict( - step_dt=self.step_dt, - debug=False - ) - ), - layers_config=[ - dict( - block=dict( - cls='gym_jiminy.common.blocks.PDController', - kwargs=dict( - update_ratio=2, - kp=self.pid_kp, - kd=self.pid_kd, - joint_position_margin=0.0, - joint_velocity_limit=float("inf"), - joint_acceleration_limit=float("inf") - ) - ), - wrapper=dict( - kwargs=dict( - augment_observation=True - ) - ) - ), dict( - block=dict( - cls='gym_jiminy.common.blocks.PDAdapter', - kwargs=dict( - update_ratio=-1, - order=1, - ) - ), - wrapper=dict( - kwargs=dict( - augment_observation=False - ) - ) - ), dict( - block=dict( - cls='gym_jiminy.common.blocks.MahonyFilter', - kwargs=dict( - update_ratio=1, - exact_init=True, - kp=1.0, - ki=0.1 - ) - ) - ), dict( - wrapper=dict( - cls='gym_jiminy.common.wrappers.StackObservation', - kwargs=dict( - nested_filter_keys=[ - ('t',), - ('measurements', 'ImuSensor'), - ('actions',) - ], - num_stack=self.num_stack, - skip_frames_ratio=self.skip_frames_ratio - ) - ) - ) - ] - ) + return build_pipeline( + env_config=dict( + cls='gym_jiminy.envs.ANYmalJiminyEnv', + kwargs=dict( + step_dt=STEP_DT, + debug=False)), + layers_config=[ + dict( + block=dict( + cls='gym_jiminy.common.blocks.PDController', + kwargs=dict( + update_ratio=2, + kp=PID_KP, + kd=PID_KD, + joint_position_margin=0.0, + joint_velocity_limit=float("inf"), + joint_acceleration_limit=float("inf"))), + wrapper=dict( + kwargs=dict( + augment_observation=True)) + ), dict( + block=dict( + cls='gym_jiminy.common.blocks.PDAdapter', + kwargs=dict( + update_ratio=-1, + order=1)), + wrapper=dict( + kwargs=dict( + augment_observation=False)) + ), dict( + block=dict( + cls='gym_jiminy.common.blocks.MahonyFilter', + kwargs=dict( + update_ratio=1, + exact_init=True, + kp=1.0, + ki=0.1)) + ), dict( + wrapper=dict( + cls='gym_jiminy.common.wrappers.StackObservation', + kwargs=dict( + nested_filter_keys=[ + ('t',), + ('measurements', 'ImuSensor'), + ('actions',)], + num_stack=num_stack, + skip_frames_ratio=skip_frames_ratio)))]) + +class PipelineDesign(unittest.TestCase): + """ TODO: Write documentation + """ def test_load_files(self): """ TODO: Write documentation """ @@ -104,7 +85,8 @@ def test_load_files(self): data_dir = os.path.join(os.path.dirname(__file__), "data") # Generate machine-dependent reference trajectory - env = self.ANYmalPipelineEnv() + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + env = ANYmalPipelineEnv() env.reset(seed=0) for _ in range(10): env.step(env.action) @@ -130,14 +112,16 @@ def test_load_files(self): def test_override_default(self): """ TODO: Write documentation """ + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + # Override default environment arguments - step_dt_2 = 2 * self.step_dt - env = self.ANYmalPipelineEnv(step_dt=step_dt_2) + step_dt_2 = 2 * STEP_DT + env = ANYmalPipelineEnv(step_dt=step_dt_2) self.assertEqual(env.unwrapped.step_dt, step_dt_2) # It does not override the default persistently - env = self.ANYmalPipelineEnv() - self.assertEqual(env.unwrapped.step_dt, self.step_dt) + env = ANYmalPipelineEnv() + self.assertEqual(env.unwrapped.step_dt, STEP_DT) def test_memory_leak(self): """Check that memory is freed when environment goes out of scope. @@ -146,7 +130,8 @@ def test_memory_leak(self): objects that cannot be tracked by Python, which would make it impossible for the garbage collector to release memory. """ - env = self.ANYmalPipelineEnv() + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + env = ANYmalPipelineEnv() env.reset(seed=0) proxy = weakref.proxy(env) env = None @@ -157,7 +142,10 @@ def test_initial_state(self): """ TODO: Write documentation """ # Get initial observation - env = self.ANYmalPipelineEnv() + num_stack = 4 + skip_frames_ratio = 3 + ANYmalPipelineEnv = _create_pipeline(num_stack, skip_frames_ratio) + env = ANYmalPipelineEnv() obs, _ = env.reset(seed=0) # Controller target is observed, and has right name @@ -165,11 +153,11 @@ def test_initial_state(self): # Target, time, and Imu data are stacked self.assertEqual(obs['t'].ndim, 1) - self.assertEqual(len(obs['t']), self.num_stack) + self.assertEqual(len(obs['t']), num_stack) self.assertEqual(obs['measurements']['ImuSensor'].ndim, 3) - self.assertEqual(len(obs['measurements']['ImuSensor']), self.num_stack) + self.assertEqual(len(obs['measurements']['ImuSensor']), num_stack) controller_target_obs = obs['actions']['pd_controller'] - self.assertEqual(len(controller_target_obs), self.num_stack) + self.assertEqual(len(controller_target_obs), num_stack) self.assertEqual(obs['measurements']['EffortSensor'].ndim, 2) # Stacked obs are zeroed @@ -192,50 +180,59 @@ def test_initial_state(self): def test_stacked_obs(self): """ TODO: Write documentation """ - # Perform a single step - env = self.ANYmalPipelineEnv() - env.reset(seed=0) - action = env.action + 1.0e-3 - obs, *_ = env.step(action) - - # Extract PD controller wrapper env - env_ctrl = env.env.env.env - assert isinstance(env_ctrl.controller, PDController) - - # Observation stacking is skipping the required number of frames - stack_dt = (self.skip_frames_ratio + 1) * env.observe_dt - t_obs_last = env.step_dt - env.step_dt % stack_dt - self.assertTrue(np.isclose( - obs['t'][-1], env.stepper_state.t, TOLERANCE)) - for i in range(1, self.num_stack): - self.assertTrue(np.isclose( - obs['t'][::-1][i], t_obs_last - (i - 1) * stack_dt, TOLERANCE)) - - # Initial observation is consistent with internal simulator state - controller_target_obs = obs['actions']['pd_controller'] - self.assertTrue(np.all(controller_target_obs[-1] == env_ctrl.action)) - imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] - imu_data_obs = obs['measurements']['ImuSensor'][-1] - self.assertTrue(np.all(imu_data_ref == imu_data_obs)) - state_ref = {'q': env.robot_state.q, 'v': env.robot_state.v} - state_obs = obs['states']['agent'] - self.assertTrue(np.all(state_ref['q'] == state_obs['q'])) - self.assertTrue(np.all(state_ref['v'] == state_obs['v'])) - - # Step until to reach the next stacking breakpoint - n_steps_breakpoint = int(stack_dt // _gcd(env.step_dt, stack_dt)) - for _ in range(1, n_steps_breakpoint): + for num_stack, skip_frames_ratio in ((3, 2), (4, 3)): + # Perform a single step + ANYmalPipelineEnv = _create_pipeline(num_stack, skip_frames_ratio) + env = ANYmalPipelineEnv() + env.reset(seed=0) + action = env.action + 1.0e-3 obs, *_ = env.step(action) - for i, t in enumerate(np.flip(obs['t'])): - self.assertTrue(np.isclose( - t, n_steps_breakpoint * env.step_dt - i * stack_dt, TOLERANCE)) - imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] - imu_data_obs = obs['measurements']['ImuSensor'][-1] - self.assertTrue(np.all(imu_data_ref == imu_data_obs)) + + # Extract PD controller wrapper env + env_ctrl = env.env.env.env + assert isinstance(env_ctrl.controller, PDController) + + # Observation stacking is skipping the required number of frames + stack_dt = (skip_frames_ratio + 1) * env.observe_dt + t_obs_last = env.step_dt - env.step_dt % stack_dt + is_shifted = env.step_dt % stack_dt > 0.0 + np.testing.assert_allclose( + obs['t'][-1], env.stepper_state.t, atol=TOLERANCE) + for i in range(1, num_stack): + t_ref = max(t_obs_last - (i - is_shifted) * stack_dt, 0.0) + np.testing.assert_allclose( + obs['t'][::-1][i], t_ref, atol=TOLERANCE) + + # Initial observation is consistent with internal simulator state + controller_target = obs['actions']['pd_controller'] + np.testing.assert_allclose(controller_target[-1], env_ctrl.action) + imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] + imu_data_obs = obs['measurements']['ImuSensor'][-1] + np.testing.assert_allclose(imu_data_ref, imu_data_obs) + state_ref = {'q': env.robot_state.q, 'v': env.robot_state.v} + state_obs = obs['states']['agent'] + np.testing.assert_allclose(state_ref['q'], state_obs['q']) + np.testing.assert_allclose(state_ref['v'], state_obs['v']) + + # Step until to next stacking breakpoint and filling buffer + n_steps_breakpoint = int(stack_dt // _gcd(env.step_dt, stack_dt)) + n_steps_full = int(np.ceil( + num_stack / int(np.ceil(env.step_dt / stack_dt) + 1))) + n_steps = n_steps_breakpoint * int( + np.ceil(n_steps_full / n_steps_breakpoint)) + for _ in range(1, n_steps): + obs, *_ = env.step(action) + for i, t in enumerate(np.flip(obs['t'])): + t_ref = n_steps * env.step_dt - i * stack_dt + np.testing.assert_allclose(t, t_ref, atol=TOLERANCE) + imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] + imu_data_obs = obs['measurements']['ImuSensor'][-1] + np.testing.assert_allclose(imu_data_ref, imu_data_obs) def test_update_periods(self): # Perform a single step and get log data - env = self.ANYmalPipelineEnv() + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + env = ANYmalPipelineEnv() def configure_telemetry() -> InterfaceJiminyEnv: engine_options = env.simulator.get_options() diff --git a/python/gym_jiminy/unit_py/test_quantities.py b/python/gym_jiminy/unit_py/test_quantities.py index be47dba11..81a1f9ce5 100644 --- a/python/gym_jiminy/unit_py/test_quantities.py +++ b/python/gym_jiminy/unit_py/test_quantities.py @@ -262,7 +262,8 @@ def test_masked(self): env.quantities["v_masked"], quantity.data[[0, 2, 4]]) def test_true_vs_reference(self): - env = gym.make("gym_jiminy.envs:atlas", debug=True) + env = gym.make("gym_jiminy.envs:atlas", debug=False) + env.eval() frame_names = [ frame.name for frame in env.robot.pinocchio_model.frames] @@ -409,7 +410,7 @@ def test_capture_point(self): update_quantities( env.robot, pin.neutral(env.robot.pinocchio_model_th), - update_physics=True, + update_dynamics=True, update_centroidal=True, update_energy=False, update_jacobian=False, diff --git a/python/gym_jiminy/unit_py/test_rewards.py b/python/gym_jiminy/unit_py/test_rewards.py index 96b3f6ddc..04b4158ee 100644 --- a/python/gym_jiminy/unit_py/test_rewards.py +++ b/python/gym_jiminy/unit_py/test_rewards.py @@ -28,15 +28,17 @@ class Rewards(unittest.TestCase): """ TODO: Write documentation """ def setUp(self): - env = gym.make("gym_jiminy.envs:atlas", debug=True) - env.reset(seed=1) - action = env.action_space.sample() + self.env = gym.make("gym_jiminy.envs:atlas-pid", debug=False) + + self.env.eval() + self.env.reset(seed=1) + action = self.env.action_space.sample() for _ in range(10): - env.step(action) - env.stop() - trajectory = extract_trajectory_from_log(env.log_data) + self.env.step(action) + self.env.stop() + trajectory = extract_trajectory_from_log(self.env.log_data) + self.env.train() - self.env = gym.make("gym_jiminy.envs:atlas") self.env.quantities.add_trajectory("reference", trajectory) self.env.quantities.select_trajectory("reference") @@ -130,12 +132,11 @@ def test_stability(self): def test_friction(self): CUTOFF = 0.5 - env = gym.make("gym_jiminy.envs:atlas-pid", debug=True) - reward_friction = MinimizeFrictionReward(env, cutoff=CUTOFF) + reward_friction = MinimizeFrictionReward(self.env, cutoff=CUTOFF) quantity = reward_friction.quantity - env.reset(seed=0) - _, _, terminated, _, _ = env.step(env.action) + self.env.reset(seed=0) + _, _, terminated, _, _ = self.env.step(self.env.action) force_tangential_rel = quantity.get() force_tangential_rel_norm = np.sum(np.square(force_tangential_rel)) diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py index cffe6eb7f..429b7aaf8 100644 --- a/python/jiminy_py/src/jiminy_py/dynamics.py +++ b/python/jiminy_py/src/jiminy_py/dynamics.py @@ -9,7 +9,7 @@ import logging from bisect import bisect_left from dataclasses import dataclass -from typing import Optional, Tuple, Sequence, Callable, Literal +from typing import List, Union, Optional, Tuple, Sequence, Callable, Literal import numpy as np @@ -359,11 +359,13 @@ def update_quantities(robot: jiminy.Model, position: np.ndarray, velocity: Optional[np.ndarray] = None, acceleration: Optional[np.ndarray] = None, - update_physics: bool = True, + f_external: Optional[ + Union[List[np.ndarray], pin.StdVec_Force]] = None, + update_dynamics: bool = True, update_centroidal: bool = True, update_energy: bool = True, update_jacobian: bool = False, - update_collisions: bool = True, + update_collisions: bool = False, use_theoretical_model: bool = True) -> None: """Compute all quantities using position, velocity and acceleration configurations. @@ -372,11 +374,11 @@ def update_quantities(robot: jiminy.Model, the model position, velocity and acceleration. This includes: - - body spatial transforms, + - body and frame spatial transforms, - body spatial velocities, - body spatial drifts, - - body transform acceleration, - - body transform jacobians, + - body spatial acceleration, + - joint transform jacobian matrices, - center-of-mass position, - center-of-mass velocity, - center-of-mass drift, @@ -387,34 +389,31 @@ def update_quantities(robot: jiminy.Model, - collisions and distances .. note:: - Computation results are stored internally in the robot, and can - be retrieved with associated getters. + Computation results are stored internally in the robot, and can be + retrieved with associated getters. .. warning:: This function modifies the internal robot data. - .. warning:: - It does not called overloaded pinocchio methods provided by - `jiminy_py.core` but the original pinocchio methods instead. As a - result, it does not take into account the rotor inertias / armatures. - One is responsible of calling the appropriate methods manually instead - of this one if necessary. This behavior is expected to change in the - future. - :param robot: Jiminy robot. - :param position: Robot position vector. - :param velocity: Robot velocity vector. - :param acceleration: Robot acceleration vector. - :param update_physics: Whether to compute the non-linear effects and - internal/external forces. - Optional: True by default. + :param position: Configuration vector. + :param velocity: Joint velocity vector. + :param acceleration: Joint acceleration vector. + :param f_external: External forces applied on each joints. + :param update_dynamics: Whether to compute the non-linear effects and the + joint internal forces. + Optional: True by default. :param update_centroidal: Whether to compute the centroidal dynamics (incl. CoM) of the robot. Optional: False by default. :param update_energy: Whether to compute the energy of the robot. Optional: False by default - :param update_jacobian: Whether to compute the jacobians. + :param update_jacobian: Whether to compute the Jacobian matrices of the + joint transforms. Optional: False by default. + :param update_collisions: Whether to detect collisions and compute + distances between all the geometry objects. + Optional: False by default. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. @@ -427,7 +426,7 @@ def update_quantities(robot: jiminy.Model, model = robot.pinocchio_model data = robot.pinocchio_data - if (update_physics and update_centroidal and + if (update_dynamics and update_centroidal and update_energy and update_jacobian and velocity is not None and acceleration is None): pin.computeAllTerms(model, data, position, velocity) @@ -440,36 +439,36 @@ def update_quantities(robot: jiminy.Model, pin.forwardKinematics( model, data, position, velocity, acceleration) - if update_centroidal: - if velocity is None: - kinematic_level = pin.POSITION - elif acceleration is None: - kinematic_level = pin.VELOCITY - else: - kinematic_level = pin.ACCELERATION - pin.centerOfMass(model, data, kinematic_level, False) - pin.computeCentroidalMomentumTimeVariation(model, data) - if update_jacobian: if update_centroidal: pin.jacobianCenterOfMass(model, data) - pin.computeJointJacobians(model, data) + if not update_dynamics: + pin.computeJointJacobians(model, data) - if update_physics: + if update_dynamics: if velocity is not None: pin.nonLinearEffects(model, data, position, velocity) - pin.crba(model, data, position) + jiminy.crba(model, data, position) if update_energy: if velocity is not None: - pin.computeKineticEnergy(model, data) + jiminy.computeKineticEnergy( + model, data, position, velocity, update_kinematics=False) pin.computePotentialEnergy(model, data) + if update_centroidal: + pin.computeCentroidalMomentumTimeVariation(model, data) + if acceleration is not None: + pin.centerOfMass(model, data, pin.ACCELERATION, False) + + if update_dynamics and acceleration is not None and f_external is not None: + jiminy.rnea(model, data, position, velocity, acceleration, f_external) + pin.updateFramePlacements(model, data) + pin.updateGeometryPlacements( + model, data, robot.collision_model, robot.collision_data) if update_collisions: - pin.updateGeometryPlacements( - model, data, robot.collision_model, robot.collision_data) pin.computeCollisions( robot.collision_model, robot.collision_data, stop_at_first_collision=False) @@ -797,7 +796,7 @@ def compute_freeflyer_state_from_fixed_body( position, velocity, acceleration, - update_physics=False, + update_dynamics=False, update_centroidal=False, update_energy=False, use_theoretical_model=use_theoretical_model) diff --git a/python/jiminy_py/unit_py/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py index 2b77878e7..7995a3201 100644 --- a/python/jiminy_py/unit_py/test_simple_pendulum.py +++ b/python/jiminy_py/unit_py/test_simple_pendulum.py @@ -230,7 +230,7 @@ def test_extract_trajectory(self): assert np.abs(state_0.t) < 1e-12 assert np.abs(state_f.t - T_END) < 1e-12 for key, value in asdict(state_f).items(): - if key == "t": + if key in ("t", "lambda_c"): continue data = getattr(robot_state, key) if key == "f_external": diff --git a/python/jiminy_py/unit_py/utilities.py b/python/jiminy_py/unit_py/utilities.py index 6c7a182c8..b187acead 100644 --- a/python/jiminy_py/unit_py/utilities.py +++ b/python/jiminy_py/unit_py/utilities.py @@ -119,6 +119,7 @@ def setup_controller_and_engine( telemetry_options["enableEffort"] = True telemetry_options["enableCommand"] = True telemetry_options["enableForceExternal"] = True + telemetry_options["enableConstraint"] = True telemetry_options["enableEnergy"] = True engine.set_options(engine_options) diff --git a/python/jiminy_pywrap/src/helpers.cc b/python/jiminy_pywrap/src/helpers.cc index 2789db25a..90ed42f3c 100644 --- a/python/jiminy_pywrap/src/helpers.cc +++ b/python/jiminy_pywrap/src/helpers.cc @@ -321,80 +321,113 @@ namespace jiminy::python void exposeHelpers() { - // clang-format off - bp::def("build_geom_from_urdf", &buildGeometryModelFromUrdf, - (bp::arg("pinocchio_model"), "urdf_filename", "geom_type", - bp::arg("mesh_package_dirs") = bp::list(), - bp::arg("load_meshes") = true, - bp::arg("make_meshes_convex") = false)); - - bp::def("build_models_from_urdf", &buildMultipleModelsFromUrdf, - (bp::arg("urdf_path"), "has_freeflyer", - bp::arg("mesh_package_dirs") = bp::list(), - bp::arg("build_visual_model") = false, - bp::arg("load_visual_meshes") = false)); + bp::def("build_geom_from_urdf", + &buildGeometryModelFromUrdf, + (bp::arg("pinocchio_model"), + "urdf_filename", + "geom_type", + bp::arg("mesh_package_dirs") = bp::list(), + bp::arg("load_meshes") = true, + bp::arg("make_meshes_convex") = false)); + + bp::def("build_models_from_urdf", + &buildMultipleModelsFromUrdf, + (bp::arg("urdf_path"), + "has_freeflyer", + bp::arg("mesh_package_dirs") = bp::list(), + bp::arg("build_visual_model") = false, + bp::arg("load_visual_meshes") = false)); bp::def("save_to_binary", &saveRobotToBinary, (bp::arg("robot"))); - bp::def("load_from_binary", &buildRobotFromBinary, - (bp::arg("data"), - bp::arg("mesh_path_dir") = bp::object(), - bp::arg("mesh_package_dirs") = bp::list())); + bp::def("load_from_binary", + &buildRobotFromBinary, + (bp::arg("data"), + bp::arg("mesh_path_dir") = bp::object(), + bp::arg("mesh_package_dirs") = bp::list())); bp::def("get_joint_type", &getJointType, bp::arg("joint_model")); - bp::def("get_joint_type", &getJointTypeFromIndex, - (bp::arg("pinocchio_model"), "joint_index")); - bp::def("get_joint_indices", &getJointIndices, - (bp::arg("pinocchio_model"), "joint_names")); - bp::def("get_joint_position_first_index", &getJointPositionFirstIndex, - (bp::arg("pinocchio_model"), "joint_name")); - bp::def("is_position_valid", &isPositionValid, - (bp::arg("pinocchio_model"), "position", - bp::arg("tol_abs") = std::numeric_limits::epsilon())); - - bp::def("get_frame_indices", &getFrameIndices, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "frame_names")); - bp::def("get_joint_indices", &getFrameIndices, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "joint_names")); + bp::def( + "get_joint_type", &getJointTypeFromIndex, (bp::arg("pinocchio_model"), "joint_index")); + bp::def( + "get_joint_indices", &getJointIndices, (bp::arg("pinocchio_model"), "joint_names")); + bp::def("get_joint_position_first_index", + &getJointPositionFirstIndex, + (bp::arg("pinocchio_model"), "joint_name")); + bp::def("is_position_valid", + &isPositionValid, + (bp::arg("pinocchio_model"), + "position", + bp::arg("tol_abs") = std::numeric_limits::epsilon())); + + bp::def("get_frame_indices", + &getFrameIndices, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "frame_names")); + bp::def("get_joint_indices", + &getFrameIndices, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "joint_names")); bp::def("array_copyto", &arrayCopyTo, (bp::arg("dst"), "src")); bp::def("multi_array_copyto", &multiArrayCopyTo, (bp::arg("dst"), "src")); - // Do not use EigenPy To-Python converter because it considers matrices with 1 column as vectors - bp::def("interpolate_positions", &interpolatePositions, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out")); + // Do NOT use EigenPy to-python converter as it considers arrays with 1 column as vectors + bp::def("interpolate_positions", + &interpolatePositions, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out")); bp::def("aba", - &pinocchio_overload::aba< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>, + &pinocchio_overload::aba, bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "u", "fext"), "Compute ABA with external forces, store the result in Data::ddq and return it."); + bp::def( + "rnea", + &pinocchio_overload::rnea, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"), + "Compute the RNEA without external forces, store the result in Data and return it."); bp::def("rnea", - &pinocchio_overload::rnea< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd>, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"), - "Compute the RNEA without external forces, store the result in Data and return it."); - bp::def("rnea", - &pinocchio_overload::rnea< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>, + &pinocchio_overload::rnea, bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a", "fext"), "Compute the RNEA with external forces, store the result in Data and return it."); bp::def("crba", - &pinocchio_overload::crba< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd>, + &pinocchio_overload:: + crba, bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fast_math") = false), + (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fastmath") = false), "Computes CRBA, store the result in Data and return it."); bp::def("computeKineticEnergy", - &pinocchio_overload::computeKineticEnergy< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd>, - (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v"), + &pinocchio_overload::computeKineticEnergy, + (bp::arg("pinocchio_model"), + "pinocchio_data", + "q", + "v", + bp::arg("update_kinematics") = true), "Computes the forward kinematics and the kinematic energy of the model for the " "given joint configuration and velocity given as input. " "The result is accessible through data.kinetic_energy."); @@ -402,9 +435,12 @@ namespace jiminy::python bp::def("computeJMinvJt", &pinocchio_overload::computeJMinvJt, bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "pinocchio_data", "J", bp::arg("update_decomposition") = true)); - bp::def("solveJMinvJtv", &solveJMinvJtv, + (bp::arg("pinocchio_model"), + "pinocchio_data", + "J", + bp::arg("update_decomposition") = true)); + bp::def("solveJMinvJtv", + &solveJMinvJtv, (bp::arg("pinocchio_data"), "v", bp::arg("update_decomposition") = true)); - // clang-format on } }