def update_rviz_markers(self, req): l = req.object_names res = UpdateRvizMarkersResponse() res.error_codes = UpdateRvizMarkersResponse.SUCCESS ma = MarkerArray() if len(l) == 0: l = self.get_world().get_object_names() for name in l: try: m = self.get_world().get_object(name).as_marker_msg() m.action = m.ADD m.header.frame_id = 'map' m.ns = u'world' + m.ns ma.markers.append(m) except TypeError as e: logging.logerr( 'failed to convert object {} to marker: {}'.format( name, str(e))) res.error_codes = UpdateRvizMarkersResponse.MARKER_CONVERSION_ERROR except KeyError as e: logging.logerr( 'could not update rviz marker for object {}: no object with that name was found' .format(name)) res.error_codes = UpdateRvizMarkersResponse.NAME_NOT_FOUND_ERROR self.pub_collision_marker.publish(ma) return res
def are_joint_limits_violated(self, p_lb, p_ub): violations = (p_ub - p_lb)[p_lb.data > p_ub.data] if len(violations) > 0: logging.logerr(u'The following joints are outside of their limits: \n {}'.format(violations)) return True logging.loginfo(u'All joints are within limits') return False
def is_nan_in_array(self, name, p_array): p_filtered = p_array.apply(lambda x: zip(x.index[x.isnull()].tolist(), x[x.isnull()]), 1) p_filtered = p_filtered[p_filtered.apply(lambda x: len(x)) > 0] if len(p_filtered) > 0: logging.logerr(u'{} has the following nans:'.format(name)) self.print_pandas_array(p_filtered) return True logging.loginfo(u'{} has no nans'.format(name)) return False
def load_compiled_function(file_name): if os.path.isfile(file_name): try: with open(file_name, u'r') as file: fast_f = pickle.load(file) return fast_f except EOFError as e: os.remove(file_name) logging.logerr(u'{} deleted because it was corrupted'.format(file_name))
def convert_dae_to_obj(path): path = path.replace(u'\'', u'') file_name = path.split(u'/')[-1] name, file_format = file_name.split(u'.') if u'dae' in file_format: input_path = resolve_ros_iris(path) new_path = u'/tmp/giskardpy/{}.obj'.format(name) create_path(new_path) try: subprocess.check_call( [u'meshlabserver', u'-i', input_path, u'-o', new_path]) except Exception as e: logging.logerr(u'meshlab not installed, can\'t convert dae to obj') return new_path return path
def get_object_info(self, req): res = GetObjectInfoResponse() res.error_codes = GetObjectInfoResponse.SUCCESS try: object = self.get_world().get_object(req.object_name) res.joint_state_topic = '' if req.object_name in self.object_js_subs.keys(): res.joint_state_topic = self.object_js_subs[req.object_name].name res.pose.pose = object.base_pose res.pose.header.frame_id = self.get_god_map().get_data(identifier.map_frame) for key, value in object.joint_state.items(): res.joint_state.name.append(key) res.joint_state.position.append(value.position) res.joint_state.velocity.append(value.velocity) res.joint_state.effort.append(value.effort) except KeyError as e: logging.logerr('no object with the name {} was found'.format(req.object_name)) res.error_codes = GetObjectInfoResponse.NAME_NOT_FOUND_ERROR return res
def __init__(self): self.action_clients = [] self.joint_names = [] while not rospy.has_param('~state_topics'): logging.loginfo('waiting for param ' + '~state_topics') rospy.sleep(1) while not rospy.has_param('~client_topics'): logging.loginfo('waiting for param ' + '~client_topics') rospy.sleep(1) self.state_topics = rospy.get_param('~state_topics', []) self.client_topics = rospy.get_param('~client_topics', []) self.number_of_clients = len(self.state_topics) if self.number_of_clients != len(self.client_topics): logging.logerr('number of state and action topics do not match') exit() if self.number_of_clients == 0: logging.logerr( 'the state_topic or client_topic parameter is empty') exit() self.client_type = [] for i in range(self.number_of_clients): waiting_for_topic = True while waiting_for_topic: try: rospy.wait_for_message(self.state_topics[i], AnyMsg, timeout=10) waiting_for_topic = False type = rostopic.get_info_text(self.client_topics[i] + '/goal').split('\n')[0][6:] self.client_type.append(type) except rostopic.ROSTopicException as e: logging.logerr( 'Exception: {} .Maybe unknown topic \'{}/goal\' \nmissing / in front of topic name?' .format(e, self.client_topics[i])) exit() except rospy.ROSException as e: if e.message == 'rospy shutdown': exit() logging.loginfo('waiting for state topic {}'.format( self.state_topics[i])) self.state_type = [] for i in range(self.number_of_clients): try: type = rostopic.get_info_text( self.state_topics[i]).split('\n')[0][6:] self.state_type.append(type) except rostopic.ROSTopicException: logging.logerr( 'unknown topic \'{}/goal\' \nmissing / in front of topic name?' .format(self.state_topics[i])) exit() for i in range(self.number_of_clients): if self.client_type[ i] == 'control_msgs/FollowJointTrajectoryActionGoal': self.action_clients.append( actionlib.SimpleActionClient( self.client_topics[i], control_msgs.msg.FollowJointTrajectoryAction)) elif self.client_type[ i] == 'pr2_controllers_msgs/JointTrajectoryActionGoal': self.action_clients.append( actionlib.SimpleActionClient( self.client_topics[i], pr2_controllers_msgs.msg.JointTrajectoryAction)) else: logging.logerr( 'wrong client topic type:' + self.client_type[i] + '\nmust be either control_msgs/FollowJointTrajectoryActionGoal or pr2_controllers_msgs/JointTrajectoryActionGoal' ) exit() self.joint_names.append( rospy.wait_for_message( self.state_topics[i], control_msgs.msg. JointTrajectoryControllerState).joint_names) logging.loginfo('connected to {}'.format(self.client_topics[i])) self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState( ) total_number_joints = 0 for joint_name_list in self.joint_names: total_number_joints += len(joint_name_list) self.current_controller_state.joint_names.extend(joint_name_list) self.current_controller_state.desired.positions = [ 0 for i in range(total_number_joints) ] self.current_controller_state.desired.accelerations = [ 0 for i in range(total_number_joints) ] self.current_controller_state.desired.effort = [ 0 for i in range(total_number_joints) ] self.current_controller_state.desired.velocities = [ 0 for i in range(total_number_joints) ] self.current_controller_state.actual = copy.deepcopy( self.current_controller_state.desired) self.current_controller_state.error = copy.deepcopy( self.current_controller_state.desired) self.state_pub = rospy.Publisher( '/whole_body_controller/state', control_msgs.msg.JointTrajectoryControllerState, queue_size=10) for i in range(self.number_of_clients): if self.state_type[ i] == 'control_msgs/JointTrajectoryControllerState': rospy.Subscriber( self.state_topics[i], control_msgs.msg.JointTrajectoryControllerState, self.state_cb_update) elif self.state_type[ i] == 'pr2_controllers_msgs/JointTrajectoryControllerState': rospy.Subscriber( self.state_topics[i], pr2_controllers_msgs.msg.JointTrajectoryControllerState, self.state_cb_update) else: logging.logerr( 'wrong state topic type ' + self.state_type[i] + '\nmust be either control_msgs/JointTrajectoryControllerState or pr2_controllers_msgs/JointTrajectoryControllerState' ) exit() rospy.Subscriber(self.state_topics[0], control_msgs.msg.JointTrajectoryControllerState, self.state_cb_publish) self._as = actionlib.SimpleActionServer( '/whole_body_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction, execute_cb=self.callback, auto_start=False) self._as.register_preempt_callback(self.preempt_cb) self._as.start() logging.loginfo(u'Joint Trajector Splitter: running') rospy.spin()
def callback(self, goal): logging.loginfo('received goal') self.success = True idx = [] for joint_name_list in self.joint_names: index_list = [] for joint_name in joint_name_list: try: index_list.append( goal.trajectory.joint_names.index(joint_name)) except ValueError: logging.logerr( 'the goal does not contain the joint ' + joint_name + ' but it is published by one of the state topics') result = control_msgs.msg.FollowJointTrajectoryResult() result.error_code = control_msgs.msg.FollowJointTrajectoryResult.INVALID_GOAL self._as.set_aborted(result) return idx.append(index_list) action_goals = [] for i in range(self.number_of_clients): if self.client_type[ i] == 'control_msgs/FollowJointTrajectoryActionGoal': action_goals.append( control_msgs.msg.FollowJointTrajectoryGoal()) else: action_goals.append( pr2_controllers_msgs.msg.JointTrajectoryGoal()) goal_trajectories_points = [[] for i in range(self.number_of_clients)] for p in goal.trajectory.points: for i, index_list in enumerate(idx): traj_point = trajectory_msgs.msg.JointTrajectoryPoint() joint_pos = [p.positions[j] for j in index_list] traj_point.positions = tuple(joint_pos) if p.velocities: joint_vel = [p.velocities[j] for j in index_list] traj_point.velocities = tuple(joint_vel) if p.accelerations: joint_acc = [p.accelerations[j] for j in index_list] traj_point.accelerations = tuple(joint_acc) if p.effort: joint_effort = [p.effort[j] for j in index_list] traj_point.effort = tuple(joint_effort) traj_point.time_from_start.nsecs = p.time_from_start.nsecs traj_point.time_from_start.secs = p.time_from_start.secs goal_trajectories_points[i].append(traj_point) for i, a_goal in enumerate(action_goals): a_goal.trajectory.header = goal.trajectory.header a_goal.trajectory.joint_names = self.joint_names[i] a_goal.trajectory.points = tuple(goal_trajectories_points[i]) logging.loginfo('send splitted goals') for i in range(self.number_of_clients): self.action_clients[i].send_goal( action_goals[i], feedback_cb=self.feedback_cb, done_cb=done_cb(self.client_topics[i], self.action_clients, self._as)) timeout = goal.trajectory.points[-1].time_from_start + rospy.Duration( secs=2) for i in range(self.number_of_clients): start = rospy.Time.now() finished_before_timeout = self.action_clients[i].wait_for_result( timeout=timeout) now = rospy.Time.now() timeout = timeout - (now - start) if not finished_before_timeout: logging.logwarn("Client took to long to finish action") self.success = False self._as.set_aborted() break if self.success: self._as.set_succeeded()
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.safe_get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.safe_set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.safe_get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.safe_set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.safe_get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(e) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weights, identifier.default_joint_weight_identifier, god_map) process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map) joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map) joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map) world = PyBulletWorld(god_map.safe_get_data(identifier.gui), blackboard.god_map.safe_get_data(identifier.data_folder)) god_map.safe_set_data(identifier.world, world) robot = WorldObject(god_map.safe_get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True, ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions), added_pairs=god_map.safe_get_data(identifier.added_self_collisions), symengine_backend=god_map.safe_get_data(identifier.symengine_backend), symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map) return god_map
def dump_state_cb(self, data): try: path = self.get_god_map().unsafe_get_data(identifier.data_folder) folder_name = datetime.now().strftime('%Y-%m-%d-%H-%M-%S') folder_path = '{}{}'.format(path, folder_name) os.mkdir(folder_path) robot = self.unsafe_get_robot() world = self.unsafe_get_world() with open("{}/dump.txt".format(folder_path), u'w') as f: tree_manager = self.get_god_map().unsafe_get_data(identifier.tree_manager) # type: TreeManager joint_state_message = tree_manager.get_node(u'js1').lock.get() f.write("initial_robot_joint_state_dict = ") write_dict(to_joint_state_position_dict(joint_state_message), f) f.write("try:\n" + " x_joint = initial_robot_joint_state_dict[\"odom_x_joint\"]\n" + " y_joint = initial_robot_joint_state_dict[\"odom_y_joint\"]\n" + " z_joint = initial_robot_joint_state_dict[\"odom_z_joint\"]\n" + " base_pose = PoseStamped()\n" + " base_pose.header.frame_id = \"map\"\n" + " base_pose.pose.position = Point(x_joint, y_joint, 0)\n" + " base_pose.pose.orientation = Quaternion(*quaternion_about_axis(z_joint, [0, 0, 1]))\n" + " zero_pose.teleport_base(base_pose)\n" + "except:\n" + " logging.loginfo(\'no x,y and z joint\')\n\n") f.write("zero_pose.send_and_check_joint_goal(initial_robot_joint_state_dict)\n") robot_base_pose = PoseStamped() robot_base_pose.header.frame_id = 'map' robot_base_pose.pose = robot.base_pose f.write("map_odom_transform_dict = ") write_dict(convert_ros_message_to_dictionary(robot_base_pose), f) f.write("map_odom_pose_stamped = convert_dictionary_to_ros_message(\'geometry_msgs/PoseStamped\', map_odom_transform_dict)\n") f.write("map_odom_transform = Transform()\n" + "map_odom_transform.rotation = map_odom_pose_stamped.pose.orientation\n" + "map_odom_transform.translation = map_odom_pose_stamped.pose.position\n\n") f.write("set_odom_map_transform = rospy.ServiceProxy('/map_odom_transform_publisher/update_map_odom_transform', UpdateTransform)\n") f.write("set_odom_map_transform(map_odom_transform)\n") original_robot = URDFObject(robot.original_urdf) link_names = robot.get_link_names() original_link_names = original_robot.get_link_names() attached_objects = list(set(link_names).difference(original_link_names)) for object_name in attached_objects: parent = robot.get_parent_link_of_joint(object_name) pose = robot.get_fk_pose(parent, object_name) world_object = robot.get_sub_tree_at_joint(object_name) f.write("#attach {}\n".format(object_name)) with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf: f_urdf.write(world_object.original_urdf) f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path, object_name)) f.write(" {}_urdf = f.read()\n".format(object_name)) f.write("{0}_name = \"{0}\"\n".format(object_name)) f.write("{}_pose_stamped_dict = ".format(object_name)) write_dict(convert_ros_message_to_dictionary(pose), f) f.write("{0}_pose_stamped = convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', {0}_pose_stamped_dict)\n".format(object_name)) f.write( "zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped)\n".format(object_name)) f.write( "zero_pose.attach_existing(name={0}_name, frame_id=\'{1}\')\n".format(object_name, parent)) for object_name, world_object in world.get_objects().items(): # type: (str, WorldObject) f.write("#add {}\n".format(object_name)) with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf: f_urdf.write(world_object.original_urdf) f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path,object_name)) f.write(" {}_urdf = f.read()\n".format(object_name)) f.write("{0}_name = \"{0}\"\n".format(object_name)) f.write("{0}_js_topic = \"{0}_js_topic\"\n".format(object_name)) f.write("{}_pose_dict = ".format(object_name)) write_dict(convert_ros_message_to_dictionary(world_object.base_pose), f) f.write("{0}_pose = convert_dictionary_to_ros_message('geometry_msgs/Pose', {0}_pose_dict)\n".format(object_name)) f.write("{}_pose_stamped = PoseStamped()\n".format(object_name)) f.write("{0}_pose_stamped.pose = {0}_pose\n".format(object_name)) f.write("{0}_pose_stamped.header.frame_id = \"map\"\n".format(object_name)) f.write("zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped, js_topic={0}_js_topic, set_js_topic=None)\n".format(object_name)) f.write("{}_joint_state = ".format(object_name)) write_dict(to_joint_state_position_dict((dict_to_joint_states(world_object.joint_state))), f) f.write("zero_pose.set_object_joint_state({0}_name, {0}_joint_state)\n\n".format(object_name)) last_goal = self.get_god_map().unsafe_get_data(identifier.next_move_goal) if last_goal: f.write(u'last_goal_dict = ') write_dict(convert_ros_message_to_dictionary(last_goal), f) f.write("last_goal_msg = convert_dictionary_to_ros_message('giskard_msgs/MoveCmd', last_goal_dict)\n") f.write("last_action_goal = MoveActionGoal()\n") f.write("last_move_goal = MoveGoal()\n") f.write("last_move_goal.cmd_seq = [last_goal_msg]\n") f.write("last_move_goal.type = MoveGoal.PLAN_AND_EXECUTE\n") f.write("last_action_goal.goal = last_move_goal\n") f.write("zero_pose.send_and_check_goal(goal=last_action_goal)\n") else: f.write(u'#no goal\n') logging.loginfo(u'saved dump to {}'.format(folder_path)) except: logging.logerr('failed to dump state pls try again') res = TriggerResponse() res.message = 'failed to dump state pls try again' return TriggerResponse() res = TriggerResponse() res.success = True res.message = 'saved dump to {}'.format(folder_path) return res
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map load_config_file() god_map.set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(str(e)) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weight, identifier.joint_weight_default, identifier.joint_weight_override, god_map) process_joint_specific_params(identifier.self_collision_avoidance_distance, identifier.self_collision_avoidance_default_threshold, identifier.self_collision_avoidance_default_override, god_map) process_joint_specific_params(identifier.external_collision_avoidance_distance, identifier.external_collision_avoidance_default_threshold, identifier.external_collision_avoidance_default_override, god_map) # TODO add checks to test if joints listed as linear are actually linear joint_velocity_linear_limit_symbols = process_joint_specific_params(identifier.joint_velocity_linear_limit, identifier.joint_velocity_linear_limit_default, identifier.joint_velocity_linear_limit_override, god_map) joint_velocity_angular_limit_symbols = process_joint_specific_params(identifier.joint_velocity_angular_limit, identifier.joint_velocity_angular_limit_default, identifier.joint_velocity_angular_limit_override, god_map) joint_acceleration_linear_limit_symbols = process_joint_specific_params(identifier.joint_acceleration_linear_limit, identifier.joint_acceleration_linear_limit_default, identifier.joint_acceleration_linear_limit_override, god_map) joint_acceleration_angular_limit_symbols = process_joint_specific_params( identifier.joint_acceleration_angular_limit, identifier.joint_acceleration_angular_limit_default, identifier.joint_acceleration_angular_limit_override, god_map) world = PyBulletWorld(False, blackboard.god_map.get_data(identifier.data_folder)) god_map.set_data(identifier.world, world) robot = WorldObject(god_map.get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, ignored_pairs=god_map.get_data(identifier.ignored_self_collisions), added_pairs=god_map.get_data(identifier.added_self_collisions)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.update_joint_symbols(joint_position_symbols.joint_map, joint_vel_symbols.joint_map, joint_weight_symbols, joint_velocity_linear_limit_symbols, joint_velocity_angular_limit_symbols, joint_acceleration_linear_limit_symbols, joint_acceleration_angular_limit_symbols) world.robot.init_self_collision_matrix() return god_map
def debug_print(self, np_H, np_A, np_lb, np_ub, np_lbA, np_ubA, xdot_full=None): import pandas as pd lb = [] lbA = [] weights = [] xdot = [] # if xdot_full is not None: # A_dot_x = np_A.dot(xdot_full) for iJ, k in enumerate(self.joint_constraints_dict.keys()): key = 'j -- ' + str(k) lb.append(key) weights.append(key) xdot.append(key) for iH, k in enumerate(self.hard_constraints_dict.keys()): key = 'h -- ' + str(k) lbA.append(key) upper_bound = np_ubA[iH] lower_bound = np_lbA[iH] if np.sign(upper_bound) == np.sign(lower_bound): logging.logwarn(u'{} out of bounds'.format(k)) if upper_bound > 0: logging.logwarn(u'{} value below lower bound by {}'.format(k, lower_bound)) vel = np_ub[iH] if abs(vel) < abs(lower_bound): logging.logerr(u'joint vel of {} to low to get back into bound in one iteration'.format(vel)) else: logging.logwarn(u'{} value above upper bound by {}'.format(k, abs(upper_bound))) vel = np_lb[iH] if abs(vel) < abs(lower_bound): logging.logerr(u'joint vel of {} to low to get back into bound in one iteration'.format(vel)) for iS, k in enumerate(self.soft_constraints_dict.keys()): key = 's -- ' + str(k) lbA.append(key) weights.append(key) # xdot.append(key) p_lb = pd.DataFrame(np_lb[:-len(self.soft_constraints_dict)], lb).sort_index() p_ub = pd.DataFrame(np_ub[:-len(self.soft_constraints_dict)], lb).sort_index() p_lbA = pd.DataFrame(np_lbA, lbA).sort_index() # if xdot_full is not None: # p_A_dot_x = pd.DataFrame(A_dot_x, lbA).sort_index() p_ubA = pd.DataFrame(np_ubA, lbA).sort_index() p_weights = pd.DataFrame(np_H.dot(np.ones(np_H.shape[0])), weights).sort_index() if xdot_full is not None: p_xdot = pd.DataFrame(xdot_full[:len(xdot)], xdot).sort_index() p_A = pd.DataFrame(np_A, lbA, weights).sort_index(1).sort_index(0) if self.lbAs is None: self.lbAs = p_lbA else: self.lbAs = self.lbAs.T.append(p_lbA.T, ignore_index=True).T # self.lbAs.T[[c for c in self.lbAs.T.columns if 'dist' in c]].plot() # arrays = [(p_weights, u'H'), # (p_A, u'A'), # (p_lbA, u'lbA'), # (p_ubA, u'ubA'), # (p_lb, u'lb'), # (p_ub, u'ub')] # for a, name in arrays: # self.check_for_nan(name, a) # self.check_for_big_numbers(name, a) pass
def __init__(self): rospy.init_node('joint_trajectory_splitter') self.action_clients = [] self.joint_names = [] self.state_topics = rospy.get_param('~state_topics', []) self.client_topics = rospy.get_param('~client_topics', []) self.number_of_clients = len(self.state_topics) if self.number_of_clients != len(self.client_topics): logging.logerr('Number of state and action topics do not match') exit() if self.number_of_clients == 0: logging.logwarn('No state/action topic found') exit() for i in range(self.number_of_clients): self.action_clients.append( actionlib.SimpleActionClient( self.client_topics[i], control_msgs.msg.FollowJointTrajectoryAction)) self.joint_names.append( rospy.wait_for_message( self.state_topics[i], control_msgs.msg. JointTrajectoryControllerState).joint_names) self.current_controller_state = control_msgs.msg.JointTrajectoryControllerState( ) total_number_joints = 0 for joint_name_list in self.joint_names: total_number_joints += len(joint_name_list) self.current_controller_state.joint_names.extend(joint_name_list) self.current_controller_state.desired.positions = [ 0 for i in range(total_number_joints) ] self.current_controller_state.desired.accelerations = [ 0 for i in range(total_number_joints) ] self.current_controller_state.desired.effort = [ 0 for i in range(total_number_joints) ] self.current_controller_state.desired.velocities = [ 0 for i in range(total_number_joints) ] self.current_controller_state.actual = copy.deepcopy( self.current_controller_state.desired) self.current_controller_state.error = copy.deepcopy( self.current_controller_state.desired) self.state_pub = rospy.Publisher( '/whole_body_controller/state', control_msgs.msg.JointTrajectoryControllerState, queue_size=10) for topic in self.state_topics: rospy.Subscriber(topic, control_msgs.msg.JointTrajectoryControllerState, self.state_cb_update) rospy.Subscriber(self.state_topics[0], control_msgs.msg.JointTrajectoryControllerState, self.state_cb_publish) self._as = actionlib.SimpleActionServer( '/whole_body_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction, execute_cb=self.callback, auto_start=False) self._as.register_preempt_callback(self.preempt_cb) self._as.start() rospy.spin()