ft[benchmark]: Adds wrapper and cases for panda arm and point robot.

benchmarks/panda_arm/.gitignore

@@ -0,0 +1 @@
+jit_*

benchmarks/panda_arm/mppi_planner/__init__.py

@@ -0,0 +1 @@
+from .mppi_planner_wrapper import MPPIPlanner

benchmarks/panda_arm/mppi_planner/mppi_planner_wrapper.py

@@ -0,0 +1,132 @@
+from omegaconf import OmegaConf
+from plannerbenchmark.generic.planner import Planner
+from mppiisaac.planner.isaacgym_wrapper import IsaacGymWrapper
+from mppiisaac.planner.mppi_isaac import MPPIisaacPlanner
+
+import torch
+
+class ObjectiveOld(object):
+    def __init__(self, goal, device):
+        self.nav_goal = torch.tensor(goal, device=device)
+
+        self.w_nav = 2.0
+        self.w_obs = 1
+
+    def compute_cost(self, sim: IsaacGymWrapper):
+        dof_state = sim.dof_state
+        pos = torch.cat((dof_state[:, 0].unsqueeze(1), dof_state[:, 2].unsqueeze(1)), 1)
+        obs_positions = sim.obstacle_positions
+
+        nav_cost = torch.clamp(
+            torch.linalg.norm(pos - self.nav_goal, axis=1), min=0, max=1999
+        )
+
+        # sim.gym.refresh_net_contact_force_tensor(sim.sim)
+        # sim.net_cf
+
+        # This can cause steady state error if the goal is close to an obstacle, better use contact forces later on
+        obs_cost = torch.sum(
+            1 / torch.linalg.norm(obs_positions[:, :, :2] - pos.unsqueeze(1), axis=2),
+            axis=1,
+        )
+
+        return nav_cost * self.w_nav + obs_cost * self.w_obs
+
+class Objective(object):
+    def __init__(self, goal, device):
+        self.nav_goal = torch.tensor(goal, device=device)
+
+        self.w_nav = 1.0
+        self.w_obs = 0.5
+
+    def compute_cost(self, sim: IsaacGymWrapper):
+        dof_state = sim.dof_state
+        pos = torch.cat((dof_state[:, 0].unsqueeze(1), dof_state[:, 2].unsqueeze(1)), 1)
+        obs_positions = sim.obstacle_positions
+
+        nav_cost = torch.clamp(
+            torch.linalg.norm(pos - self.nav_goal, axis=1) - 0.05, min=0, max=1999
+        )
+
+        # sim.gym.refresh_net_contact_force_tensor(sim.sim)
+        # sim.net_cf
+
+        # This can cause steady state error if the goal is close to an obstacle, better use contact forces later on
+        obs_cost = torch.sum(
+            1 / torch.linalg.norm(obs_positions[:, :, :2] - pos.unsqueeze(1), axis=2),
+            axis=1,
+        )
+
+        return nav_cost * self.w_nav + obs_cost * self.w_obs
+
+class EndEffectorGoalObjective(object):
+    def __init__(self, goal, device):
+        self.nav_goal = torch.tensor(goal, device=device)
+        self.ort_goal = torch.tensor([1, 0, 0, 0], device=device)
+
+    def compute_cost(self, sim):
+        pos = sim.rigid_body_state[:, sim.robot_rigid_body_ee_idx, :3]
+        ort = sim.rigid_body_state[:, sim.robot_rigid_body_ee_idx, 3:7]
+        # dof_states = gym.acquire_dof_state_tensor(sim)
+
+        reach_cost = torch.linalg.norm(pos - self.nav_goal, axis=1)
+        align_cost = torch.linalg.norm(ort - self.ort_goal, axis=1)
+        return 10 * reach_cost + align_cost
+        # return torch.clamp(
+        #     torch.linalg.norm(pos - self.nav_goal, axis=1) - 0.05, min=0, max=1999
+        # )
+
+
+
+
+
+class MPPIPlanner(Planner):
+
+    def __init__(self, exp, **kwargs):
+        super().__init__(exp, **kwargs)
+        self.cfg = kwargs['config']
+        initial_actor_position = exp.initState()[0].tolist() 
+        initial_actor_position[2] += 0.05
+        initial_actor_position = [0.0, 0.0, 0.05]
+        self.cfg['initial_actor_positions'] = [initial_actor_position]
+        self._config = OmegaConf.create(kwargs)
+
+        self.reset()
+
+    def setJointLimits(self, limits):
+        self._limits = limits
+
+    def setGoal(self, motionPlanningGoal):
+        cfg = OmegaConf.create(self.cfg)
+        goal_position = motionPlanningGoal.sub_goals()[0].position()
+        objective = EndEffectorGoalObjective(goal_position, cfg.mppi.device)
+        if not hasattr(self, '_planner'):
+            self._planner = MPPIisaacPlanner(cfg, objective)
+
+
+    def setSelfCollisionAvoidance(self, r_body):
+        pass
+
+    def setObstacles(self, obstacles, r_body):
+        pass
+
+    def concretize(self):
+        pass
+
+    def save(self, folderPath):
+        file_name = folderPath + "/planner.yaml"
+        OmegaConf.save(config=self._config, f=file_name)
+
+    def computeAction(self, **kwargs):
+        ob = kwargs
+        obst = ob["FullSensor"]["obstacles"]
+        for o in obst.values():
+            o['type'] = 'sphere'
+
+        action = self._planner.compute_action(
+            q=ob["joint_state"]["position"],
+            qdot=ob["joint_state"]["velocity"],
+            obst=obst,
+        )
+        return action.numpy()
+

