def makeFromDict(cls, params, tns=None):
"""
Create a controller from the given dictionary
The controller classes possess a serialize() function to create those dictionaries
Either the dictionary contains the index sizes (r,g,d), or you can provide a TensorNameSpace instead
"""
if tns == None:
tns = _mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(
r=params['r'], g=params['g'], d=params['d'])
c = LTIGoal(tns, **params)
return c
import numpy as _np
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
import namedtensors
import mechanicalstate
import staticprimitives
variants = []
for r,g,d in [(2,1,1),(2,2,1),(2,3,1),(2,2,8)]: #test on various combinations of r,g,d parameters
#make a tensor namespace that hold the right index definitions:
tns_global = mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(r=r, g=g, d=d)
tns_global.registerTensor('CurrentMean', (('r', 'g', 'd'),()) )
tns_global.registerTensor('CurrentCov', (('r', 'g', 'd'),('r_', 'g_', 'd_')), initial_values='identity' )
msd_current = mechanicalstate.MechanicalStateDistribution(tns_global, 'CurrentMean', 'CurrentCov')
ltigoal = staticprimitives.LTIGoal(tns_global, name='test_7_a')
variants.append( (tns_global, ltigoal, msd_current))
positions = _np.zeros((d))
positions[0] = 13.8
velocities = _np.zeros((d))
velocities[d-1] = 3.21
Kv = _np.ones((d,d))
Kd = _np.arange(d)
def __init__(self,
tensornamespace,
current_msd_from=None,
task_space='jointspace',
name='unnamed',
expected_torque_noise=0.123,
**kwargs):
self.name = name
self.phaseAssociable = False #indicate that this motion generator is not parameterized by phase
self.timeAssociable = True #indicate that this motion generator is parameterizable by time
self.taskspace_name = task_space
if not tensornamespace is None:
self.tns = _nt.TensorNameSpace(
tensornamespace) #inherit index sizes
if self.tns['r'].size < 2:
raise NotImplementedError(
) #sorry, this "trivial" functionality is not implemented. Try using a simple, fixed msd instead
else:
self.tns = _mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(
r=2, g=2, d=1)
self.tns.cloneIndex('r', 'r2')
self.tns.cloneIndex('g', 'g2')
self.tns.cloneIndex('d', 'd2')
#desired values:
self.tns.registerTensor('DesiredMean', (('r', 'g', 'd'), ()))
self.tns.registerTensor('DesiredCov',
(('r', 'g', 'd'), ('r_', 'g_', 'd_')))
self.msd_desired = _mechanicalstate.MechanicalStateDistribution(
self.tns, "DesiredMean", "DesiredCov")
#initialize the desired torque variance to some non-zero value:
self.tns.setTensorToIdentity('DesiredCov', scale=1e-6)
self.msd_desired.addVariance('torque', expected_torque_noise**2)
# self.msd_desired.addVariance('impulse', expected_torque_noise**2)
self.commonnames2rg = self.msd_desired.commonnames2rg #we use them a lot here, so remember a shorthand
self.commonnames2rglabels = self.msd_desired.commonnames2rglabels
if current_msd_from is None: #local copy
self.tns.registerTensor('CurrentMean', (('r', 'g', 'd'), ()))
self.tns.registerTensor('CurrentCov',
(('r', 'g', 'd'), ('r_', 'g_', 'd_')))
self.msd_current = _mechanicalstate.MechanicalStateDistribution(
self.tns, "CurrentMean", "CurrentCov")
else: #use data from somewhere else:
self.tns.registerTensor(
'CurrentMean',
current_msd_from.tns[current_msd_from.meansName].index_tuples,
external_array=current_msd_from.tns[
current_msd_from.meansName].data,
initial_values='keep')
self.tns.registerTensor(
'CurrentCov',
current_msd_from.tns[
current_msd_from.covariancesName].index_tuples,
external_array=current_msd_from.tns[
current_msd_from.covariancesName].data,
initial_values='keep')
self.msd_current = current_msd_from
rlabel_torque, glabel_torque = self.commonnames2rglabels['torque']
rlabel_pos, glabel_pos = self.commonnames2rglabels['position']
self.tns.registerBasisTensor('e_ktau', (('r2', 'g2'), ('r', 'g')),
((rlabel_torque, glabel_torque),
(rlabel_torque, glabel_torque)))
self.tns.registerTensor('delta_d2d', (('d2', ), ('d', )),
initial_values='identity')
self.tns.registerBasisTensor('e_kp', (('r2', 'g2'), ('r', 'g')),
((rlabel_torque, glabel_torque),
(rlabel_pos, glabel_pos)))
self.tns.registerTensor('Kp', (('d2', ), ('d', )))
slice_tau = self.tns.registerContraction('e_ktau', 'delta_d2d')
slice_kp = self.tns.registerContraction('e_kp', 'Kp')
if 'velocity' in self.commonnames2rg:
rlabel_vel, glabel_vel = self.commonnames2rglabels['velocity']
self.tns.registerBasisTensor('e_kv', (('r2', 'g2'), ('r', 'g')),
((rlabel_torque, glabel_torque),
(rlabel_vel, glabel_vel)))
self.tns.registerTensor('Kv', (('d2', ), ('d', )))
slice_kv = self.tns.registerContraction('e_kv', 'Kv')
#add together:
self.tns.registerSum([slice_tau, slice_kp, slice_kv],
result_name='U')
else:
self.tns.registerAddition(slice_tau, slice_kp, result_name='U')
self.tns.registerTensor('I',
self.tns['U'].index_tuples,
initial_values='identity')
if 'velocity' in self.commonnames2rg: #can we add damping terms? (kd)
#Compute the K tensor:
self.tns.registerTensor(
'Kd', (('d2', ), ('d', ))
) #kd is equal to kv but with goal velocity=0 -> only appears here and not in the computation of 'U'
slice_kd = self.tns.registerContraction('e_kv', 'Kd')
U_minus_damping = self.tns.registerAddition('U', slice_kd)
self.tns.registerSubtraction('I', U_minus_damping, result_name='K')
else:
self.tns.registerSubtraction('I', 'U', result_name='K')
self.tns.registerTranspose('U')
self.tns.registerTranspose('K')
#influence of desired mean on expected mean:
term_meanU = self.tns.registerContraction('U', 'DesiredMean')
#influence of desired cov on expected cov:
previous = 'DesiredCov'
# previous = self.tns.registerAddition('DesiredCov','CurrentCov')
previous = self.tns.registerContraction('U', previous)
term_covU = self.tns.registerContraction(previous, '(U)^T')
#up to here we can precompute the equations if goals don't change
self._update_cheap_start = len(self.tns.update_order)
#influence of current cov to expected cov:
previous = self.tns.registerContraction('K', 'CurrentCov')
term_covK = self.tns.registerContraction(previous, '(K)^T')
self.tns.registerAddition(term_covK,
term_covU,
result_name='ExpectedCov')
#influence of current mean on expected mean:
previous = self.tns.registerContraction('K', 'CurrentMean')
self.tns.registerAddition(previous,
term_meanU,
result_name='ExpectedMean')
droptwos = {
'r2': 'r',
'g2': 'g',
'd2': 'd',
'r2_': 'r_',
'g2_': 'g_',
'd2_': 'd_'
}
self.tns.renameIndices('ExpectedCov', droptwos, inPlace=True)
self.tns.renameIndices('ExpectedMean', droptwos, inPlace=True)
#package the result:
self.msd_expected = _mechanicalstate.MechanicalStateDistribution(
self.tns, "ExpectedMean", "ExpectedCov")
#set values, if provided:
self.setDesired(**kwargs)
self.tns.update(*self.tns.update_order[:self._update_cheap_start])
def __init__(self, DefinitionsDirectory, doProfiling=False):
"""
"""
self._profiler = None
if doProfiling:
import cProfile
self._profiler = cProfile.Profile()
self._profiler.bias = 3e-6
self.watchdogPeriod = rospy.Duration.from_sec(
rospy.get_param('watchdog timeout', 0.5))
self.jointaccmax = rospy.get_param('max joint acceleration',
1.0) #rad²/s²
self.jointvelmax = rospy.get_param('max joint acceleration',
2.0) #rad/s
self.updateFrequency = rospy.get_param('frequency', 50.0)
self.max_active_inputs = rospy.get_param('max_active_inputs', 5)
#some global paramters related to MSDs and phases:
self.msd_size_realms = rospy.get_param('msd_size_realms',
2) #number of realms to use
self.msd_size_derivatives = rospy.get_param(
'msd_size_derivatives',
2) #number of derivatives to consider for MSDs and phase
self.msd_size_dofs = rospy.get_param('msd_size_derivatives', 8)
self.phase_size_derivatives = rospy.get_param(
'phase_size_derivatives',
2) #number of derivatives to expect for the generalized phase
self.integration_timestep = 1.0 / self.updateFrequency
self.DefinitionsDirectory = DefinitionsDirectory
#make a tensor namespace that hold the right index definitions:
self.tns = mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(
r=self.msd_size_realms,
g=self.msd_size_derivatives,
d=self.msd_size_dofs)
self.mixer = mechanicalstate.Mixer(
self.tns, max_active_inputs=self.max_active_inputs)
self.last_msd = self.mixer.msd_mixed
#load the promp definitions and instantiate a phase-state machine
with open(os.path.join(self.DefinitionsDirectory, 'phasta.yaml')) as f:
graphconfig = yaml.safe_load(f)
#create msd_generators:
self.size_states = graphconfig['number_of_states']
msd_generators = _np.array([[None] * self.size_states] *
self.size_states)
t = graphconfig['transition_names']
for transitionLabel in t: #for all transitions:
filename = os.path.join(DefinitionsDirectory,
"{}.promep.h5".format(transitionLabel))
p = promep.ProMeP.makeFromFile(filename)
msd_generators[[t[transitionLabel][1], t[transitionLabel][0]]] = p
for i, params in enumerate(
graphconfig['state_controllers']): #for all states:
c = staticprimitives.LTIGoal.makeFromDict(params)
msd_generators[i, i] = c
#initial node-global values:
self.msd_generators = msd_generators
self.phases = _np.zeros(
(self.phase_size_derivatives, self.size_states, self.size_states))
self.activations = _np.zeros(self.size_states, self.size_states)
self.activationsTime = None
self.validUntil = None
self.meansDesired = None
self.jacobianEE = None
self.jacobianBase = None
self.hTransform = None
self.hTransformGoal = None
try:
dynamicsModel = pandadynamicsmodel.PandaURDFModel()
except:
dynamicsModel = None
self.timeintegrator = mechanicalstate.TimeIntegrator(
self.tns, dynamicsModel=dynamicsModel)
self.kinematicsModel = pandadynamicsmodel.PandaURDFModel()
self.publishMarkers()
# Subscribe to Task Space Goal Marker
self.JacobianHTListener = rospy.Subscriber(
"/task_space_goal_marker/feedback",
InteractiveMarkerFeedback,
self.desiredTaskSpacePoseCallback,
queue_size=3)
rospy.loginfo("MSD mixer node initialized")