Skip to content

Commit

Permalink
Merge pull request #41 from tud-airlab/ft-benchmarks
Browse files Browse the repository at this point in the history
Ft benchmarks
  • Loading branch information
Chadi Salmi authored May 10, 2023
2 parents 2f3c5f6 + 5e603c5 commit 72c59a8
Show file tree
Hide file tree
Showing 23 changed files with 894 additions and 124 deletions.
1 change: 1 addition & 0 deletions benchmarks/panda_arm/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
jit_*
1 change: 1 addition & 0 deletions benchmarks/panda_arm/mppi_planner/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
from .mppi_planner_wrapper import MPPIPlanner
132 changes: 132 additions & 0 deletions benchmarks/panda_arm/mppi_planner/mppi_planner_wrapper.py
Original file line number Diff line number Diff line change
@@ -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()

1 change: 1 addition & 0 deletions benchmarks/panda_arm/results/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
*/*
7 changes: 7 additions & 0 deletions benchmarks/panda_arm/setup/acados_mpc.yaml
Original file line number Diff line number Diff line change
@@ -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
108 changes: 108 additions & 0 deletions benchmarks/panda_arm/setup/exp.yaml
Original file line number Diff line number Diff line change
@@ -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

25 changes: 25 additions & 0 deletions benchmarks/panda_arm/setup/fabric.yaml
Original file line number Diff line number Diff line change
@@ -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

13 changes: 13 additions & 0 deletions benchmarks/panda_arm/setup/mpc.yaml
Original file line number Diff line number Diff line change
@@ -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
102 changes: 102 additions & 0 deletions benchmarks/panda_arm/setup/mppi.yaml
Original file line number Diff line number Diff line change
@@ -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'
Loading

0 comments on commit 72c59a8

Please sign in to comment.