benchmarks/panda_arm/results/.gitignore

@@ -0,0 +1 @@
+*/*

benchmarks/panda_arm/setup/acados_mpc.yaml

@@ -0,0 +1,7 @@
+name: AcadosMpc
+N: 20
+w_pos: 4.0
+w_coll: 0.00
+w_input: 0.01
+w_joints: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
+w_vref: 5.0

benchmarks/panda_arm/setup/exp.yaml

@@ -0,0 +1,108 @@
+T: 2000
+dt: 0.01
+urdf_file_name: "panda.urdf"
+base_type: "holonomic"
+root_link : "panda_link0"
+control_mode: 'acc'
+ee_links:
+  - "panda_link7"
+robot_type: panda
+n: 7
+goal:
+  subgoal1:
+    is_primary_goal: True
+    weight: 1
+    indices:
+      - 0
+      - 1
+      - 2
+    parent_link: "panda_link0"
+    child_link: "panda_link7"
+    desired_position:
+      - 0.5
+      - -0.4
+      - 0.3
+    low:
+      - -0.4
+      - -0.4
+      - 0.1
+    high: 
+      - 0.7
+      - 0.0
+      - 1.1
+    type: staticSubGoal
+    epsilon: 0.05
+initState:
+  q0: 
+    - 1.0
+    - 0.0
+    - 0.0
+    - -1.50
+    - 0.0
+    - 1.8675
+    - 0.0
+  q0dot:
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+limits:
+  low: 
+    - -2.8973
+    - -1.7628
+    - -2.8973
+    - -3.0718
+    - -2.8973
+    - -0.0175
+    - -2.8973
+  high:
+    - 2.8973
+    - 1.7628
+    - 2.8973
+    - 0.0698
+    - 2.8973
+    - 3.7525
+    - 2.8973
+r_body: 0.15
+selfCollision:
+  pairs:
+    - ["panda_link0", "panda_link6"]
+    - ["panda_link0", "panda_link7"]
+    - ["panda_link1", "panda_link6"]
+    - ["panda_link1", "panda_link7"]
+    - ["panda_link2", "panda_link6"]
+    - ["panda_link2", "panda_link7"]
+collision_links:
+  - "panda_link2"
+  - "panda_link3"
+  - "panda_link5"
+  - "panda_link7"
+obstacles:
+  obst0:
+    geometry:
+      position:
+        - 0.5
+        - 0.0
+        - 0.6
+      radius: 0.15
+    type: sphere
+    high:
+      position:
+        - 1.0
+        - 1.0
+        - 1.0
+      radius:
+        0.3
+    low:
+      position:
+        - 0.0
+        - -1.0
+        - 0.0
+      radius:
+        0.1
+randomObstacles:
+  number: 5
+

benchmarks/panda_arm/setup/fabric.yaml

@@ -0,0 +1,25 @@
+interval: 1
+name: fabric
+robot_type: simPanda
+n: 7
+m_base: 1.0
+obst:
+  exp: 1.0
+  lam: 1.0
+selfCol:
+  exp: 3.0
+  lam: 2.0
+attractor:
+  k_psi: 2.0
+limits:
+  exp: 1.0
+  lam: 1.0
+speed:
+  ex_factor: 1.0
+damper:
+  r_d: 0.05
+  b:
+    - 0.01
+    - 3.5
+dynamic: False
+

benchmarks/panda_arm/setup/mpc.yaml

@@ -0,0 +1,13 @@
+time_horizon: 30
+time_step: 0.1
+interval: 10
+n: 7
+number_obstacles: 5
+slack: true
+name: forcesprompc
+model_name: panda
+weights:
+  ws: 1e10
+  wu: 1.0
+  wvel: 1.0
+  wx: 10.0

benchmarks/panda_arm/setup/mppi.yaml

@@ -0,0 +1,102 @@
+name: mppi
+config:
+  render: true
+  n_steps: 1000
+  mppi:
+    num_samples: 100
+    horizon: 12
+    mppi_mode: halton-spline
+    sampling_method: halton
+    noise_sigma:
+    - - 1.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 1.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 1.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
+      - 0.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
+      - 0.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
+      - 0.0
+    - - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 0.0
+      - 1.0
+    noise_mu: null
+    device: cuda:0
+    lambda_: 0.1
+    update_lambda: false
+    update_cov: false
+    u_min:
+    - -2
+    u_max:
+    - 2
+    u_init: 0.0
+    U_init: null
+    u_scale: 1.0
+    u_per_command: 1
+    rollout_var_discount: 0.99
+    sample_null_action: true
+    noise_abs_cost: false
+    filter_u: true
+    use_priors: true
+  isaacgym:
+    dt: 0.05
+    substeps: 1
+    use_gpu_pipeline: true
+    num_client_threads: 0
+    viewer: false
+    num_obstacles: 10
+    spacing: 6.0
+  goal:
+  - 2.0
+  - 2.0
+  nx: 6
+  urdf_file: point_robot.urdf
+  fix_base: true
+  flip_visual: false
+  disable_gravity: false
+  differential_drive: false
+  wheel_base: 0.0
+  wheel_radius: 0.0
+  wheel_count: 0
+  ee_link: null
+  actors:
+    - 'panda'
+    - 'xaxis'
+    - 'yaxis'