import numpy as _np
import scipy as _scipy
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
len_derivs = 2
len_dofs = 1
p = promep.ProMeP(index_sizes={
'dofs': len_dofs,
'interpolation_parameters':5,
'g': len_derivs,
'gphi': len_derivs,
'gtilde': len_derivs,
}, name='test_2')
from promep import _kumaraswamy
#create a set of "observations:"
num = 100
duration = 1.0
kv = _np.array((0.0, 0.0))
kp = _np.array((10.0, 1.0))
observations = []
free_variables = []
import numpy as _np
import scipy as _scipy
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
len_derivs = 2
len_dofs = 1
trajectory_parameters = 8
p = promep.ProMeP(index_sizes={
'dofs': len_dofs,
'interpolation_parameters':trajectory_parameters,
'g': len_derivs,
'gphi': len_derivs,
'gtilde': len_derivs,
}, name='test_6a')
modelfreepromp = promep.ProMeP(index_sizes={
'dofs': len_dofs,
'interpolation_parameters':trajectory_parameters,
'g': len_derivs,
'gphi': len_derivs,
'gtilde': len_derivs,
}, name='test_6b')
from promep import _kumaraswamy
Test whether interface functions work in the default
@author: Raphael Deimel
"""
import sys
import os
sys.path.insert(0, "../src/")
import promep
import numpy as _np
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
p_different = promep.ProMeP(index_sizes={
'gphi': 1
}) #create promep where all indices used are of different size
#promps only model motion,
promp = promep.ProMeP(index_sizes={
'r': 2,
'rtilde': 2,
'g': 2,
'gphi': 2,
'gtilde': 1
})
promp = promep.ProMeP(index_sizes={'derivatives': 1, 'realms': 1})
promp_modelfree = promep.ProMeP(index_sizes={'g': 2, 'gphi': 1, 'gtilde': 3})
dofs = 4
def learnGraphFromDemonstration(demonstrationSessionConfig,
observationsHDFStore=None):
"""
Main function to create a state graph and learn the primitives associated with the edges
as well as the static goals associated with states
"""
jointsList = []
#get the state graph:
if observationsHDFStore is None: #if no database is given, try to look up its name from the config file:
observationsHDFStore = _pd.HDFStore(config['hdf5_filename'])
#get metadata from database:
observationsMetadata = observationsHDFStore.get('metadata')
avilableLabels = observationsHDFStore.get('available_labels')
graphconfig = ExtractStateGraph(observationsMetadata)
print("GraphConfig: {}".format(graphconfig))
if 'joint_indices_list' in demonstrationSessionConfig:
jointsList = demonstrationSessionConfig['joint_indices_list']
else:
jointsList = None
if jointsList is None:
graphconfig['dofs'] = None
else:
graphconfig['dofs'] = len(jointsList)
observationsPerLabel = observationsMetadata['label'].value_counts()
for label in observationsPerLabel.index:
if observationsPerLabel[label] < demonstrationSessionConfig[
'minimum_sample_count']:
print(
"Warning: Transition {0} has only {1} observations!. ProMeP quality may suffer."
.format(label, observationsPerLabel[label]))
primitives = []
for label in graphconfig['transition_names']:
print("Learning {0} using {1} observations ...".format(
label, observationsPerLabel[label]))
if label in ('None', 'unlabeled', 'discarded'):
continue #skip
#load/combine parameters for the promep to learn:
learningParams = dict(
demonstrationSessionConfig[u'promep_learning_prior']['defaults'])
if label in demonstrationSessionConfig[u'promep_learning_prior']:
learningParams.update(learningParams[label])
observations_indices = observationsMetadata[
observationsMetadata['label'] == label]['i']
if learningParams['trim_strategy'] != 'inpoint_outpoint':
raise NotImplementedError(
"Only trim_stragey: inpoint_outpoint is supported!")
observations = []
for i in observations_indices:
observation_untrimmed = observationsHDFStore.get(
'observations/observation{0}'.format(i))
metadata = observationsMetadata.iloc[i]
#trim data:
observation = (
observation_untrimmed.
iloc[metadata['inpoint']:metadata['outpoint']]).copy(deep=True)
#annotate observations with phase values (maybe learn in the future, or use phasta simulation to generate more accurate profiles)
t = observation['observed', 'time', 't']
duration = t.iloc[-1] - t.iloc[0]
time_normalized = (t - t[0]) / duration
phaseVelocityProfileAssumption = learningParams[
'velocity_profile_assumption']
if phaseVelocityProfileAssumption == 'constant':
phi = time_normalized
elif phaseVelocityProfileAssumption == 'beta22':
phi = _betainc(
2, 2, time_normalized.as_matrix()) # expensive to compute
elif phaseVelocityProfileAssumption == 'kumaraswamy1.400,1.455':
phi = _promep._kumaraswamy.cdf(
1.400, 1.455, time_normalized.as_matrix(
)) #cheap approximation of beta(2,2)
elif phaseVelocityProfileAssumption == 'kumaraswamy1.645,1.800':
phi = _promep._kumaraswamy.cdf(
1.645, 1.800, time_normalized.as_matrix(
)) #closer to phase-state-machine's usual phase profile
elif phaseVelocityProfileAssumption == 'sine': #sinusoidal phase velocity
phi = 1.0 - 0.5 * _np.cos(time_normalized * _np.pi)
else:
raise ValueError("Unknown phase velocity profile specified")
phi = _pd.Series(phi)
#synthesize phase observations and add them to the observation:
dt = t.diff()
dphi = phi.diff()
ddphi = dphi.diff()
observation[('observed', 'phi', 0)] = phi
observation[('observed', 'phi',
1)] = (dphi / dt).interpolate(method='index',
limit_direction='both')
observation[('observed', 'phi', 2)] = (ddphi / dt**2).interpolate(
method='index', limit_direction='both') #todo: check
#append to the observations:
observations.append(observation)
index_sizes = {
'r': 2,
'rtilde': 2,
'g': 2,
'gtilde': 2,
'gphi': 2,
'stilde': learningParams['supports_count'],
'd': graphconfig['dofs'],
'dtilde': graphconfig['dofs'],
}
p = _promep.ProMeP(name=label, index_sizes=index_sizes)
p.learnFromObservations(observations)
primitives.append(p)
#set the phasta parameters:
if "phasta_parameters" in demonstrationSessionConfig:
graphconfig["phasta_parameters"] = demonstrationSessionConfig[
"phasta_parameters"]
else:
graphconfig["phasta_parameters"] = {
'alpha': 20.0,
'epsilon': 1e-5,
'nu': 2.0,
'dt': 1e-2,
'initial_bias': 1e-9,
'initial_transition_velocity_exponents': -5,
}
graphconfig[
'state_controllers'] = createPDControllerParamsFromDemonstration(
graphconfig, primitives, demonstrationSessionConfig)
graphconfig['mixer_weighing_method'] = demonstrationSessionConfig[
'mixer_weighing_method']
return (graphconfig, primitives)
import matplotlib.pyplot as plt
import promep._trajectorycompositors
from promep import _kumaraswamy
len_derivs = 2
len_dofs = 1
for repeatExtremal in (0, 3):
for len_trajectoryparameters in (3, 5, 11):
p = promep.ProMeP(index_sizes={
'dofs': len_dofs,
'interpolation_parameters': len_trajectoryparameters,
'g': len_derivs,
'gphi': len_derivs,
'gtilde': len_derivs
},
name='test_4')
p.trajectorycompositor.changeExtremalRepetition(repeatExtremal)
num = 100
observed_phases = _kumaraswamy.cdf(1.45, 1.68,
_np.linspace(0, 1.0, num))
observed_phases = _np.linspace(0.0, 1.0, num)
tc = _np.array([
p.trajectorycompositor.getPhi(phase) for phase in observed_phases
])
sys.path.insert(0, "../src/")
import promep
import numpy as _np
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
realms = 2
derivatives = 3
interpolation_parameters = 5
dofs = 7
p = promep.ProMeP(index_sizes={
'dofs': dofs,
'interpolation_parameters': interpolation_parameters,
'realms': realms,
'derivatives': derivatives
},
expected_duration=10.,
name='test1')
if p.tns['Wcov'].index_tuples != (
('rtilde', 'gtilde', 'stilde', 'dtilde'),
('rtilde_', 'gtilde_', 'stilde_',
'dtilde_')): #make sure we have the same notion of index order
raise Exception()
Wcov = p.tns['Wcov'].data
Wcov_flat = p.tns['Wcov'].data_flat
cov_dofs = _np.linspace(-1, 1.0, dofs)[:, None] * _np.linspace(-1, 1.0,
dofs)[None, :]