class ControllerPlugin(GiskardBehavior):
def __init__(self, name):
super(ControllerPlugin, self).__init__(name)
self.path_to_functions = self.get_god_map().safe_get_data(
identifier.data_folder)
self.nWSR = self.get_god_map().safe_get_data(identifier.nWSR)
self.soft_constraints = None
self.qp_data = {}
self.get_god_map().safe_set_data(
identifier.qp_data,
self.qp_data) # safe dict on godmap and work on ref
def initialise(self):
super(ControllerPlugin, self).initialise()
self.init_controller()
def setup(self, timeout=0.0):
return super(ControllerPlugin, self).setup(5.0)
def init_controller(self):
new_soft_constraints = self.get_god_map().safe_get_data(
identifier.soft_constraint_identifier)
if self.soft_constraints is None or set(
self.soft_constraints.keys()) != set(
new_soft_constraints.keys()):
self.soft_constraints = copy(new_soft_constraints)
self.controller = SymEngineController(
self.get_robot(),
u'{}/{}/'.format(self.path_to_functions,
self.get_robot().get_name()),
self.get_god_map().safe_get_data(identifier.symengine_backend),
self.get_god_map().safe_get_data(
identifier.symengine_opt_level))
self.controller.set_controlled_joints(
self.get_robot().controlled_joints)
self.controller.update_soft_constraints(self.soft_constraints)
# p = Process(target=self.controller.compile)
# p.start()
# while p.is_alive():
# sleep(0.05)
# p.join()
self.controller.compile()
self.qp_data[identifier.weight_keys[-1]], \
self.qp_data[identifier.b_keys[-1]], \
self.qp_data[identifier.bA_keys[-1]], \
self.qp_data[identifier.xdot_keys[-1]] = self.controller.get_qpdata_key_map()
def update(self):
last_cmd = self.get_god_map().safe_get_data(identifier.cmd)
self.get_god_map().safe_set_data(identifier.last_cmd, last_cmd)
expr = self.controller.get_expr()
expr = self.god_map.get_values(expr)
next_cmd, \
self.qp_data[identifier.H[-1]], \
self.qp_data[identifier.A[-1]], \
self.qp_data[identifier.lb[-1]], \
self.qp_data[identifier.ub[-1]], \
self.qp_data[identifier.lbA[-1]], \
self.qp_data[identifier.ubA[-1]], \
self.qp_data[identifier.xdot_full[-1]] = self.controller.get_cmd(expr, self.nWSR)
self.get_god_map().safe_set_data(identifier.cmd, next_cmd)
return Status.RUNNING
class TestController(object):
"""docstring for TestController"""
def __init__(self, urdf_string):
super(TestController, self).__init__()
# Roboter aus URDF erzeugen
self.robot = Robot(urdf_string)
self.robot.parse_urdf()
# Regler fuer den Roboter anlegen
self.controller = SymEngineController(self.robot, '.controller_cache/')
# Transformation vom Greifer zur Basis des Roboters erzeugen
gripper_pose = self.robot.get_fk_expression('base_link',
'hand_palm_link')
# Parameterisierbaren Punkt erzeugen
self.point_input = Point3Input(to_expr, 'goal')
# Mathematischen Punkt vom Punkt-Input erzeugen lassen
goal_point = self.point_input.get_expression()
# Distanz zwischen Zielpunkt und Greiferposition
d = norm(goal_point - position_of(gripper_pose))
a = dot(unitX, gripper_pose * unitZ)
c_dist_angular = SoftConstraint(1 - a, 1 - a, 1, a)
# Constraint, der die Distanz Richtung 0 zwingt
c_dist = SoftConstraint(-d, -d, 1, d)
free_symbols = configure_controller(self.controller, self.robot, {
'dist': c_dist,
'dist_angular': c_dist_angular
})
# Dictionary fuer die derzeitige Variablenbelegung. Initial sind alle Variablen 0
self.cur_subs = {s: 0 for s in free_symbols}
# Publisher fuer Kommandos
self.pub_cmd = rospy.Publisher('/hsr/commands/joint_velocities',
JointStateMsg,
queue_size=1)
# Abonent fuer Zielpunkt
self.sub_point = rospy.Subscriber('/goal_point',
PointMsg,
self.cb_point,
queue_size=1)
# Abonent fuer Gelenkpositionen
self.sub_js = rospy.Subscriber('/hsr/joint_states',
JointStateMsg,
self.cb_js,
queue_size=1)
def cb_point(self, msg):
# Werte der Nachricht in Variablenbelegung uebertragen
self.cur_subs[self.point_input.x] = msg.x
self.cur_subs[self.point_input.y] = msg.y
self.cur_subs[self.point_input.z] = msg.z
def cb_js(self, msg):
# Map von Jointnamen auf Variablen
robot_joint_symbols = self.robot.joint_to_symbol_map
# ueber alle Felder iterieren
for x in range(len(msg.name)):
# Wenn Joint tatsaechlich im Roboter definiert ist
if msg.name[x] in robot_joint_symbols:
# Variablenwert aktualisieren mit neuer Position
self.cur_subs[robot_joint_symbols[
msg.name[x]]] = msg.position[x]
# Kommandos generieren lassen. Variablenbelegung muss als {str: float} erfolgen.
cmd = self.controller.get_cmd(
{str(s): p
for s, p in self.cur_subs.items()})
# Kommandonachricht anlegen
cmd_msg = JointStateMsg()
# Kommandozeitstempel setzen
cmd_msg.header.stamp = rospy.Time.now()
# Positionen und Kraefte auf 0 setzen
cmd_msg.position = [0] * len(cmd)
cmd_msg.effort = [0] * len(cmd)
# Kommando in Nachricht schreiben
for j, v in cmd.items():
cmd_msg.name.append(j)
cmd_msg.velocity.append(v)
# Kommando abschicken
self.pub_cmd.publish(cmd_msg)