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 CartesianBulletControllerPlugin(RobotPlugin): """ Instantaneous controller that can do joint/cartesian movements while avoiding collisions. """ def __init__(self, root_link, js_identifier, fk_identifier, goal_identifier, next_cmd_identifier, collision_identifier, closest_point_identifier, controlled_joints_identifier, controllable_links_identifier, robot_description_identifier, collision_goal_identifier, pyfunction_identifier, path_to_functions, nWSR, default_joint_vel_limit): """ :param root_link: the robots root link :type root_link: str :type js_identifier: str :type fk_identifier: str :type goal_identifier: str :type next_cmd_identifier: str :type collision_identifier: str :type closest_point_identifier: str :type controlled_joints_identifier: str :type controllable_links_identifier: str :type robot_description_identifier: str :type collision_goal_identifier: str :type pyfunction_identifier: str :param path_to_functions: path to folder where compiled functions and the self collision matrix are stored :type path_to_functions: str :param nWSR: magic number for QP solver. has to be big for difficult problems. choose None if you're like wtf. :type nWSR: Union[int, None] :param default_joint_vel_limit: caps the joint velocities defined in the urdf. :type default_joint_vel_limit: float """ self.collision_goal_identifier = collision_goal_identifier self.controlled_joints_identifier = controlled_joints_identifier self._pyfunctions_identifier = pyfunction_identifier self._fk_identifier = fk_identifier self._collision_identifier = collision_identifier self._closest_point_identifier = closest_point_identifier self.path_to_functions = path_to_functions self.nWSR = nWSR self.default_joint_vel_limit = default_joint_vel_limit self.root = root_link self._joint_states_identifier = js_identifier self._goal_identifier = goal_identifier self._next_cmd_identifier = next_cmd_identifier self.controllable_links_identifier = controllable_links_identifier self.controller = None self.known_constraints = set() self.controlled_joints = set() self.max_number_collision_entries = 10 self.robot = None self.used_joints = set() self.controllable_links = set() super(CartesianBulletControllerPlugin, self).__init__(robot_description_identifier, self._joint_states_identifier, self.default_joint_vel_limit) def copy(self): cp = self.__class__( self.root, self._joint_states_identifier, self._fk_identifier, self._goal_identifier, self._next_cmd_identifier, self._collision_identifier, self._closest_point_identifier, self.controlled_joints_identifier, self.controllable_links_identifier, self._robot_description_identifier, self.collision_goal_identifier, self._pyfunctions_identifier, self.path_to_functions, self.nWSR, self.default_joint_vel_limit) cp.controller = self.controller cp.robot = self.robot cp.known_constraints = self.known_constraints cp.controlled_joints = self.controlled_joints return cp def start_always(self): super(CartesianBulletControllerPlugin, self).start_always() self.next_cmd = {} self.update_controlled_joints_and_links() if self.was_urdf_updated(): self.init_controller() self.add_js_controller_soft_constraints() self.add_collision_avoidance_soft_constraints() self.add_cart_controller_soft_constraints() self.set_unused_joint_goals_to_current() def update(self): expr = self.god_map.get_symbol_map() next_cmd = self.controller.get_cmd(expr, self.nWSR) self.next_cmd.update(next_cmd) self.god_map.set_data([self._next_cmd_identifier], self.next_cmd) def update_controlled_joints_and_links(self): """ Gets controlled joints from god map and uses this to calculate the controllable link, which are written to the god map. """ self.controlled_joints = self.god_map.get_data( [self.controlled_joints_identifier]) self.controllable_links = set() for joint_name in self.controlled_joints: self.controllable_links.update( self.get_robot().get_sub_tree_link_names_with_collision( joint_name)) self.god_map.set_data([self.controllable_links_identifier], self.controllable_links) def init_controller(self): self.controller = SymEngineController(self.robot, self.path_to_functions) self.controller.set_controlled_joints(self.controlled_joints) def set_unused_joint_goals_to_current(self): """ Sets the goal for all joints which are not used in another goal to their current position. """ joint_goal = self.god_map.get_data( [self._goal_identifier, str(Controller.JOINT)]) for joint_name in self.controlled_joints: if joint_name not in joint_goal: joint_goal[joint_name] = { u'weight': 0.0, u'p_gain': 10, u'max_speed': self.get_robot().default_joint_velocity_limit, u'position': self.god_map.get_data([ self._joint_states_identifier, joint_name, u'position' ]) } if joint_name not in self.used_joints: joint_goal[joint_name][u'weight'] = 1 self.god_map.set_data([self._goal_identifier, str(Controller.JOINT)], joint_goal) def get_expr_joint_current_position(self, joint_name): """ :type joint_name: str :rtype: sw.Symbol """ key = [self._joint_states_identifier, joint_name, u'position'] return self.god_map.to_symbol(key) def get_expr_joint_goal_position(self, joint_name): """ :type joint_name: str :rtype: sw.Symbol """ key = [ self._goal_identifier, str(Controller.JOINT), joint_name, u'position' ] return self.god_map.to_symbol(key) def get_expr_joint_goal_weight(self, joint_name): """ :type joint_name: str :rtype: sw.Symbol """ weight_key = [ self._goal_identifier, str(Controller.JOINT), joint_name, u'weight' ] return self.god_map.to_symbol(weight_key) def get_expr_joint_goal_gain(self, joint_name): """ :type joint_name: str :rtype: sw.Symbol """ gain_key = [ self._goal_identifier, str(Controller.JOINT), joint_name, u'p_gain' ] return self.god_map.to_symbol(gain_key) def get_expr_joint_goal_max_speed(self, joint_name): """ :type joint_name: str :rtype: sw.Symbol """ max_speed_key = [ self._goal_identifier, str(Controller.JOINT), joint_name, u'max_speed' ] return self.god_map.to_symbol(max_speed_key) def get_expr_joint_distance_to_goal(self, joint_name): """ :type joint_name: str :rtype: ShortestAngularDistanceInput """ current_joint_key = [ self._joint_states_identifier, joint_name, u'position' ] goal_joint_key = [ self._goal_identifier, str(Controller.JOINT), joint_name, u'position' ] return ShortestAngularDistanceInput(self.god_map.to_symbol, [self._pyfunctions_identifier], current_joint_key, goal_joint_key) def add_js_controller_soft_constraints(self): """ to self.controller and saves functions for continuous joints in god map. """ pyfunctions = {} for joint_name in self.controlled_joints: joint_current_expr = self.get_expr_joint_current_position( joint_name) goal_joint_expr = self.get_expr_joint_goal_position(joint_name) weight_expr = self.get_expr_joint_goal_weight(joint_name) gain_expr = self.get_expr_joint_goal_gain(joint_name) max_speed_expr = self.get_expr_joint_goal_max_speed(joint_name) if self.get_robot().is_joint_continuous(joint_name): change = self.get_expr_joint_distance_to_goal(joint_name) pyfunctions[change.get_key()] = change soft_constraints = continuous_joint_position( joint_current_expr, change.get_expression(), weight_expr, gain_expr, max_speed_expr, joint_name) self.controller.update_soft_constraints( soft_constraints, self.god_map.get_registered_symbols()) else: soft_constraints = joint_position(joint_current_expr, goal_joint_expr, weight_expr, gain_expr, max_speed_expr, joint_name) self.controller.update_soft_constraints( soft_constraints, self.god_map.get_registered_symbols()) self.god_map.set_data([self._pyfunctions_identifier], pyfunctions) def add_collision_avoidance_soft_constraints(self): """ Adds a constraint for each link that pushed it away from its closest point. """ soft_constraints = {} for link in list(self.controllable_links): point_on_link_input = Point3Input( self.god_map.to_symbol, prefix=[ self._closest_point_identifier, link, u'position_on_a' ]) other_point_input = Point3Input(self.god_map.to_symbol, prefix=[ self._closest_point_identifier, link, u'position_on_b' ]) current_input = FrameInput(self.god_map.to_symbol, translation_prefix=[ self._fk_identifier, (self.root, link), u'pose', u'position' ], rotation_prefix=[ self._fk_identifier, (self.root, link), u'pose', u'orientation' ]) min_dist = self.god_map.to_symbol( [self._closest_point_identifier, link, u'min_dist']) contact_normal = Vector3Input(self.god_map.to_symbol, prefix=[ self._closest_point_identifier, link, u'contact_normal' ]) soft_constraints.update( link_to_link_avoidance( link, self.get_robot().get_fk_expression(self.root, link), current_input.get_frame(), point_on_link_input.get_expression(), other_point_input.get_expression(), contact_normal.get_expression(), min_dist)) self.controller.update_soft_constraints( soft_constraints, self.god_map.get_registered_symbols()) def add_cart_controller_soft_constraints(self): """ Adds cart controller constraints for each goal. """ print(u'used chains:') for t in [Controller.TRANSLATION_3D, Controller.ROTATION_3D]: for (root, tip), value in self.god_map.get_data( [self._goal_identifier, str(t)]).items(): self.used_joints.update( self.get_robot().get_joint_names_from_chain_controllable( root, tip)) print(u'{} -> {} type: {}'.format(root, tip, t)) self.controller.update_soft_constraints( self.cart_goal_to_soft_constraints(root, tip, t), self.god_map.get_registered_symbols()) def cart_goal_to_soft_constraints(self, root, tip, type): """ :type root: str :type tip: str :param type: as defined in Controller msg :type type: int :rtype: dict """ # TODO split this into 2 functions, for translation and rotation goal_input = FrameInput(self.god_map.to_symbol, translation_prefix=[ self._goal_identifier, str(Controller.TRANSLATION_3D), (root, tip), u'goal_pose', u'pose', u'position' ], rotation_prefix=[ self._goal_identifier, str(Controller.ROTATION_3D), (root, tip), u'goal_pose', u'pose', u'orientation' ]) current_input = FrameInput(self.god_map.to_symbol, translation_prefix=[ self._fk_identifier, (root, tip), u'pose', u'position' ], rotation_prefix=[ self._fk_identifier, (root, tip), u'pose', u'orientation' ]) weight_key = [self._goal_identifier, str(type), (root, tip), u'weight'] weight = self.god_map.to_symbol(weight_key) p_gain_key = [self._goal_identifier, str(type), (root, tip), u'p_gain'] p_gain = self.god_map.to_symbol(p_gain_key) max_speed_key = [ self._goal_identifier, str(type), (root, tip), u'max_speed' ] max_speed = self.god_map.to_symbol(max_speed_key) if type == Controller.TRANSLATION_3D: return position_conv( goal_input.get_position(), sw.position_of(self.get_robot().get_fk_expression(root, tip)), weights=weight, trans_gain=p_gain, max_trans_speed=max_speed, ns=u'{}/{}'.format(root, tip)) elif type == Controller.ROTATION_3D: return rotation_conv( goal_input.get_rotation(), sw.rotation_of(self.get_robot().get_fk_expression(root, tip)), current_input.get_rotation(), weights=weight, rot_gain=p_gain, max_rot_speed=max_speed, ns=u'{}/{}'.format(root, tip)) return {}