Skip to content

Commit

Permalink
Add test case
Browse files Browse the repository at this point in the history
  • Loading branch information
JeanElsner committed Feb 23, 2023
1 parent 15f4b9a commit cd3d62e
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 10 deletions.
4 changes: 2 additions & 2 deletions src/adnap/evaluate.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
from panda_model import Model

from .optimize import coriolis_error, gravity_error, mass_error
from .panda_param import PandaParametrized, sample, unflatten_params
from .panda_param import PandaParameterized, sample, unflatten_params


def run():
Expand All @@ -26,7 +26,7 @@ def run():

params = np.load(args.file)
m, c, I = unflatten_params(params)
r = PandaParametrized(m, c, I)
r = PandaParameterized(m, c, I)
model = Model(libfranka)

q_data, dq_data, __ = sample(args.n)
Expand Down
10 changes: 5 additions & 5 deletions src/adnap/optimize.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,23 +9,23 @@
import panda_model
from scipy.optimize import least_squares

from .panda_param import PandaParametrized, sample
from .panda_param import PandaParameterized, sample


def coriolis_error(model: panda_model.Model, opt_model: PandaParametrized,
def coriolis_error(model: panda_model.Model, opt_model: PandaParameterized,
q_data: np.ndarray, dq_data: np.ndarray):
return opt_model.coriolis(q_data, dq_data) @ dq_data - model.coriolis(
q_data, dq_data, np.zeros((3, 3)), 0, np.zeros(3))


def mass_error(model: panda_model.Model, opt_model: PandaParametrized,
def mass_error(model: panda_model.Model, opt_model: PandaParameterized,
q_data: np.ndarray):
I_indices = np.tril_indices(7)
return model.mass(q_data, np.zeros(
(3, 3)), 0, np.zeros(3))[I_indices] - opt_model.inertia(q_data)[I_indices]


def gravity_error(model: panda_model.Model, opt_model: PandaParametrized,
def gravity_error(model: panda_model.Model, opt_model: PandaParameterized,
q_data: np.ndarray):
return opt_model.gravload(q_data) - model.gravity(q_data, 0, np.zeros(3))

Expand Down Expand Up @@ -57,7 +57,7 @@ def residual(params):
I = params[28:].reshape((7, 6))

model = panda_model.Model(lib_path)
r = PandaParametrized(m, c, I)
r = PandaParameterized(m, c, I)

mass_res = []
coriolis_res = []
Expand Down
6 changes: 3 additions & 3 deletions src/adnap/panda_param.py
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ def unflatten_params(params: np.ndarray):
return m, c, I


class PandaParametrized(DHRobot):
class PandaParameterized(DHRobot):
"""
DH version of the Panda robot parameterized in the
dynamic physical parameters.
Expand Down Expand Up @@ -171,7 +171,7 @@ def __init__(self, m, c, I, **kwargs):
self.addconfiguration("qz", self.qz)


def create_sym_panda() -> PandaParametrized:
def create_sym_panda() -> PandaParameterized:
"""
Create symbolic version of parameterized Panda.
"""
Expand All @@ -182,4 +182,4 @@ def create_sym_panda() -> PandaParametrized:
I.append(create_inertia_symbols(i + 1))
c.append(create_com_symbols(i + 1))

return PandaParametrized(m, c, I, symbolic=True)
return PandaParameterized(m, c, I, symbolic=True)
12 changes: 12 additions & 0 deletions tests/test_ParameterizedPanda.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
import unittest
import numpy as np
from adnap import panda_param


class testParameterizedPanda(unittest.TestCase):

def test_Panda(self):
params = np.zeros(70)
m, c, I = panda_param.unflatten_params(params)
panda_param.PandaParameterized(m, c, I)
panda_param.create_sym_panda()

0 comments on commit cd3d62e

Please sign in to comment.