Help regarding ResidualModelPairCollision

[jakiroshah]

Hello!

I am currently working on modeling a jump action between two predefined elevated platforms. I followed your excellent bipedal gaits tutorial which allowed me to setup a planner around the jump action (for the talos leg robot). However, I am struggling to incorporate the pair collision residual into the cost function.

Below, I've shared my attempt. Specifically, the additional cost is implemented in the _get_collision_model function, which I call each time a contact occurs—such as when the robot is positioned on the platform at both the start and end of the jump. After running this code several times, with and without the _get_collision_model function calls, I unfortunately observe no significant difference in the results from Crocoddyl...that is, the L2 norm of the initial and final force vectors remains almost identical in both cases.

Could you please advise on what I am missing? What am I doing wrong?

  def get_swing_foot_model(self, add_contact=True, com_pos=None):
      costs_list = crocoddyl.CostModelSum(self.state, self.actuation.nu)

      if isinstance(com_pos, np.ndarray):
          com_residual = crocoddyl.ResidualModelCoMPosition(self.state, com_pos, self.actuation.nu)
          com_cost = crocoddyl.CostModelResidual(self.state, com_residual)
          costs_list.addCost("com_track", com_cost, 1e6)

      for frame_id in self.foot_frame_ids_list:
          if not add_contact:
              continue
          w_cone = crocoddyl.WrenchCone(self.surface_normal, self.friction_coefficient, np.array([0.1, 0.05]))
          w_cone_res = crocoddyl.ResidualModelContactWrenchCone(self.state, frame_id, w_cone, self.actuation.nu)
          w_cone_act = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(w_cone.lb, w_cone.ub))
          w_cone_cost = crocoddyl.CostModelResidual(self.state, w_cone_act, w_cone_res)
          costs_list.addCost(self.robot_model.frames[frame_id].name + "_wrench_cone", w_cone_cost, 1e1)

      state_weights = np.array([0] * 3 + [500.0] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv)
      state_residual = crocoddyl.ResidualModelState(self.state, self.robot_model.defaultState, self.actuation.nu)
      state_activation = crocoddyl.ActivationModelWeightedQuad(state_weights**2)
      state_cost = crocoddyl.CostModelResidual(self.state, state_activation, state_residual)
      costs_list.addCost("state_cost", state_cost, 1e1)

      ctrl_residual = crocoddyl.ResidualModelControl(self.state, self.actuation.nu)
      ctrl_cost = crocoddyl.CostModelResidual(self.state, ctrl_residual)
      costs_list.addCost("ctrl_cost", ctrl_cost, 1e-1)

      contacts_list = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu)
      for frame_id in self.foot_frame_ids_list:
          if not add_contact:
              continue
          contact_model = crocoddyl.ContactModel6D(self.state,
                                                   frame_id,
                                                   pinocchio.SE3.Identity(),  # noqa
                                                   pinocchio.LOCAL_WORLD_ALIGNED,
                                                   self.actuation.nu,
                                                   np.array([0.0, 30.0]))
          contacts_list.addContact(self.robot_model.frames[frame_id].name + "_contact", contact_model)

      if add_contact:
          self._get_collision_model(costs_list)

      diff_action_model = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation,
                                                                              contacts_list, costs_list, 0.0, True)

      control = crocoddyl.ControlParametrizationModelPolyZero(self.actuation.nu)

      return crocoddyl.IntegratedActionModelEuler(diff_action_model, control, JumpProblem.TIME_STEP)

  def _get_collision_model(self, costs_list):
      geom_model = pinocchio.GeometryModel()  # noqa

      for foot_link_name, foot_frame_id in zip(self.foot_links_list, self.foot_frame_ids_list):
          geom_model.addGeometryObject(pinocchio.GeometryObject(foot_link_name,
                                                                foot_frame_id,
                                                                self.robot_model.frames[foot_frame_id].parentJoint,
                                                                pinocchio.SE3.Identity(),  # noqa
                                                                hppfcl.Capsule(0, 0.5)))

      for foot_link in self.foot_links_list:
          foot_geom_id = geom_model.getGeometryId(foot_link)
          for platform in self.platform_set.get_platforms_list():
              platform_geom_id = geom_model.addGeometryObject(platform.get_as_pinocchio_geom(self.robot_model))
              geom_model.addCollisionPair(pinocchio.CollisionPair(foot_geom_id, platform_geom_id))

      for pair_id, collision_pair in enumerate(geom_model.collisionPairs):
          foot_link_id = collision_pair.first
          platform = self.platform_set.get_platform_by_name(geom_model.geometryObjects[collision_pair.second].name)

          nu = self.actuation.nu

          collision_act = crocoddyl.ActivationModel2NormBarrier(3, platform.get_platform_convex_hull())
          collision_res = crocoddyl.ResidualModelPairCollision(self.state, nu, geom_model, pair_id, foot_link_id)
          collision_cost = crocoddyl.CostModelResidual(self.state, collision_act, collision_res)
          costs_list.addCost("collision_" + str(pair_id), collision_cost, 1e6)

Thank you so much :)