def CRAM2CDS_client():
# Creates the SimpleActionClient, passing the type of the action to the constructor.
client = actionlib.SimpleActionClient('cram2cds', learning_actionlib.msg.CRAM2CDSAction)
client.wait_for_server()
# Send goal and wait for result
desired_motion_phase_model = MotionPhase()
desired_motion_phase_model.id = 'reaching'
desired_motion_phase_model.object = 'table'
desired_motion_phase_model.threshold = 0.01
desired_motion_phase_model.attractor = [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]
goal = learning_actionlib.msg.CRAM2CDSGoal(desired_motion_phase_model)
client.send_goal(goal)
client.wait_for_result()
# Prints out the result of executing the action
return client.get_result()
#[gauss] )
gmm = GaussianMixtureModel()
gmm.id = solution['GMM']
gmm.gaussian_dist.append(gauss)
gmm.type = solution['GMMType']
gmm.input_type = solution['InputType']
gmm.output_type = solution['InputType']
gmm.input_dim = solution['InputDim']
gmm.output_dim = solution['InputDim']
m = MotionModel()
m.id = solution['Model']
m.type = solution['Type']
m.described_by_GMM.append(gmm)
p = MotionPhase()
p.id = solution['Phase']
p.described_by_motion_model.append(m)
pub.publish(p)
rospy.sleep(1.0)
# also print some parts to the terminal
print "\n\n= = = = = = = = = = = = = = = = = = = = = "
print solution['Phase'].split('#')[1]
print solution['Model'].split('#')[1]
print solution['GMM'].split('#')[1]
print solution['Gaussian'].split('#')[1]
print 'Mean = %s' % (solution['MeanVec'])
print 'Cov = %s' % (solution['CovMat'])
print 'Prior = %s' % (solution['Prior'])
def execute_cb(self, goal):
# helper variables
r = rospy.Rate(100)
success = True
# append the seeds for the fibonacci sequence
self._feedback.found = 1
# publish info to the console for the user
rospy.loginfo('Retrieving information from Knowledge Base')
# start executing the action
prolog = json_prolog.Prolog()
try:
query = prolog.query("""rdfs_subclass_of(Phase, seds:'SEDSMotion'),
phase_properties(Phase, ID, Object, Threshold, Attractor, Models),
member(Model, Models),
motion_properties(Model, Type, GMMs),
member(GMM, GMMs),
gmm_properties(GMM, GMMType, InputType, InputDim, OutputType, OutputDim, Gaussians),
member(Gaussian, Gaussians),
gaussian_components(Gaussian, Mean, Cov, Prior),
vector_elements(Mean, MeanVec),
matrix_elements(Cov, CovMat)""")
phaseModel = MotionPhase()
for solution in query.solutions():
gd = solution['Gaussian'].encode('ascii','ignore')
gd = gd[gd.index('#')+1:gd.index('_')]
gauss = GaussianDistribution(
gd,
solution['Prior'],
solution['MeanVec'],
solution['CovMat'])
gmm = GaussianMixtureModel()
gmm.id = solution['GMM'].encode('ascii','ignore')
gmm.id = gmm.id[gmm.id.index('#')+1:gmm.id.index('_')]
gmm.gaussian_dist.append(gauss)
gmm.type = solution['GMMType'].encode('ascii','ignore')
gmm.type = gmm.type[gmm.type.index('#')+1:len(gmm.type)]
gmm.input_type = solution['InputType'].encode('ascii','ignore')
gmm.output_type = solution['InputType'].encode('ascii','ignore')
#gmm.input_dim = solution['InputDim']
#gmm.output_dim = solution['InputDim']
m = MotionModel()
m.id = solution['Model'].encode('ascii','ignore')
m.id = m.id[m.id.index('#')+1:m.id.index('_')]
m.type = solution['Type'].encode('ascii','ignore')
m.type = m.type[m.type.index('#')+1:len(m.type)]
m.described_by_GMM.append(gmm)
p = MotionPhase()
p.id = solution['Phase'].encode('ascii','ignore')
p.id = p.id[p.id.index('#')+1:p.id.index('_')]
p.object = solution['Object']
p.object = p.object[u'term'].pop(1)[u'term'].pop(2)
p.threshold = solution['Threshold']
p.threshold = p.threshold[u'term'].pop(1)[u'term'].pop(2)
p.attractor = solution['Attractor']
p.described_by_motion_model.append(m)
if p.id == goal.desired_motion_phase:
phaseModel.described_by_motion_model.append(m)
phaseModel.id = p.id
phaseModel.object = p.object.encode('ascii','ignore')
phaseModel.attractor = p.attractor
phaseModel.threshold = float(p.threshold)
query.finish()
except rospy.ROSInterruptException:
pass
###########################################################
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._feedback.found = 1
self._as.set_preempted()
success = False
# publish the feedback
self._as.publish_feedback(self._feedback)
r.sleep()
if success:
self._result.desired_motion_phase_model.described_by_motion_model = phaseModel.described_by_motion_model
self._result.desired_motion_phase_model.id = phaseModel.id
self._result.desired_motion_phase_model.object = phaseModel.object
self._result.desired_motion_phase_model.threshold = phaseModel.threshold
self._result.desired_motion_phase_model.attractor = phaseModel.attractor
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)