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,
