class MicoBlackshipGazeboInterface(object): def __init__(self): self._mobile_vel_cmd_publisher = rospy.Publisher(rospy.get_param(PARAM_NAME_MOBILE_VW_CMD_TOPIC, default=PARAM_NAME_MOBILE_VW_CMD_TOPIC[1:]), TwistStamped, queue_size=1) self._joint_vel_cmd_publishers = [] param_names = [PARAM_NAME_JOINT1_CMD_TOPIC, PARAM_NAME_JOINT2_CMD_TOPIC, PARAM_NAME_JOINT3_CMD_TOPIC, PARAM_NAME_JOINT4_CMD_TOPIC, PARAM_NAME_JOINT5_CMD_TOPIC, PARAM_NAME_JOINT6_CMD_TOPIC] self._joint_vel_cmd_publishers = [rospy.Publisher(rospy.get_param(param_name, default=param_name[1:]), Float64, queue_size=1) for param_name in param_names] self._js_publisher = rospy.Publisher(rospy.get_param(PARAM_NAME_ARM_JOINT_STATE_TOPIC), JointState, queue_size=1) self._joint_state = JointState() self.Rw = rospy.get_param(PARAM_NAME_MOBILE_WHEEL_RADIUS) self.T = rospy.get_param(PARAM_NAME_MOBILE_AXLE_TRACK) self._robot_vel_cur_container = DataContainer('RobotVelCur', data_class=list, header_list=['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'v', 'w']) def activate(self): rospy.Subscriber(rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC, default=PARAM_NAME_BS_INPUT_TOPIC[1:]), TwistStamped, self._mobile_vel_callback) rospy.Subscriber(rospy.get_param(PARAM_NAME_JOINTVEL_INPUT_TOPIC, default=PARAM_NAME_JOINTVEL_INPUT_TOPIC[1:]), JointVelocity, self._joint_vel_callback) rospy.Subscriber(rospy.get_param(PARAM_NAME_JOINT_STATE_TOPIC, default=PARAM_NAME_JOINT_STATE_TOPIC[1:]), JointState, self._joint_state_callback) rospy.Subscriber(rospy.get_param(PARAM_NAME_BS_STATE_TOPIC, default=PARAM_NAME_BS_STATE_TOPIC[1:]), JointState, self._bs_state_callback) return self def _mobile_vel_callback(self, twist_stamped): self._mobile_vel_cmd_publisher.publish(twist_stamped) def _joint_vel_callback(self, joint_vel): for attr, publisher in zip(JointVelocity.__slots__, self._joint_vel_cmd_publishers): publisher.publish(Float64(data=getattr(joint_vel, attr))) def _joint_state_callback(self, js): self._joint_state = js self._js_publisher.publish(js) def _bs_state_callback(self, js): thetal_dot = (js.velocity[0] + js.velocity[2]) * 0.5 thetar_dot = (js.velocity[1] + js.velocity[3]) * 0.5 v = (thetal_dot + thetar_dot) * self.Rw /2 w = (thetal_dot - thetar_dot) * self.Rw / self.T self._robot_vel_cur_container.write(list(self._joint_state.velocity) + [v, w])
def __init__(self, registered_tf_buffer=None): super(MicoBSCtrl_MaxManipulability, self).__init__([JointVelocity, TwistStamped], [rospy.get_param(PARAM_NAME_MICO_JOINTVEL_INPUT_TOPIC), rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC)], registered_tf_buffer) self._param = ParameterManager() self._robot_state_cur = RobotState() self._mani_ref = ManipulationReference() self._is_initialized_robot_state_cur = False self._is_initialized_mani_ref = False self._decimals = 4 self._interval_sec = 3.0 self._ee_state_err_container = DataContainer('eeStateErr', data_class=list, header_list=['x', 'y', 'z', 'roll', 'pitch', 'yaw']) self._ee_state_ref_container = DataContainer('eeStateRef', data_class=list, header_list=['x', 'y', 'z', 'roll', 'pitch', 'yaw']) self._ee_state_cur_container = DataContainer('eeStateCur', data_class=list, header_list=['x', 'y', 'z', 'roll', 'pitch', 'yaw']) self._robot_vel_input_container = DataContainer('RobotVelInput', data_class=list, header_list=['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'v', 'w'])
def __init__(self): self._mobile_vel_cmd_publisher = rospy.Publisher(rospy.get_param(PARAM_NAME_MOBILE_VW_CMD_TOPIC, default=PARAM_NAME_MOBILE_VW_CMD_TOPIC[1:]), TwistStamped, queue_size=1) self._joint_vel_cmd_publishers = [] param_names = [PARAM_NAME_JOINT1_CMD_TOPIC, PARAM_NAME_JOINT2_CMD_TOPIC, PARAM_NAME_JOINT3_CMD_TOPIC, PARAM_NAME_JOINT4_CMD_TOPIC, PARAM_NAME_JOINT5_CMD_TOPIC, PARAM_NAME_JOINT6_CMD_TOPIC] self._joint_vel_cmd_publishers = [rospy.Publisher(rospy.get_param(param_name, default=param_name[1:]), Float64, queue_size=1) for param_name in param_names] self._js_publisher = rospy.Publisher(rospy.get_param(PARAM_NAME_ARM_JOINT_STATE_TOPIC), JointState, queue_size=1) self._joint_state = JointState() self.Rw = rospy.get_param(PARAM_NAME_MOBILE_WHEEL_RADIUS) self.T = rospy.get_param(PARAM_NAME_MOBILE_AXLE_TRACK) self._robot_vel_cur_container = DataContainer('RobotVelCur', data_class=list, header_list=['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'v', 'w'])
[-1] + _input_shape) _test_labels = to_categorical(test_pairs['label'].tolist()) # Build model _keras_model = VggishModel(class_weights=_class_weights, input_shape=_input_shape, num_classes=_num_classes, batch_size=64, learning_rate=0.0001, metric_baseline=0.5, num_epochs=30, load_pretrained=True, feature_type='raw', output_dir='outputs') # Build container. _container = DataContainer(data=_train_pairs, labeled_percent=0.1, num_classes=_num_classes, feature_shape=_keras_model.input_shape, label_shape=_keras_model.output_shape) print(_keras_model.get_model_summary()) run_entropy(data_container=_container, model=_keras_model, test_features=_test_features, test_lablels=_test_labels, stats_path='outputs/stats/al_stats.pd', max_round=60)
class MicoBSCtrl_MaxManipulability(RobotControllerAbstract): def __init__(self, registered_tf_buffer=None): super(MicoBSCtrl_MaxManipulability, self).__init__([JointVelocity, TwistStamped], [rospy.get_param(PARAM_NAME_MICO_JOINTVEL_INPUT_TOPIC), rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC)], registered_tf_buffer) self._param = ParameterManager() self._robot_state_cur = RobotState() self._mani_ref = ManipulationReference() self._is_initialized_robot_state_cur = False self._is_initialized_mani_ref = False self._decimals = 4 self._interval_sec = 3.0 self._ee_state_err_container = DataContainer('eeStateErr', data_class=list, header_list=['x', 'y', 'z', 'roll', 'pitch', 'yaw']) self._ee_state_ref_container = DataContainer('eeStateRef', data_class=list, header_list=['x', 'y', 'z', 'roll', 'pitch', 'yaw']) self._ee_state_cur_container = DataContainer('eeStateCur', data_class=list, header_list=['x', 'y', 'z', 'roll', 'pitch', 'yaw']) self._robot_vel_input_container = DataContainer('RobotVelInput', data_class=list, header_list=['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'v', 'w']) def activate_controller(self): rospy.Subscriber(rospy.get_param(PARAM_NAME_MANI_REF_TOPIC), ManipulationReference, self._mani_ref_callback) rospy.Subscriber(rospy.get_param(PARAM_NAME_ROBOT_STATE_TOPIC), RobotState, self._robot_state_callback) while (self._is_initialized_robot_state_cur is False): rospy.loginfo('wait for initialing robot_state_cur...') rospy.sleep(self._interval_sec) return True def update_input(self): m2mm = np.matrix(np.diag(np.array([1000, 1000, 1000, 1, 1, 1]))) mm2m = np.matrix(np.diag(np.array([0.001, 0.001, 0.001, 1, 1, 1]))) jacobi = get_jacobi_matrix(self._robot_state_cur.arm_joint_angles, euler_from_rosquaternion(self._robot_state_cur.base_pose.orientation)[0]) # pinv_jacobi = np.linalg.pinv(jacobi) pinv_jacobi = jacobi.T.dot(np.linalg.inv(jacobi.dot(jacobi.T))) # vec_redundancy = get_vec_redundancy(self._robot_state_cur, jacobi) Kpgain = np.matrix(np.diag(np.array([2, 2, 2, 0, 0, 0]))) Kredundancy = np.matrix(np.diag(np.array([1, 1, 1, 1, 1, 1, 1, 1]))) # rospy.loginfo('\n' + '\n'.join(['EEStateErr:\t' + ', '.join([str(d[0]) for d in self._ee_state_err.tolist()]), # 'EEVelRef:\t' + ', '.join([str(d[0]) for d in self._ee_vel_ref.tolist()])])) u = pinv_jacobi * m2mm * (self._ee_vel_ref - Kpgain * self._ee_state_err) # + (np.identity(8) - pinv_jacobi * jacobi) * Kredundancy * vec_redundancy u = u.round(decimals=self._decimals) v = (u[6, 0] + u[7, 0]) * self._param.Rw / 2 w = (u[6, 0] - u[7, 0]) * self._param.Rw / self._param.T self._ee_state_err_container.write(self._ee_state_err.T.tolist()[0]) self._ee_state_ref_container.write(self._ee_state_ref.T.tolist()[0]) self._ee_state_cur_container.write(self._ee_state_cur.T.tolist()[0]) self._ee_state_cur_container.write(self._ee_state_cur.T.tolist()[0]) self._robot_vel_input_container.write(u.T.tolist()[0][:6] + [v, w]) # FIXME u[0, 0] return [JointVelocity(joint1=-u[0, 0], joint2=u[1, 0], joint3=u[2, 0], joint4=u[3, 0], joint5=u[4, 0], joint6=u[5, 0]), TwistStamped(header=Header(stamp=rospy.Time.now()), twist=twist_from_vw(v, w))] def _mani_ref_callback(self, mani_ref): self._mani_ref = mani_ref if self._is_initialized_mani_ref is False: self._is_initialized_mani_ref = True def _robot_state_callback(self, robot_state): self._robot_state_cur = robot_state if self._is_initialized_robot_state_cur is False: self._is_initialized_robot_state_cur = True @property def _ee_state_ref(self): pose_ref = self._mani_ref.ee_pose if self._is_initialized_mani_ref else self._robot_state_cur.ee_pose return np.matrix(rosmsg2nparray(pose_ref)).T.round(decimals=self._decimals) @property def _ee_state_cur(self): return np.matrix(rosmsg2nparray(self._robot_state_cur.ee_pose)).T.round(decimals=self._decimals) @property def _ee_vel_ref(self): twist_ref = self._mani_ref.ee_twist if self._is_initialized_mani_ref else Twist() return np.matrix(rosmsg2nparray(twist_ref)).T.round(decimals=self._decimals) @property def _ee_vel_cur(self): return np.matrix(rosmsg2nparray(self._robot_state_cur.ee_twist)).T.round(decimals=self._decimals) @property def _ee_state_err(self): pose_ref = self._mani_ref.ee_pose if self._is_initialized_mani_ref else self._robot_state_cur.ee_pose pose_err = diff_poses(self._robot_state_cur.ee_pose, pose_ref) return np.matrix(rosmsg2nparray(pose_err)).T.round(decimals=self._decimals)
if flags.framework_type == 'entropy' or \ flags.framework_type == 'edpc' or \ flags.framework_type == 'hist_select': _embeddings = _keras_model.embedding( np.reshape(_pairs['feature'].tolist(), [-1] + _input_shape)) else: _embeddings = None # Reset weights params. _keras_model.reload_weights() # Build container. _container = DataContainer(data=_train_pairs, labeled_percent=flags.labeled_percent, num_classes=_num_classes, feature_shape=_keras_model.input_shape, label_shape=_keras_model.output_shape, embeddings=_embeddings) print(_keras_model.get_model_summary()) # Create framework object. framework = framework_creator( data_container=_container, model=_keras_model, test_features=_test_features, test_labels=_test_labels, stats_path='%s/al_stats_temp.csv' % cur_work_space, max_round=flags.max_round, num_select_per_round=flags.num_select_per_round, pre_train=flags.pre_train,