コード例 #1
0
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()
コード例 #2
0
ファイル: test_seds_model.py プロジェクト: RoboHow/cram_seds
                #[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'])
コード例 #3
0
  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)