diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 547d699d53..9a6c02c831 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -135,6 +135,15 @@ if(BUILD_PYTHON_INTERFACE) geometry-models constraint-cholesky ) + if(BUILD_ADVANCED_TESTING) + # Set as advanced testing because it need a new + # example_robot_data release (> 4.4) + # and because computation is long in Debug + list( + APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES + g1-constraint-simulation + ) + endif() endif() if(BUILD_WITH_COLLISION_SUPPORT) diff --git a/examples/g1-constraint-simulation.py b/examples/g1-constraint-simulation.py new file mode 100644 index 0000000000..80159016cb --- /dev/null +++ b/examples/g1-constraint-simulation.py @@ -0,0 +1,299 @@ +""" +Simulation of a g1 robot whose left arm is attached by a point anchor. +We also simulate the joint limits and the joint friction of the entire robot. + +This example demonstrates how to: + - Load a URDF model of a g1 robot + - Construct a point anchor constraint + - Construct a joint limit constraint + - Construct a joint friction constraint + - Compute the drift term for all of these constraints + - Add Baumgarte stabilisation to the drift term for the position-level constraints + - Construct a simulation loop to solve the constraint dynamics of this system + - Set up the ADMM solver for solving hard constraint problems + - Visualize the system's trajectory in Meshcat (optional) +""" + +import os +import time +from pathlib import Path + +import numpy as np +import pinocchio as pin + +# ─── 1. Kinematic / dynamic model ──────────────────────────────────────────── + +model_path = Path(os.environ.get("EXAMPLE_ROBOT_DATA_MODEL_DIR")) +mesh_dir = model_path.parent.parent +urdf_filename = "g1_29dof_rev_1_0.urdf" +urdf_model_path = model_path / "g1_description/urdf" / urdf_filename +model, collision_model, visual_model = pin.buildModelsFromUrdf( + urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() +) + +# ─── 1. (Optional) Visualize the robot in Meshcat ──────────────────────────── + +has_viz = False +if pin.WITH_COLLISION: + try: + from pinocchio.visualize import MeshcatVisualizer + + viz = MeshcatVisualizer(model, collision_model, visual_model) + viz.initViewer(open=True) + viz.loadViewerModel() + has_viz = True + print("Meshcat viewer ready.") + except Exception: + print("Meshcat not available - running without visualization.") + +# ─── 2. Initial configuration ───────────────────────────────────────────────── + +q0 = pin.neutral(model) +q0[2] += 1.0 # for visualization purposes only +v0 = np.zeros(model.nv) # zero initial velocity +zero_torque = np.zeros(model.nv) # no external torques +dt = 1e-3 # time-step [s] + +if has_viz: + viz.display(q0) + +# ─── 3. Build the point anchor, joint limit and joint friction constraints ──── + +constraint_models = [] + +# ── 3a. Point anchor on the left arm ───────────────────────────────────────── +# Anchor the left hand (left_rubber_hand frame) to its current world position. +# The PointAnchorConstraintModel enforces that a point fixed in the body frame +# of joint2 coincides with a point fixed in the frame of joint1. +# +# joint1 = universe (0): placement1 gives the anchor location in world frame. +# joint2 = left_wrist_yaw_joint (23): placement2 is the hand frame offset in +# joint 23's local frame, taken directly from the URDF frame definition. + +anchor_frame_name = "left_rubber_hand" +anchor_fid = model.getFrameId(anchor_frame_name) +anchor_frame = model.frames[anchor_fid] +anchor_joint_id = anchor_frame.parentJoint # 23 = left_wrist_yaw_joint + +# Placement of the anchor point in the universe (world) frame at q0. +# Note: run forward kinematics once at q0 to get the world pose of the left hand. +data_init = model.createData() +pin.forwardKinematics(model, data_init, q0) +pin.updateFramePlacements(model, data_init) +anchor_world_pos = data_init.oMf[anchor_fid].translation.copy() +anchor_placement_world = pin.SE3(np.eye(3), anchor_world_pos) + +# Placement of the same point in joint 23's local frame (from the URDF). +anchor_placement_local = anchor_frame.placement + +pacm = pin.PointAnchorConstraintModel( + model, + 0, # universe joint + anchor_placement_world, + anchor_joint_id, + anchor_placement_local, +) +constraint_models.append(pin.ConstraintModel(pacm)) + +# Display a red ball at the anchor position in Meshcat. +if has_viz: + import meshcat.geometry as mg + import meshcat.transformations as mt + + viz.viewer["point_anchor"].set_object( + mg.Sphere(0.05), + mg.MeshLambertMaterial(color=0xFF0000), + ) + viz.viewer["point_anchor"].set_transform(mt.translation_matrix(anchor_world_pos)) + +# ── 3b. Joint limit constraints ─────────────────────────────────────────────── +# Apply joint limits to all actuated joints (skip joint 0 = universe and +# joint 1 = root free-flyer which has no meaningful position limits here). +actuated_joint_ids = list(range(2, model.njoints)) +# Note: lower/upper limits which are +/- inf will be ignored by the constraint model. +# Note: the constructor of joint limit constraint expects the limits +# to be of size (nq,). +# It will automatically select the relevant entries based on the provided +# actuated_joint_ids. +assert model.lowerPositionLimit.shape == (model.nq,) +assert model.upperPositionLimit.shape == (model.nq,) +jlcm = pin.JointLimitConstraintModel( + model, # + actuated_joint_ids, # + model.lowerPositionLimit, + model.upperPositionLimit, +) +constraint_models.append(pin.ConstraintModel(jlcm)) + +# ── 3c. Joint friction constraints ─────────────────────────────────────────── +# The g1 URDF does not specify dry-friction limits, so we set them manually +# as a fraction of the actuator effort limits. +# If the URDF had specified effort limits, +# we could then use model.[lower/upper]DryFrictionLimit directly. +joint_friction_coeff = 1.0 +lower_friction = np.zeros(model.nv) +upper_friction = np.zeros(model.nv) +# skip universe and free-flyer +for joint in model.joints[2:]: + lower_friction[joint.idx_v : joint.idx_v + joint.nv] = -joint_friction_coeff + upper_friction[joint.idx_v : joint.idx_v + joint.nv] = joint_friction_coeff +fjcm = pin.JointFrictionConstraintModel( + model, actuated_joint_ids, lower_friction, upper_friction +) +# Joint friction is velocity-dependent, so we must inform the time-step for +# proper scaling of the upper/lower limits +fjcm.setTimeStep(dt) +constraint_models.append(pin.ConstraintModel(fjcm)) + +total_residual_size = sum(cm.residualSize() for cm in constraint_models) +print(f"Number of constraints: {len(constraint_models)}") +print(f"Total constraint residual size: {total_residual_size}") + +# ─── 4. Setup and run the simulation loop ──────────────────────────────────── + +data = model.createData() +fext = [pin.Force.Zero() for _ in range(model.njoints)] + +# Initialise constraint data. +constraint_datas = [] +for cmodel in constraint_models: + constraint_datas.append(cmodel.createData()) + +# Initialise the Cholesky factorisation structure (allocates memory based on +# the sparsity pattern; actual values are filled by chol.compute in the loop). +pin.crba(model, data, q0, pin.Convention.WORLD) +chol = pin.ConstraintCholeskyDecomposition( + model, data, constraint_models, constraint_datas +) + +# Initialise the constraint solver, its settings and the result container. +# Note: this simulation is hard to solve: the point anchor and the joint limits +# are very stiff constraints. +# We add anderson acceleration and set a relatively high mu_prox to help convergence. +solver = pin.ADMMConstraintSolver() +settings = pin.ADMMSolverSettings() +settings.max_iterations = 10000 +settings.absolute_feasibility_tol = 1e-6 +settings.relative_feasibility_tol = 1e-6 +settings.absolute_complementarity_tol = 1e-6 +settings.relative_complementarity_tol = 1e-6 +settings.admm_update_rule = pin.ADMMUpdateRule.OSQP +settings.anderson_capacity = 10 +settings.mu_prox = 1e-4 +settings.solve_ncp = True +result = pin.ADMMSolverResult() + +# Simulate for a few time-steps, solving the constraint problem at each step. +horizon = 1500 +qs = [q0] +q = q0.copy() +v = v0.copy() +for t in range(horizon): + # Data needs to be informed of the current state of the system for + # downstream computations. + data.q_in = q + data.v_in = v + data.tau_in = zero_torque + + # CRBA is required before building the Cholesky decomposition of the + # Delassus matrix G = J M⁻¹ Jᵀ. + pin.crba(model, data, q, pin.Convention.WORLD) + + # Free velocity: what velocity the system would reach in one time-step + # without any constraint forces. + v_free = v + dt * pin.aba(model, data, q, v, zero_torque, fext) + + # Evaluate constraint Jacobians and residuals at the current state. + for cmodel, cdata in zip(constraint_models, constraint_datas): + cmodel.calc(model, data, cdata) + + # Cholesky decomposition of the Delassus matrix. + chol.compute(model, data, constraint_models, constraint_datas, 1e-10) + + # DelassusCholeskyExpression wraps the Cholesky factors for efficient solves. + delassus_expr = chol.getDelassusOperatorCholeskyExpression() + + # Constraint Jacobian and drift g = J v_free. + Jc = pin.getConstraintsJacobian(model, data, constraint_models, constraint_datas) + g = Jc @ v_free + # For position-level constraints (point anchor, joint limits) the drift can + # also include the current position error to enforce Baumgarte stabilisation: + # g_pos = J v_free + kp * position_error / dt + idx_cm = 0 + for cm, cd in zip(constraint_models, constraint_datas): + cm_size = cm.residualSize() + name = cm.shortname() + if name == "PointAnchorConstraintModel": + kp = cm.baumgarte_corrector_parameters.Kp + g_anchor = g[idx_cm : idx_cm + cm_size] + g_anchor += kp * cd.extract().constraint_position_error / dt + if name == "JointLimitConstraintModel": + kp = cm.baumgarte_corrector_parameters.Kp + g_limits = g[idx_cm : idx_cm + cm_size] + for i in range(cm_size): + # Note: the constraint residual is negative when the position is + # above the upper limit, and positive when below the lower limit. + if cd.extract().constraint_residual[i] < 0.0: + # constraint violated, apply Baumgarte correction to push it + # back within limits. + g_limits[i] += kp * cd.extract().constraint_residual[i] / dt + else: + # constraint not violated, the drift should include the velocity + # error to make sure no constraint impulse is applied when + # the joint is moving away from the limit. + g_limits[i] += cd.extract().constraint_residual[i] / dt + idx_cm += cm_size + + # Solve the constraint problem with the ADMM solver. + has_converged = solver.solve( + delassus_expr, g, constraint_models, constraint_datas, settings, result + ) + + # Update configuration and velocity for the next time step by applying + # the constraint impulses. + constraint_impulses = result.retrieveConstraintImpulses() + # convert impulses to forces/torques + constraint_forces = (1.0 / dt) * constraint_impulses + # map to generalised torques + tau_constraints = Jc.T @ constraint_forces + v_new = v + dt * pin.aba(model, data, q, v, zero_torque + tau_constraints, fext) + q_new = pin.integrate(model, q, v_new * dt) + + q = q_new.copy() + v = v_new.copy() + qs.append(q) + + print( + f" step {t:3d} | converged: {result.converged}" + f" it: {result.iterations:4d}" + f" primal: {result.primal_feasibility:.2e}" + f" dual: {result.dual_feasibility:.2e}" + f" compl: {result.complementarity:.2e}" + ) + + +# ─── 5. (Optional) Visualize the robot trajectory ──────────────────────────── +if has_viz: + print("\nReplaying trajectory in Meshcat …") + + def subSample(qs: np.ndarray, duration: float, fps: float): + nb_frames = len(qs) + nb_subframes = int(duration * fps) + if nb_frames < nb_subframes: + return qs + else: + step = nb_frames // nb_subframes + qs_sub = [qs[i] for i in range(0, nb_frames, step)] + return qs_sub + + fps = 60.0 + dt_vis = 1.0 / fps + qs = subSample(qs, dt * horizon, fps) + for _ in range(3): + for q_vis in qs: + step_start = time.time() + viz.display(q_vis) + time_until_next_step = dt_vis - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + time.sleep(1.0) diff --git a/include/pinocchio/bindings/python/algorithm/constraints/constraints-models.hpp b/include/pinocchio/bindings/python/algorithm/constraints/constraints-models.hpp index d89ce3f2b8..4dad543c76 100644 --- a/include/pinocchio/bindings/python/algorithm/constraints/constraints-models.hpp +++ b/include/pinocchio/bindings/python/algorithm/constraints/constraints-models.hpp @@ -47,6 +47,13 @@ namespace pinocchio (bp::arg("self"), bp::arg("model"), bp::arg("m_active_joints")), "Constructor from given joint index vector " "implied in the constraint.")) + .def( + bp::init< + const context::Model &, const JointIndexVector &, const context::VectorXs &, + const context::VectorXs &>( + (bp::arg("self"), bp::arg("model"), bp::arg("m_active_joints"), bp::arg("lb"), + bp::arg("ub")), + "Full constructor from model, active joints, lower and upper friction bounds.")) .def( bp::init( (bp::arg("self"), bp::arg("model")), "Constructor from the model only.")) diff --git a/include/pinocchio/src/constraints/joint-friction-constraint.hxx b/include/pinocchio/src/constraints/joint-friction-constraint.hxx index 95040a8873..fd60bcf8d7 100644 --- a/include/pinocchio/src/constraints/joint-friction-constraint.hxx +++ b/include/pinocchio/src/constraints/joint-friction-constraint.hxx @@ -261,6 +261,7 @@ namespace pinocchio res.m_active_joint_ids = m_active_joint_ids; res.m_friction_lower_limit = m_friction_lower_limit.template cast(); res.m_friction_upper_limit = m_friction_upper_limit.template cast(); + res.m_dt = static_cast(m_dt); return res; } @@ -280,7 +281,7 @@ namespace pinocchio && m_active_joints == other.m_active_joints && m_active_dofs == other.m_active_dofs && m_active_joint_ids == other.m_active_joint_ids && m_friction_lower_limit == other.m_friction_lower_limit - && m_friction_upper_limit == other.m_friction_upper_limit; + && m_friction_upper_limit == other.m_friction_upper_limit && m_dt == other.m_dt; } /// \brief Comparison operator @@ -726,7 +727,8 @@ namespace pinocchio /// \brief Comparison operator bool operator==(const JointFrictionConstraintDataTpl & /*other*/) const { - return true; + return friction_impulse_lower_limit == friction_impulse_lower_limit + && friction_impulse_upper_limit == friction_impulse_upper_limit; } /// \brief Comparison operator diff --git a/include/pinocchio/src/serialization/constraints-data.hxx b/include/pinocchio/src/serialization/constraints-data.hxx index e386f396b1..21ace75e99 100644 --- a/include/pinocchio/src/serialization/constraints-data.hxx +++ b/include/pinocchio/src/serialization/constraints-data.hxx @@ -73,6 +73,8 @@ namespace boost typedef ::pinocchio::JointFrictionConstraintDataTpl Self; typedef typename Self::Base Base; ar & make_nvp("base", boost::serialization::base_object(cdata)); + ar & make_nvp("friction_impulse_lower_limit", cdata.friction_impulse_lower_limit); + ar & make_nvp("friction_impulse_upper_limit", cdata.friction_impulse_upper_limit); } template diff --git a/include/pinocchio/src/serialization/constraints-model.hxx b/include/pinocchio/src/serialization/constraints-model.hxx index a53d7f8570..57aa50d185 100644 --- a/include/pinocchio/src/serialization/constraints-model.hxx +++ b/include/pinocchio/src/serialization/constraints-model.hxx @@ -214,6 +214,7 @@ namespace boost using Base::m_active_dofs; using Base::m_active_joint_ids; using Base::m_active_joints; + using Base::m_dt; using Base::m_friction_lower_limit; using Base::m_friction_upper_limit; }; @@ -239,6 +240,7 @@ namespace boost ar & make_nvp("m_active_joint_ids", cmodel_.m_active_joint_ids); ar & make_nvp("friction_lower_limit", cmodel_.m_friction_lower_limit); ar & make_nvp("friction_upper_limit", cmodel_.m_friction_upper_limit); + ar & make_nvp("dt", cmodel_.m_dt); } template diff --git a/models/example-robot-data b/models/example-robot-data index 6ed0cc9124..cdd6a3f938 160000 --- a/models/example-robot-data +++ b/models/example-robot-data @@ -1 +1 @@ -Subproject commit 6ed0cc91241562972190da6e69430834ae2b1158 +Subproject commit cdd6a3f938778cb6aed07a12a179125b700b1d04