def compile(self):
a = ''.join(str(x) for x in sorted(chain([(x,) for x in self.soft_constraints.keys()],
self.hard_constraints.keys(),
self.joint_constraints.keys())))
function_hash = hashlib.md5((a + self.robot.get_urdf_str()).encode('utf-8')).hexdigest()
path_to_functions = self.path_to_functions + function_hash
self.qp_problem_builder = QProblemBuilder(self.joint_constraints,
self.hard_constraints,
self.soft_constraints,
list(self.joint_to_symbols_str.values()),
path_to_functions)
def compile(self):
a = ''.join(
str(x) for x in sorted(
chain(self.soft_constraints.keys(), self.hard_constraints.keys(
), self.joint_constraints.keys())))
function_hash = hashlib.md5(a + self.robot.get_hash()).hexdigest()
path_to_functions = self.path_to_functions + function_hash
self.qp_problem_builder = QProblemBuilder(
self.joint_constraints,
self.hard_constraints, self.soft_constraints,
self.joint_to_symbols_str.values(), self.free_symbols,
path_to_functions)
class QPController(Controller):
def __init__(self, robot, builder_backend=None, logging=print_wrapper):
self.builder_backend = builder_backend
self.logging = logging
super(QPController, self).__init__(robot)
def init(self):
self._controllable_constraints = OrderedDict()
self._hard_constraints = OrderedDict()
self._soft_constraints = OrderedDict()
self.add_inputs(self.get_robot())
self.make_constraints(self.get_robot())
self.build_builder()
def add_inputs(self, robot):
raise (NotImplementedError)
def make_constraints(self, robot):
self._controllable_constraints = self.get_robot().joint_constraints
self._hard_constraints = self.get_robot().hard_constraints
def build_builder(self):
self.qp_problem_builder = QProblemBuilder(
self._controllable_constraints, self._hard_constraints,
self._soft_constraints, self.builder_backend, self.logging)
def get_next_command(self):
# TODO add dt parameter and return next state + cmds instead of only cmds
self.update_observables(self.get_robot().get_state())
return self.qp_problem_builder.update_observables(self.get_state())
def build_builder(self):
self.qp_problem_builder = QProblemBuilder(
self._controllable_constraints, self._hard_constraints,
self._soft_constraints, self.builder_backend, self.logging)
def test_differentiation_speed_cython(self):
builder = QProblemBuilder(self.robot.joint_constraints,
self.robot.hard_constraints, self.s_dict)
class InstantaneousController(object):
"""
This class handles constraints and computes joint commands using symengine and qpOases.
"""
# TODO should anybody who uses this class know about constraints?
def __init__(self, robot, path_to_functions):
"""
:type robot: Robot
:param path_to_functions: location where compiled functions are stored
:type: str
"""
self.path_to_functions = path_to_functions
self.robot = robot
self.controlled_joints = []
self.hard_constraints = {}
self.joint_constraints = {}
self.soft_constraints = {}
self.free_symbols = None
self.qp_problem_builder = None
def set_controlled_joints(self, joint_names):
"""
:type joint_names: set
"""
self.controlled_joints = joint_names
self.joint_to_symbols_str = OrderedDict(
(x, self.robot.get_joint_position_symbol(x))
for x in self.controlled_joints)
self.joint_constraints = OrderedDict(
((self.robot.get_name(), k), self.robot._joint_constraints[k])
for k in self.controlled_joints)
self.hard_constraints = OrderedDict(
((self.robot.get_name(), k), self.robot._hard_constraints[k])
for k in self.controlled_joints
if k in self.robot._hard_constraints)
def get_qpdata_key_map(self):
b_keys = []
weights_keys = []
xdot_keys = []
bA_keys = []
for iJ, k in enumerate(self.joint_constraints.keys()):
key = u'j -- ' + str(k)
b_keys.append(key)
weights_keys.append(key)
xdot_keys.append(key)
for iH, k in enumerate(self.hard_constraints.keys()):
key = u'h -- ' + str(k)
bA_keys.append(key)
for iS, k in enumerate(self.soft_constraints.keys()):
key = str(k)
bA_keys.append(key)
weights_keys.append(key)
xdot_keys.append(key)
return weights_keys, b_keys, bA_keys, xdot_keys
def update_soft_constraints(self, soft_constraints, free_symbols=None):
"""
Triggers a recompile if the number of soft constraints has changed.
:type soft_constraints: dict
:type free_symbols: set
"""
if free_symbols is not None:
warnings.warn(u'use of free_symbols deprecated',
DeprecationWarning)
# TODO bug if soft constraints get replaced, actual amount does not change.
last_number_of_constraints = len(self.soft_constraints)
if free_symbols is not None:
if self.free_symbols is None:
self.free_symbols = set()
self.free_symbols.update(free_symbols)
self.soft_constraints.update(soft_constraints)
if last_number_of_constraints != len(self.soft_constraints):
self.qp_problem_builder = None
def compile(self):
a = ''.join(
str(x) for x in sorted(
chain(self.soft_constraints.keys(), self.hard_constraints.keys(
), self.joint_constraints.keys())))
function_hash = hashlib.md5(a + self.robot.get_urdf_str()).hexdigest()
path_to_functions = self.path_to_functions + function_hash
self.qp_problem_builder = QProblemBuilder(
self.joint_constraints,
self.hard_constraints, self.soft_constraints,
self.joint_to_symbols_str.values(), self.free_symbols,
path_to_functions)
def get_cmd(self, substitutions, nWSR=None):
"""
Computes joint commands that satisfy constrains given substitutions.
:param substitutions: maps symbol names as str to floats.
:type substitutions: dict
:param nWSR: magic number, if None throws errors, increase this until it stops.
:type nWSR: int
:return: maps joint names to command
:rtype: dict
"""
next_cmd, H, A, lb, ub, lbA, ubA, xdot_full = self.qp_problem_builder.get_cmd(
substitutions, nWSR)
if next_cmd is None:
pass
return {name: next_cmd[symbol] for name, symbol in self.joint_to_symbols_str.items()}, \
H, A, lb, ub, lbA, ubA, xdot_full
def get_expr(self):
return self.qp_problem_builder.get_expr()
class SymEngineController(object):
"""
This class handles constraints and computes joint commands using symengine and qpOases.
"""
# TODO should anybody how uses this card know about constrains?
def __init__(self, robot, path_to_functions):
"""
:type robot: Robot
:param path_to_functions: location where compiled functions are stored
:type: str
"""
self.path_to_functions = path_to_functions
self.robot = robot
self.controlled_joints = []
self.hard_constraints = {}
self.joint_constraints = {}
self.soft_constraints = {}
self.free_symbols = set()
self.qp_problem_builder = None
def set_controlled_joints(self, joint_names):
"""
:type joint_names: set
"""
self.controlled_joints = joint_names
self.joint_to_symbols_str = OrderedDict(
(x, self.robot.get_joint_symbol(x))
for x in self.controlled_joints)
self.joint_constraints = OrderedDict(
((self.robot.get_name(), k), self.robot.joint_constraints[k])
for k in self.controlled_joints)
self.hard_constraints = OrderedDict(
((self.robot.get_name(), k), self.robot.hard_constraints[k])
for k in self.controlled_joints
if k in self.robot.hard_constraints)
def update_soft_constraints(self, soft_constraints, free_symbols):
"""
Triggers a recompile if the number of soft constraints has changed.
:type soft_constraints: dict
:type free_symbols: set
"""
# TODO bug if soft constraints get replaced, actual amount does not change.
last_number_of_constraints = len(self.soft_constraints)
self.free_symbols.update(free_symbols)
self.soft_constraints.update(soft_constraints)
if last_number_of_constraints != len(self.soft_constraints):
self.qp_problem_builder = None
def compile(self):
a = ''.join(
str(x) for x in sorted(
chain(self.soft_constraints.keys(), self.hard_constraints.keys(
), self.joint_constraints.keys())))
function_hash = hashlib.md5(a + self.robot.get_hash()).hexdigest()
path_to_functions = self.path_to_functions + function_hash
self.qp_problem_builder = QProblemBuilder(
self.joint_constraints,
self.hard_constraints, self.soft_constraints,
self.joint_to_symbols_str.values(), self.free_symbols,
path_to_functions)
def get_cmd(self, substitutions, nWSR=None):
"""
Computes joint commands that satisfy constrains given substitutions.
:param substitutions: maps symbol names as str to floats.
:type substitutions: dict
:param nWSR: magic number, if None throws errors, increase this until it stops.
:type nWSR: int
:return: maps joint names to command
:rtype: dict
"""
try:
next_cmd = self.qp_problem_builder.get_cmd(substitutions, nWSR)
if next_cmd is None:
pass
return {
name: next_cmd[symbol]
for name, symbol in self.joint_to_symbols_str.items()
}
except AttributeError:
self.compile()
return self.get_cmd(substitutions, nWSR)