diff --git a/examples/config/diffDriveMpc.yaml b/examples/config/diffDriveMpc.yaml index d839ea5..3705025 100644 --- a/examples/config/diffDriveMpc.yaml +++ b/examples/config/diffDriveMpc.yaml @@ -1,7 +1,7 @@ type: mpc -n: 3 +dof: 3 dt: 0.01 -H: 100 +time_horizon: 100 interval: 1 weights: w: 2.0 diff --git a/examples/config/planarArmMpc.yaml b/examples/config/planarArmMpc.yaml index 9950881..ade820d 100644 --- a/examples/config/planarArmMpc.yaml +++ b/examples/config/planarArmMpc.yaml @@ -1,7 +1,7 @@ type: mpc -n: 4 +dof: 4 dt: 0.01 -H: 10 +time_horizon: 10 interval: 1 weights: w: 5.0 diff --git a/examples/config/pointRobotMpc.yaml b/examples/config/pointRobotMpc.yaml index 4f8490a..ec491d4 100644 --- a/examples/config/pointRobotMpc.yaml +++ b/examples/config/pointRobotMpc.yaml @@ -1,7 +1,7 @@ type: mpc -n: 2 +dof: 2 dt: 0.01 -H: 10 +time_horizon: 10 interval: 1 weights: w: 10.0 diff --git a/examples/makeSolver.py b/examples/makeSolver.py index 7f62226..18941b0 100644 --- a/examples/makeSolver.py +++ b/examples/makeSolver.py @@ -7,13 +7,13 @@ def main(): - N = 10 - m = 2 + time_horizon = 10 + dim_goal = 2 dt = 0.01 - #mpcModel = PlanarArmMpcModel(m, N, 4) - #mpcModel = PointRobotMpcModel(m, N) - #mpcModel = DiffDriveMpcModel(2, N) - mpcModel = BoxerMpcModel(N) + #mpcModel = PlanarArmMpcModel(dim_goal, 4, time_horizon) + #mpcModel = PointRobotMpcModel(dim_goal, time_horizon) + #mpcModel = DiffDriveMpcModel(2, time_horizon) + mpcModel = BoxerMpcModel(time_horizon) mpcModel.setDt(dt) #mpcModel.setSlack() mpcModel.setObstacles(1, 2) diff --git a/examples/mpcPlanner.py b/examples/mpcPlanner.py index 5f0be44..39eae7a 100644 --- a/examples/mpcPlanner.py +++ b/examples/mpcPlanner.py @@ -4,9 +4,9 @@ import sys import forcespro import gym -import planarenvs.groundRobots -import planarenvs.pointRobot -import planarenvs.nLinkReacher +import planarenvs.ground_robots +import planarenvs.point_robot +import planarenvs.n_link_reacher import re from MotionPlanningEnv.sphereObstacle import SphereObstacle from MotionPlanningGoal.staticSubGoal import StaticSubGoal @@ -41,7 +41,7 @@ def main(): return myMPCPlanner.concretize() myMPCPlanner.reset() - n = myMPCPlanner.n() + n = myMPCPlanner.dof() staticGoalDict = { "m": 2, "w": 1.0, "prime": True, 'indices': [0, 1], 'parent_link': 0, 'child_link': n, 'desired_position': [2, -3], 'epsilon': 0.2, 'type': "staticSubGoal", @@ -52,7 +52,7 @@ def main(): if robotType == 'diffDrive': env = gym.make(envName, render=True, dt=myMPCPlanner.dt()) else: - env = gym.make(envName, render=True, dt=myMPCPlanner.dt(), n=myMPCPlanner.n()) + env = gym.make(envName, render=True, dt=myMPCPlanner.dt(), n=myMPCPlanner.dof()) limits = np.array([[-50, ] * n, [50, ] * n]) myMPCPlanner.setJointLimits(limits) q0 = np.random.random(n) diff --git a/robotmpcs/models/boxerMpcModel.py b/robotmpcs/models/boxerMpcModel.py index 82a0154..82c7646 100644 --- a/robotmpcs/models/boxerMpcModel.py +++ b/robotmpcs/models/boxerMpcModel.py @@ -4,8 +4,8 @@ class BoxerMpcModel(DiffDriveMpcModel): - def __init__(self, N): - super().__init__(2, N) + def __init__(self, time_horizon): + dim_goal = 2 + super().__init__(dim_goal, time_horizon) self._fk = FkCreator('boxer', 3).fk() self._modelName = 'boxer' - diff --git a/robotmpcs/models/diffDriveMpcModel.py b/robotmpcs/models/diffDriveMpcModel.py index 5d6010f..55c35b3 100644 --- a/robotmpcs/models/diffDriveMpcModel.py +++ b/robotmpcs/models/diffDriveMpcModel.py @@ -6,25 +6,25 @@ class DiffDriveMpcModel(MpcModel): - def __init__(self, m, N): - n = 3 - super().__init__(m, n, N, initParamMap=False) - self._nx = 8 # [x, y, theta, vel_rel0, vel_rel1, xdot, ydot, thetadot] - self._nu = 2 # [a_l, a_r] + def __init__(self, dim_goal, time_horizon): + dof = 3 + super().__init__(dim_goal, dof, time_horizon, initParamMap=False) + self._n_state = 8 # [x, y, theta, vel_rel0, vel_rel1, xdot, ydot, thetadot] + self._n_control_input = 2 # [a_l, a_r] self._limits = { - "x": {"low": np.ones(self._nx) * -100, "high": np.ones(self._nx) * 100}, - "u": {"low": np.ones(self._nu) * -100, "high": np.ones(self._nu) * 100}, + "x": {"low": np.ones(self._n_state) * -100, "high": np.ones(self._n_state) * 100}, + "u": {"low": np.ones(self._n_control_input) * -100, "high": np.ones(self._n_control_input) * 100}, "s": {"low": np.zeros(1), "high": np.ones(1) * np.inf}, } - self._fk = FkCreator('groundRobot', self._n).fk() + self._fk = FkCreator('groundRobot', self._n_dof).fk() self.initParamMap() self._modelName = "diffDrive" def extractVariables(self, z): - q = z[0: self._n] - qdot = z[self._n:2 * self._n] - vel_rel = z[2*self._n: 2*self._n + self._nu] - qddot = z[-self._nu:] + q = z[0: self._n_dof] + qdot = z[self._n_dof:2 * self._n_dof] + vel_rel = z[2 * self._n_dof: 2 * self._n_dof + self._n_control_input] + qddot = z[-self._n_control_input:] return q, qdot, qddot, vel_rel def eval_objectiveCommon(self, z, p): @@ -32,21 +32,21 @@ def eval_objectiveCommon(self, z, p): w = p[self._paramMap["w"]] wvel = p[self._paramMap["wvel"]] g = p[self._paramMap["g"]] - W = diagSX(w, self._m) - Wvel = diagSX(wvel, self._nu) - fk_ee = self._fk.fk(q, self._n, positionOnly=True)[0:self._m] + W = diagSX(w, self._dim_goal) + Wvel = diagSX(wvel, self._n_control_input) + fk_ee = self._fk.fk(q, self._n_dof, positionOnly=True)[0:self._dim_goal] Jvel = ca.dot(vel, ca.mtimes(Wvel, vel)) err = fk_ee - g Jx = ca.dot(err, ca.mtimes(W, err)) Jobst = 0 Js = 0 if self._obstaclesInCosts: - obstDistances = 1/ca.vcat(self.eval_obstacleDistances(z, p) ) + obstDistances = 1 / ca.vcat(self.eval_obstacleDistances(z, p)) wobst = ca.SX(np.ones(obstDistances.shape[0]) * p[self._paramMap['wobst']]) Wobst = diagSX(wobst, obstDistances.shape[0]) Jobst += ca.dot(obstDistances, ca.mtimes(Wobst, obstDistances)) - if self._ns > 0: - s = z[self._nx] + if self._n_slack > 0: + s = z[self._n_state] ws = p[self._paramMap["ws"]] Js += ws * s ** 2 return Jx, Jvel, Js, Jobst @@ -55,10 +55,10 @@ def computeXdot(self, x, vel): assert x.size() == (3, 1) assert vel.size() == (2, 1) xdot = ca.vertcat( - ca.cos(x[2]) * vel[0], - ca.sin(x[2]) * vel[0], - vel[1], - ) + ca.cos(x[2]) * vel[0], + ca.sin(x[2]) * vel[0], + vel[1], + ) return xdot def continuous_dynamics(self, x, u): @@ -70,4 +70,3 @@ def continuous_dynamics(self, x, u): xddot = ca.SX(np.zeros(3)) state_dot = ca.vertcat(xdot, xddot, veldot) return state_dot - diff --git a/robotmpcs/models/mpcModel.py b/robotmpcs/models/mpcModel.py index ff25c90..0002d4e 100644 --- a/robotmpcs/models/mpcModel.py +++ b/robotmpcs/models/mpcModel.py @@ -14,20 +14,20 @@ def diagSX(val, size): class MpcModel(object): - def __init__(self, m, n, N, initParamMap=True): - self._m = m - self._nx = 2 * n - self._nu = n - self._ns = 0 - self._n = n + def __init__(self, dim_goal, dof, time_horizon, initParamMap=True): + self._dim_goal = dim_goal + self._n_state = 2 * dof + self._n_control_input = dof + self._n_slack = 0 + self._n_dof = dof self._n_obst = 0 - self._m_obst = 0 + self._dim_obst = 0 self._pairs = [] - self._N = N + self._time_horizon = time_horizon if initParamMap: self._limits = { - "x": {"low": np.ones(self._nx) * -100, "high": np.ones(self._nx) * 100}, - "u": {"low": np.ones(self._nu) * -100, "high": np.ones(self._nu) * 100}, + "x": {"low": np.ones(self._n_state) * -100, "high": np.ones(self._n_state) * 100}, + "u": {"low": np.ones(self._n_control_input) * -100, "high": np.ones(self._n_control_input) * 100}, "s": {"low": np.zeros(1), "high": np.ones(1) * np.inf}, } self.initParamMap() @@ -35,22 +35,22 @@ def __init__(self, m, n, N, initParamMap=True): def initParamMap(self): self._paramMap = {} self._npar = 0 - self.addEntry2ParamMap("wu", self._nu) - self.addEntry2ParamMap("wvel", self._n) - self.addEntry2ParamMap("w", self._m) - if self._ns > 0: + self.addEntry2ParamMap("wu", self._n_control_input) + self.addEntry2ParamMap("wvel", self._n_dof) + self.addEntry2ParamMap("w", self._dim_goal) + if self._n_slack > 0: self.addEntry2ParamMap("ws", 1) - self.addEntry2ParamMap("g", self._m) + self.addEntry2ParamMap("g", self._dim_goal) self.addEntry2ParamMap("r_body", 1) - self.addEntry2ParamMap("lower_limits", self._n) - self.addEntry2ParamMap("upper_limits", self._n) + self.addEntry2ParamMap("lower_limits", self._n_dof) + self.addEntry2ParamMap("upper_limits", self._n_dof) def addEntry2ParamMap(self, name, n_par): self._paramMap[name] = list(range(self._npar, self._npar + n_par)) self._npar += n_par def setSlack(self): - self._ns = 1 + self._n_slack = 1 self.addEntry2ParamMap("ws", 1) def setSelfCollisionAvoidance(self, pairs): @@ -58,15 +58,15 @@ def setSelfCollisionAvoidance(self, pairs): def setObstacles(self, n_obst, m_obst, inCostFunction=False): self._n_obst = n_obst - self._m_obst = m_obst + self._dim_obst = m_obst self.addEntry2ParamMap("obst", (m_obst + 1) * n_obst) self._obstaclesInCosts = inCostFunction self.addEntry2ParamMap('wobst', 1) def extractVariables(self, z): - q = z[0: self._n] - qdot = z[self._n: self._nx] - qddot = z[self._nx + self._ns : self._nx + self._ns + self._nu] + q = z[0: self._n_dof] + qdot = z[self._n_dof: self._n_state] + qddot = z[self._n_state + self._n_slack: self._n_state + self._n_slack + self._n_control_input] return q, qdot, qddot def eval_objectiveCommon(self, z, p): @@ -74,9 +74,9 @@ def eval_objectiveCommon(self, z, p): w = p[self._paramMap["w"]] wvel = p[self._paramMap["wvel"]] g = p[self._paramMap["g"]] - W = diagSX(w, self._m) - Wvel = diagSX(wvel, self._nu) - fk_ee = self._fk.fk(q, self._n, positionOnly=True)[0:self._m] + W = diagSX(w, self._dim_goal) + Wvel = diagSX(wvel, self._n_control_input) + fk_ee = self._fk.fk(q, self._n_dof, positionOnly=True)[0:self._dim_goal] Jvel = ca.dot(qdot, ca.mtimes(Wvel, qdot)) err = fk_ee - g Jx = ca.dot(err, ca.mtimes(W, err)) @@ -87,8 +87,8 @@ def eval_objectiveCommon(self, z, p): wobst = ca.SX(np.ones(obstDistances.shape[0]) * p[self._paramMap['wobst']]) Wobst = diagSX(wobst, obstDistances.shape[0]) Jobst += ca.dot(obstDistances, ca.mtimes(Wobst, obstDistances)) - if self._ns > 0: - s = z[self._nx] + if self._n_slack > 0: + s = z[self._n_state] ws = p[self._paramMap["ws"]] Js += ws * s ** 2 return Jx, Jvel, Js, Jobst @@ -99,7 +99,7 @@ def eval_objectiveN(self, z, p): def eval_objective(self, z, p): wu = p[self._paramMap["wu"]] - Wu = diagSX(wu, self._nu) + Wu = diagSX(wu, self._n_control_input) Jx, Jvel, Js, Jobst = self.eval_objectiveCommon(z, p) _, _, qddot, *_ = self.extractVariables(z) Ju = ca.dot(qddot, ca.mtimes(Wu, qddot)) @@ -107,8 +107,8 @@ def eval_objective(self, z, p): def eval_inequalities(self, z, p): all_ineqs = self.eval_obstacleDistances(z, p) + self.eval_jointLimits(z, p) + self.eval_selfCollision(z, p) - if self._ns > 0: - s = z[self._nx] + if self._n_slack > 0: + s = z[self._n_state] for ineq in all_ineqs: ineq += s return all_ineqs @@ -116,19 +116,19 @@ def eval_inequalities(self, z, p): def eval_obstacleDistances(self, z, p): ineqs = [] q, *_ = self.extractVariables(z) - if self._ns > 0: - s = z[self._nx] + if self._n_slack > 0: + s = z[self._n_state] else: s = 0.0 if "obst" in self._paramMap.keys(): obsts = p[self._paramMap["obst"]] r_body = p[self._paramMap["r_body"]] - for j in range(self._n): - fk = self._fk.fk(q, j + 1, positionOnly=True)[0:self._m] + for j in range(self._n_dof): + fk = self._fk.fk(q, j + 1, positionOnly=True)[0:self._dim_goal] for i in range(self._n_obst): - obst = obsts[i * (self._m_obst + 1) : (i + 1) * (self._m_obst + 1)] - x = obst[0 : self._m_obst] - r = obst[self._m_obst] + obst = obsts[i * (self._dim_obst + 1): (i + 1) * (self._dim_obst + 1)] + x = obst[0 : self._dim_obst] + r = obst[self._dim_obst] dist = ca.norm_2(fk - x) ineqs.append(dist - r - r_body) return ineqs @@ -138,8 +138,8 @@ def eval_selfCollision(self, z, p): r_body = p[self._paramMap["r_body"]] ineqs = [] for pair in self._pairs: - fk1 = self._fk.fk(q, pair[0], positionOnly=True)[0: self._m] - fk2 = self._fk.fk(q, pair[1], positionOnly=True)[0: self._m] + fk1 = self._fk.fk(q, pair[0], positionOnly=True)[0: self._dim_goal] + fk2 = self._fk.fk(q, pair[1], positionOnly=True)[0: self._dim_goal] dist = ca.norm_2(fk1 - fk2) ineqs.append(dist - (2 * r_body)) return ineqs @@ -150,7 +150,7 @@ def eval_jointLimits(self, z, p): lower_limits = p[self._paramMap["lower_limits"]] upper_limits = p[self._paramMap["upper_limits"]] ineqs = [] - for j in range(self._n): + for j in range(self._n_dof): dist_lower = q[j] - lower_limits[j] dist_upper = upper_limits[j] - q[j] ineqs.append(dist_lower) @@ -161,8 +161,8 @@ def setLimits(self, limits): self._limits = limits def continuous_dynamics(self, x, u): - qdot = x[self._n: self._nx] - qddot = u[-self._nu:] + qdot = x[self._n_dof: self._n_state] + qddot = u[-self._n_control_input:] acc = ca.vertcat(qdot, qddot) return acc @@ -170,15 +170,15 @@ def setDt(self, dt): self._dt = dt def setModel(self): - self._model = forcespro.nlp.SymbolicModel(self._N) + self._model = forcespro.nlp.SymbolicModel(self._time_horizon) self._model.continuous_dynamics = self.continuous_dynamics self._model.objective = self.eval_objective self._model.objectiveN = self.eval_objectiveN E = np.concatenate( - [np.eye(self._nx), np.zeros((self._nx, self._nu + self._ns))], axis=1 + [np.eye(self._n_state), np.zeros((self._n_state, self._n_control_input + self._n_slack))], axis=1 ) self._model.E = E - if self._ns > 0: + if self._n_slack > 0: self._model.lb = np.concatenate( (self._limits["x"]["low"], self._limits["s"]["low"], self._limits["u"]["low"]) ) @@ -193,19 +193,19 @@ def setModel(self): (self._limits["x"]["high"], self._limits["u"]["high"]) ) self._model.npar = self._npar - self._model.nvar = self._nx + self._nu + self._ns - self._model.neq = self._nx - nbInequalities = self._n_obst * self._n + 2 * self._n + len(self._pairs) + self._model.nvar = self._n_state + self._n_control_input + self._n_slack + self._model.neq = self._n_state + nbInequalities = self._n_obst * self._n_dof + 2 * self._n_dof + len(self._pairs) self._model.nh = nbInequalities self._model.hu = np.ones(nbInequalities) * np.inf self._model.hl = np.zeros(nbInequalities) self._model.ineq = self.eval_inequalities - self._model.xinitidx = range(0, self._nx) + self._model.xinitidx = range(0, self._n_state) def setCodeoptions(self, **kwargs): debug = False - solverName = self._modelName + "_n" + str(self._n) + "_" + str(self._dt).replace('.','') + "_H" + str(self._N) - if self._ns == 0: + solverName = self._modelName + "_n" + str(self._n_dof) + "_" + str(self._dt).replace('.', '') + "_H" + str(self._time_horizon) + if self._n_slack == 0: solverName += "_noSlack" if debug in kwargs: debug = kwargs.get('debug') @@ -227,7 +227,7 @@ def generateSolver(self, location="./"): _ = self._model.generate_solver(self._codeoptions) with open(self._solverName + '/paramMap.yaml', 'w') as outfile: yaml.dump(self._paramMap, outfile, default_flow_style=False) - properties = {"nx": self._nx, "nu": self._nu, "npar": self._npar, "ns": self._ns} + properties = {"n_state": self._n_state, "n_control_input": self._n_control_input, "npar": self._npar, "n_slack": self._n_slack} with open(self._solverName + '/properties.yaml', 'w') as outfile: yaml.dump(properties, outfile, default_flow_style=False) move(self._solverName, location + self._solverName) diff --git a/robotmpcs/models/pandaMpcModel.py b/robotmpcs/models/pandaMpcModel.py index 52a90b4..10cb5fe 100644 --- a/robotmpcs/models/pandaMpcModel.py +++ b/robotmpcs/models/pandaMpcModel.py @@ -4,9 +4,9 @@ class PandaMpcModel(MpcModel): - def __init__(self, m, N): - n = 7 - super().__init__(m, n, N) - self._fk = FkCreator('panda', n).fk() + def __init__(self, dim_goal, time_horizon): + dof = 7 + super().__init__(dim_goal, dof, time_horizon) + self._fk = FkCreator('panda', dof).fk() self._modelName = 'panda' diff --git a/robotmpcs/models/planarArmMpcModel.py b/robotmpcs/models/planarArmMpcModel.py index 2ecd995..81265f4 100644 --- a/robotmpcs/models/planarArmMpcModel.py +++ b/robotmpcs/models/planarArmMpcModel.py @@ -4,8 +4,8 @@ class PlanarArmMpcModel(MpcModel): - def __init__(self, m, N, n): - super().__init__(m, n, N) - self._fk = FkCreator('planarArm', n).fk() + def __init__(self, dim_goal, dof, time_horizon): + super().__init__(dim_goal, dof, time_horizon) + self._fk = FkCreator('planarArm', dof).fk() self._modelName = "planarArm" diff --git a/robotmpcs/models/pointRobotMpcModel.py b/robotmpcs/models/pointRobotMpcModel.py index 162a15a..8020646 100644 --- a/robotmpcs/models/pointRobotMpcModel.py +++ b/robotmpcs/models/pointRobotMpcModel.py @@ -4,9 +4,9 @@ class PointRobotMpcModel(MpcModel): - def __init__(self, m, N): - n = 2 - super().__init__(m, n, N) - self._fk = FkCreator('pointMass', n).fk() + def __init__(self, dim_goal, time_horizon): + dof = 2 + super().__init__(dim_goal, dof, time_horizon) + self._fk = FkCreator('pointMass', dof).fk() self._modelName = 'pointRobot' diff --git a/robotmpcs/planner/mpcPlanner.py b/robotmpcs/planner/mpcPlanner.py index f403602..67e9f1d 100644 --- a/robotmpcs/planner/mpcPlanner.py +++ b/robotmpcs/planner/mpcPlanner.py @@ -35,13 +35,13 @@ class PlannerSettingIncomplete(Exception): class MPCPlanner(object): def __init__(self, setupFile, robotType, solversDir): - required_keys = ["type", "n", "obst", "weights", "interval", "H", "dt"] + required_keys = ["type", "dof", "obst", "weights", "interval", "time_horizon", "dt"] self._required_keys = required_keys self._setupFile = setupFile self._robotType = robotType self.parseSetup() """ - self._paramMap, self._npar, self._nx, self._nu, self._ns = getParameterMap( + self._paramMap, self._npar, self._n_state, self._n_control_input, self._n_slack = getParameterMap( self.n(), self.m(), self.nbObstacles(), self.m(), self.useSlack() ) """ @@ -49,11 +49,11 @@ def __init__(self, setupFile, robotType, solversDir): self._solverFile = ( solversDir + self._robotType - + "_n" + str(self.n()) + + "_n" + str(self.dof()) + "_" + dt_str + "_H" - + str(self.H()) + + str(self.timeHorizon()) ) if not self.useSlack(): self._solverFile += "_noSlack" @@ -69,9 +69,9 @@ def __init__(self, setupFile, robotType, solversDir): self._properties = yaml.safe_load(stream) except yaml.YAMLError as exc: print(exc) - self._nx = self._properties['nx'] - self._nu = self._properties['nu'] - self._ns = self._properties['ns'] + self._n_state = self._properties['n_state'] + self._n_control_input = self._properties['n_control_input'] + self._n_slack = self._properties['n_slack'] self._npar = self._properties['npar'] try: print("Loading solver %s" % self._solverFile) @@ -100,13 +100,13 @@ def plannerType(self): def reset(self): print("RESETTING PLANNER") - self._x0 = np.zeros(shape=(self.H(), self._nx + self._nu + self._ns)) - self._xinit = np.zeros(self._nx) + self._x0 = np.zeros(shape=(self.timeHorizon(), self._n_state + self._n_control_input + self._n_slack)) + self._xinit = np.zeros(self._n_state) if self.useSlack(): self._slack = 0.0 self._x0[-1, -1] = 0.1 - self._params = np.zeros(shape=(self._npar * self.H()), dtype=float) - for i in range(self.H()): + self._params = np.zeros(shape=(self._npar * self.timeHorizon()), dtype=float) + for i in range(self.timeHorizon()): self._params[ [self._npar * i + val for val in self._paramMap["w"]] ] = self.weights()["w"] @@ -125,7 +125,7 @@ def reset(self): [self._npar * i + val for val in self._paramMap["wobst"]] ] = self.weights()["wobst"] - def m(self): + def dimGoal(self): return 2 def interval(self): @@ -137,11 +137,11 @@ def useSlack(self): else: return True - def n(self): - return self._setup["n"] + def dof(self): + return self._setup["dof"] - def H(self): - return self._setup["H"] + def timeHorizon(self): + return self._setup["time_horizon"] def dt(self): return self._setup["dt"] @@ -160,7 +160,7 @@ def nbObstacles(self): def setObstacles(self, obsts, r_body): self._r = 0.1 - for i in range(self.H()): + for i in range(self.timeHorizon()): self._params[self._npar * i + self._paramMap["r_body"][0]] = r_body for j in range(self.nbObstacles()): if j < len(obsts): @@ -168,37 +168,37 @@ def setObstacles(self, obsts, r_body): else: obst = EmptyObstacle() for m_i in range(obst.dim()): - paramsIndexObstX = self._npar * i + self._paramMap['obst'][j * (self.m() + 1) + m_i] + paramsIndexObstX = self._npar * i + self._paramMap['obst'][j * (self.dimGoal() + 1) + m_i] self._params[paramsIndexObstX] = obst.position()[m_i] - paramsIndexObstR = self._npar * i + self._paramMap['obst'][j * (self.m() + 1) + self.m()] + paramsIndexObstR = self._npar * i + self._paramMap['obst'][j * (self.dimGoal() + 1) + self.dimGoal()] self._params[paramsIndexObstR] = obst.radius() def updateDynamicObstacles(self, obstArray): - nbDynamicObsts = int(obstArray.size / 3 / self.m()) + nbDynamicObsts = int(obstArray.size / 3 / self.dimGoal()) for j in range(self.nbObstacles()): if j < nbDynamicObsts: - obstPos = obstArray[:self.m()] - obstVel = obstArray[self.m():2*self.m()] - obstAcc = obstArray[2*self.m():3*self.m()] + obstPos = obstArray[:self.dimGoal()] + obstVel = obstArray[self.dimGoal():2 * self.dimGoal()] + obstAcc = obstArray[2 * self.dimGoal():3 * self.dimGoal()] else: - obstPos = np.ones(self.m()) * -100 - obstVel = np.zeros(self.m()) - obstAcc = np.zeros(self.m()) - for i in range(self.H()): - for m_i in range(self.m()): - paramsIndexObstX = self._npar * i + self._paramMap['obst'][j * (self.m() + 1) + m_i] + obstPos = np.ones(self.dimGoal()) * -100 + obstVel = np.zeros(self.dimGoal()) + obstAcc = np.zeros(self.dimGoal()) + for i in range(self.timeHorizon()): + for m_i in range(self.dimGoal()): + paramsIndexObstX = self._npar * i + self._paramMap['obst'][j * (self.dimGoal() + 1) + m_i] predictedPosition = obstPos[m_i] + obstVel[m_i] * self.dt() * i + 0.5 * (self.dt() * i)**2 * obstAcc[m_i] self._params[paramsIndexObstX] = predictedPosition - paramsIndexObstR = self._npar * i + self._paramMap['obst'][j * (self.m() + 1) + self.m()] + paramsIndexObstR = self._npar * i + self._paramMap['obst'][j * (self.dimGoal() + 1) + self.dimGoal()] self._params[paramsIndexObstR] = self._r def setSelfCollisionAvoidance(self, r_body): - for i in range(self.H()): + for i in range(self.timeHorizon()): self._params[self._npar * i + self._paramMap["r_body"][0]] = r_body def setJointLimits(self, limits): - for i in range(self.H()): - for j in range(self.n()): + for i in range(self.timeHorizon()): + for j in range(self.dof()): self._params[ self._npar * i + self._paramMap["lower_limits"][j] ] = limits[0][j] @@ -207,8 +207,8 @@ def setJointLimits(self, limits): ] = limits[1][j] def setGoal(self, goal): - for i in range(self.H()): - for j in range(self.m()): + for i in range(self.timeHorizon()): + for j in range(self.dimGoal()): self._params[self._npar * i + self._paramMap["g"][j]] = goal.position()[j] def concretize(self): @@ -216,36 +216,36 @@ def concretize(self): def shiftHorizon(self, output, ob): for key in output.keys(): - if self.H() < 10: + if self.timeHorizon() < 10: stage = int(key[-1:]) - elif self.H() >= 10 and self.H() < 100: + elif self.timeHorizon() >= 10 and self.timeHorizon() < 100: stage = int(key[-2:]) - elif self.H() >= 100: + elif self.timeHorizon() >= 100: stage = int(key[-3:]) if stage == 1: continue self._x0[stage - 2, 0 : len(output[key])] = output[key] def setX0(self, xinit): - for i in range(self.H()): - self._x0[i][0 : self._nx] = xinit + for i in range(self.timeHorizon()): + self._x0[i][0 : self._n_state] = xinit def solve(self, ob): - # print("Observation : " , ob[0:self._nx]) - self._xinit = ob[0 : self._nx] - if ob.size > self._nx: - self.updateDynamicObstacles(ob[self._nx:]) - action = np.zeros(self._nu) + # print("Observation : " , ob[0:self._n_state]) + self._xinit = ob[0 : self._n_state] + if ob.size > self._n_state: + self.updateDynamicObstacles(ob[self._n_state:]) + action = np.zeros(self._n_control_input) problem = {} problem["xinit"] = self._xinit - self._x0[0][0 : self._nx] = self._xinit + self._x0[0][0 : self._n_state] = self._xinit self.setX0(self._xinit) problem["x0"] = self._x0.flatten()[:] problem["all_parameters"] = self._params # debug debug = False if debug: - nbPar = int(len(self._params)/self.H()) + nbPar = int(len(self._params) / self.timeHorizon()) if self.useSlack(): z = np.concatenate((self._xinit, np.array([self._slack]))) else: @@ -266,19 +266,19 @@ def solve(self, ob): output, exitflag, info = self._solver.solve(problem) if exitflag < 0: print(exitflag) - if self.H() < 10: + if self.timeHorizon() < 10: key1 = 'x1' - elif self.H() >= 10 and self.H() < 100: + elif self.timeHorizon() >= 10 and self.timeHorizon() < 100: key1 = 'x01' - elif self.H() >= 100: + elif self.timeHorizon() >= 100: key1 = 'x001' - action = output[key1][-self._nu :] + action = output[key1][-self._n_control_input:] if self.useSlack(): - self._slack = output[key1][self._nx] + self._slack = output[key1][self._n_state] if self._slack > 1e-3: print("slack : ", self._slack) # print('action : ', action) - # print("prediction : ", output["x02"][0:self._nx]) + # print("prediction : ", output["x02"][0:self._n_state]) self.shiftHorizon(output, ob) return action, info