def check_CAchirality(self, coords):
"""
Sanity check on the CA to check if it is L of D
Returns False if the check fails i.e. if any D-amino acid is present
"""
# print 'in check CA chirality'
import measure
m = measure.Measure()
isL = True
for i in self.CAneighborList:
atNum = i[0]
rCA = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
atNum = i[1]
rC = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
atNum = i[2]
rCB = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
atNum = i[3]
rN = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
# compute improper torsion angle between C-CA-CB and CA-CB-N
rad, deg = m.torsion(rC,rCA,rCB,rN)
# check cis
if deg < 180 :
# this condition was found by inspection of structures todo
isL = False
print 'chiral state of CA atom ', i[0], ' is D'
print 'CA improper torsion (deg) ', i, ' = ', deg
return isL
def check_cistrans(self, coords):
"""
Sanity check on the isomer state of peptide bonds
Returns False if the check fails i.e. if any of the peptide bond is CIS
"""
import measure
m = measure.Measure()
isTrans = True
for i in self.peptideBondAtoms:
atNum = i[0]
rC = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
atNum = i[1]
rO = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
atNum = i[2]
rN = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
atNum = i[3]
rH = np.array( [ coords[3*atNum] , coords[3*atNum+1] , coords[3*atNum+2] ])
# compute O-C-N-H torsion angle
rad, deg = m.torsion(rO,rC,rN,rH)
# print 'peptide torsion (deg) ', i, ' = ', deg
# check cis
if deg < 90 or deg > 270:
isTrans = False
print 'CIS peptide bond between atoms ', i, ' torsion (deg) = ', deg
return isTrans
def __init__(self, top):
'''
top = OpenMM topology
'''
print 'in sanity check init'
self.topology = top
import measure
self.measure = measure.Measure()
self.populate_CAneighborList()
self.populate_peptideAtomList()
def test(self):
i = 0
self.meas = measure.Measure(self.names)
for s in self.tst:
c, r = self.class_sample(s)
self.meas.update_res(r, self.s_lab[i])
if c == self.s_lab[i]:
cls = True
else:
cls = False
self.meas.update_count(cls, self.s_lab[i])
i += 1
return self.meas
def __init__(self, samps = None,
bounds = None,
volume = None,
measure = None,
is_eq = True,
bandlims = None):
#TODO: remove this member
self.is_eq = is_eq
if samps is None:
self.samps = gd.Grid()
self.bounds = gd.Grid()
else:
self.samps = samps
self.bounds = bounds
self.ns = self.samps.get_curr_cards()
if volume is None:
self.volume = vol.Volume()
else:
self.volume = volume
if measure is None:
self.measure = ms.Measure()
else:
self.measure = measure
if bandlims is None:
bounds = deep(self.bounds)
self.bandlims = deep(bounds.get_lims() )
else:
self.bandlims = deep(bandlims)
return
def abc(measure1, measure_length, tied_notes, previous_measure):
voices = list(voices_generator(measure1, measure_length,
tied_notes)) # enthält 4 stimmen
measure_object = measure.Measure(previous_measure)
measure_object.add(voices)
measure_object.arrange_voices_for_ties()
measure_object.merge_voices_to_chords()
measure_object.set_rests_invisible()
return measure_object
def __init__(self, interval):
self.interval = interval
self.lock = threading.Lock()
self.config_obj = config.Config()
self.measure_obj = measure.Measure()
def new_sample(self, samples):
self.timer += 1
if self.timer > 10000:
self.stop()
def stop(self):
self.end = time.time()
self.running = False
def run(self):
print "starting"
self.start = time.time()
self.sub.subscribe_on_samples(self.new_sample)
while self.running:
time.sleep(1)
print "end, took", self.end - self.start
print "Starting"
timer = Timer()
m = measure.Measure()
thread = threading.Thread(name="receive_thread",
target=m.start_measurement,
args=(1, ))
thread.daemon = True
thread.start()
timer.run()
print "complete"
def factory(bandlims, ns, shf = [0.0], gdtype = 'cartes'):
res = None
if 'cartes' == gdtype:
ct = ns[0]
lim_t = ct * 4 - 1
lbpar_t = cd.LbFixParams( [ct], [lim_t] )
shf_t = shf[0]
T = bandlims[0][1]
dt = T / ct
diff = [dt]
shf = [shf_t]
band = [T]
ptpar = cd.PtFixParams(diff, shf, band)
lb_t = cd.LbGen(lbpar_t)
pt_t = cd.PtGen(ptpar)
dim_lb = (0, 0)
obj_lb = [lb_t]
dims_lb = [dim_lb]
mst_lb = mst.MdimStruct(obj_lb, dims_lb)
dims_pt = [ (0, 0) ]
obj_pt = [pt_t]
mst_pt = mst.MdimStruct(obj_pt, dims_pt)
gen = gn.Generator(mst_lb, mst_pt)
samps = gd.Grid(gen)
shf = [0.0]
ptpar = cd.PtFixParams(diff, shf, band)
pt_t = cd.PtGen(ptpar)
dims_pt = [ (0, 0) ]
obj_pt = [pt_t]
mst_pt = mst.MdimStruct(obj_pt, dims_pt)
gen = gn.Generator(mst_lb, mst_pt)
bounds = gd.Grid(gen)
bandstr = mst.MdimStruct( [ [0, T] ], [ (0,0) ] )
res = Domain(samps, bounds, None, None, True, bandstr)
elif 'polar' == gdtype:
R = bandlims[0][1]
A = bandlims[1][1]
cr = int( ns[0] )
ca = int( ns[1] )
lim_r = cr * 4 - 1
lim_a = ca * 2 - 1
lbpar_r = pl.LbFixParams( [cr], [lim_r] )
lbpar_a = pl.LbFixParams( [ca], [lim_a] )
shf_r = shf[0]
shf_a = shf[1]
dr = R / cr
da = A / ca
diff = [dr, da]
shf = [shf_r, shf_a]
band = [R, A]
ptpar = pl.PtFixParams(diff, shf, band)
pol_lb_r = pl.PolarLbGen(lbpar_r)
pol_lb_a = pl.PolarLbGen(lbpar_a)
pol_pt = pl.PolarPtGen(ptpar)
dim_lb_r = (0, 0)
dim_lb_a = (1, 1)
obj_lb = [pol_lb_r, pol_lb_a]
dims_lb = [dim_lb_r, dim_lb_a]
mst_lb = mst.MdimStruct(obj_lb, dims_lb)
dims_pt = [ (0, 1) ]
obj_pt = [pol_pt]
mst_pt = mst.MdimStruct(obj_pt, dims_pt)
gen = gn.Generator(mst_lb, mst_pt)
samps = gd.Grid(gen)
shf_r = 0.0
shf_a = 0.0
ptpar = pl.PtFixParams(diff, shf, band)
pol_pt = pl.PolarPtGen(ptpar)
dims_pt = [ (0, 1) ]
obj_pt = [pol_pt]
mst_pt = mst.MdimStruct(obj_pt, dims_pt)
gen = gn.Generator(mst_lb, mst_pt)
bounds = gd.Grid(gen)
volume = vol.Volume( mst.MdimStruct( [vol.sector], [ (0,1) ] ) )
meas = ms.Measure( mst.MdimStruct( [ms.sector], [ (0,1) ] ) )
bandstr = mst.MdimStruct( [ [0, R], [0, A] ], [ (0,0), (1,1) ] )
res = Domain(samps, bounds, volume, meas, True, bandstr)
else:
pass
return res
def main():
rospy.init_node('hcr_state_node')
sm_hcr = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
"""
Initialize userdata
"""
# target and current positions
pose_init = geometry_msgs.msg.PoseStamped()
pose_init.pose.position.x = 0
pose_init.pose.position.y = 0
pose_init.pose.position.z = 0
pose_init.pose.orientation.x = 0
pose_init.pose.orientation.y = 0
pose_init.pose.orientation.z = 0
pose_init.pose.orientation.w = 1
sm_hcr.userdata.pose_base_cur = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_base_tar = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_object_cur = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_object_tar = geometry_msgs.msg.PoseStamped()
sm_hcr.userdata.pose_base_cur = pose_init
sm_hcr.userdata.pose_base_cur.header.frame_id = "map"
sm_hcr.userdata.pose_base_tar = pose_init
sm_hcr.userdata.pose_base_tar.header.frame_id = "map"
sm_hcr.userdata.pose_object_cur = pose_init
sm_hcr.userdata.pose_object_tar = pose_init
# Flags (flg)
sm_hcr.userdata.flg_move_detect_done = False
sm_hcr.userdata.flg_move_search_done = False
sm_hcr.userdata.flg_object_grasp_done = False
sm_hcr.userdata.flg_object_place_done = False
sm_hcr.userdata.flg_move_new_goal = False
# List of detected frames (type class Frame) for each run
sm_hcr.userdata.list_detected_frames_tmp = []
sm_hcr.userdata.list_detected_frames_1 = []
sm_hcr.userdata.list_detected_frames_2 = []
sm_hcr.userdata.list_detected_frames_3 = []
"""
Initialize subscribers
"""
def _move_base_goal_cb(pose):
sm_hcr.userdata.pose_base_tar = pose
sm_hcr.userdata.flg_move_new_goal = True
rospy.Subscriber('/move_base_simple/goal', geometry_msgs.msg.PoseStamped,
_move_base_goal_cb)
"""
Initialize thread
"""
def thread_tf_target_object_pose():
while 1:
br = tf2_ros.TransformBroadcaster()
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = sm_hcr.userdata.pose_object_tar.header.frame_id
t.child_frame_id = "target_object"
t.transform.translation = sm_hcr.userdata.pose_object_tar.pose.position
t.transform.rotation = sm_hcr.userdata.pose_object_tar.pose.orientation
try:
br.sendTransform(t)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo(
"ERROR: thread_tf_target_object_pose: tf-exeption")
with sm_hcr:
### LOGIC ###
smach.StateMachine.add('LOGIC',
logic.Logic(),
transitions={
'aborted': 'aborted',
'reset': 'RESET',
'move_detect': 'MOVE_DETECT',
'move_search': 'MOVE_SEARCH',
'object_grasp': 'OBJECT_GRASP',
'object_place': 'OBJECT_PLACE'
},
remapping={
'flg_reset': 'flg_reset',
'flg_score': 'flg_score',
'flg_move_detect_done':
'flg_move_detect_done',
'flg_move_search_done':
'flg_move_search_done',
'flg_object_grasp_done':
'flg_object_grasp_done',
'flg_object_place_done':
'flg_object_place_done'
})
### RESET ###
smach.StateMachine.add('RESET',
logic.Reset(),
transitions={'succeeded': 'LOGIC'},
remapping={
'flg_move_detect_done':
'flg_move_detect_done',
'flg_move_search_done':
'flg_move_search_done',
'flg_move_new_goal':
'flg_move_new_goal',
'flg_object_grasp_done':
'flg_object_grasp_done',
'flg_object_place_done':
'flg_object_place_done',
'pose_base_tar':
'pose_base_tar',
'list_detected_frames_tmp':
'list_detected_frames_tmp',
'list_detected_frames_1':
'list_detected_frames_1',
'list_detected_frames_2':
'list_detected_frames_2',
'list_detected_frames_3':
'list_detected_frames_3'
})
### SCORE ###
smach.StateMachine.add('SCORE',
logic.Score(),
transitions={
'succeeded': 'LOGIC',
'aborted': 'RESET'
},
remapping={
'list_detected_frames_1':
'list_detected_frames_1',
'list_detected_frames_2':
'list_detected_frames_2',
'list_detected_frames_3':
'list_detected_frames_3'
})
### MOVE_DETECT ###
sm_MoveDetect = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=['pose_base_cur', 'list_detected_frames_tmp'],
output_keys=[
'pose_base_cur', 'list_detected_frames_tmp',
'list_detected_frames_1', 'flg_move_detect_done'
])
# sub Detect
smach.StateMachine.add('MOVE_DETECT', sm_MoveDetect, {
'succeeded': 'SCORE',
'aborted': 'RESET'
})
with sm_MoveDetect:
smach.StateMachine.add('DETECT',
measure.Detect(),
transitions={'succeeded': 'STOP'})
smach.StateMachine.add(
'STOP',
move.Stop(),
transitions={'succeeded': 'MEASURE'},
remapping={'pose_base_cur': 'pose_base_cur'})
smach.StateMachine.add(
'MEASURE',
measure.Measure(),
transitions={'succeeded': 'MOVE_DETECT_EXIT'},
remapping={
'pose_base_cur': 'pose_base_cur',
'list_detected_frames_tmp': 'list_detected_frames_tmp'
})
smach.StateMachine.add('MOVE_DETECT_EXIT',
logic.MoveDetectExit(),
transitions={'succeeded': 'succeeded'},
remapping={
'list_detected_frames_tmp':
'list_detected_frames_tmp',
'list_detected_frames_1':
'list_detected_frames_1',
'flg_move_detect_done':
'flg_move_detect_done'
})
### MOVE_SEARCH ###
sm_MoveSearch = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=[
'pose_base_tar', 'pose_base_cur', 'pose_object_tar',
'list_detected_frames_tmp'
],
output_keys=[
'pose_base_tar', 'pose_base_cur', 'list_detected_frames_tmp',
'list_detected_frames_2', 'flg_move_search_done'
])
smach.StateMachine.add('MOVE_SEARCH', sm_MoveSearch, {
'succeeded': 'SCORE',
'aborted': 'RESET'
})
with sm_MoveSearch:
smach.StateMachine.add(
'BASEPOSE_FINDER',
logic.BasePoseFinder(),
transitions={'succeeded': 'MOVE'},
# transitions={'succeeded':'MEASURE'},
remapping={
'pose_object_tar': 'pose_object_tar',
'pose_base_tar': 'pose_base_tar'
})
smach.StateMachine.add('MOVE',
move.Move(),
transitions={'succeeded': 'MEASURE'},
remapping={
'pose_base_tar': 'pose_base_tar',
'pose_base_cur': 'pose_base_cur'
})
smach.StateMachine.add(
'MEASURE',
measure.Measure(),
transitions={'succeeded': 'MOVE_SEARCH_EXIT'},
remapping={
'pose_base_cur': 'pose_base_cur',
'list_detected_frames_tmp': 'list_detected_frames_tmp'
})
smach.StateMachine.add('MOVE_SEARCH_EXIT',
logic.MoveSearchExit(),
transitions={'succeeded': 'succeeded'},
remapping={
'list_detected_frames_tmp':
'list_detected_frames_tmp',
'list_detected_frames_2':
'list_detected_frames_2',
'flg_move_search_done':
'flg_move_search_done'
})
### OBJECT_GRASP ###
sm_ObjectGrasp = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=['flg_object_grasp_done', 'pose_object_tar'],
output_keys=['flg_object_grasp_done'])
smach.StateMachine.add('OBJECT_GRASP', sm_ObjectGrasp, {
'succeeded': 'LOGIC',
'aborted': 'RESET'
})
with sm_ObjectGrasp:
smach.StateMachine.add(
'PICK',
grasp.Pick(),
transitions={'succeeded': 'OBJECT_GRASP_EXIT'},
remapping={'pose_object_tar': 'pose_object_tar'})
smach.StateMachine.add(
'OBJECT_GRASP_EXIT',
logic.ObjectGraspExit(),
transitions={'succeeded': 'succeeded'},
remapping={'flg_object_grasp_done': 'flg_object_grasp_done'})
### OBJECT_PLACE ###
sm_ObjectPlace = smach.StateMachine(
outcomes=['succeeded', 'aborted'],
input_keys=[
'pose_base_cur', 'pose_base_tar', 'flg_object_place_done'
],
output_keys=['pose_base_cur', 'flg_object_place_done'])
smach.StateMachine.add('OBJECT_PLACE', sm_ObjectPlace, {
'succeeded': 'LOGIC',
'aborted': 'RESET'
})
with sm_ObjectPlace:
smach.StateMachine.add('MOVE',
move.Move(),
transitions={'succeeded': 'PLACE'},
remapping={
'pose_base_tar': 'pose_base_tar',
'pose_base_cur': 'pose_base_cur'
})
smach.StateMachine.add(
'PLACE',
grasp.Place(),
transitions={'succeeded': 'OBJECT_PLACE_EXIT'})
smach.StateMachine.add(
'OBJECT_PLACE_EXIT',
logic.ObjectPlaceExit(),
transitions={'succeeded': 'succeeded'},
remapping={'flg_object_place_done': 'flg_object_place_done'})
# # attach a SMACH introspection server
sis = smach_ros.IntrospectionServer('hcr_state_machine', sm_hcr,
'/HCR_State_Machine')
sis.start()
# set preempt handler
smach_ros.set_preempt_handler(sm_hcr)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
thread_tf_tarBasePose = threading.Thread(
target=thread_tf_target_object_pose)
thread_tf_tarBasePose.start()
thread_smach = threading.Thread(target=sm_hcr.execute)
thread_smach.start()
# signal handler
rospy.spin()
