void
Arbiter::arbitrate(const ArbiterMessage& _message)
{
// no anonymous messages allowed
if (_message.id) {
// arbiter could be disabled due to a transition
if (active_) {
Guard guard(mutex_);
// get behaviours priority
ArbiterParameters::RegistrationMap::const_iterator i
= params_->priorities.find(_message.id);
// if the behaviour belongs to the current action pattern
if (i != params_->priorities.end()) {
// copy the message
message_[i->second]->assign(_message);
// set actuators according to arbitration message
calcActivation();
}
else {
std::cerr << "PriorityArbiter: got message from unregistered behaviour: "
<< _message.id->getBehaviourName() << std::endl;
}
}
else {
std::cerr << "PriorityArbiter: got arbitrate call while not active from behaviour: "
<< _message.id->getBehaviourName() << std::endl;
}
}
else
std::cerr << "PriorityArbiter: received message without behaviour id." << std::endl;
}
/**
* Called on transition to a new action pattern. If the arbiter is currently
* inactive open is called afterwards. Note that if the arbiter is already
* part of the current behaviour, arbitrate can be called concurrently.
*/
void
Arbiter::init(const ArbiterParameters * _params)
{
Guard guard(mutex_);
// convert current arbitration to new configuration
// build new message vector
MessageVector tmp(_params->priorities.size(), NULL);
MessageVector::iterator first, last = tmp.end();
for (first = tmp.begin(); first != last; ++first)
delete *first;
// copy old message values if available
// allways except first time
if (params_) {
ArbiterParameters::RegistrationMap::const_iterator i, j;
// for each behaviour
for (i = _params->priorities.begin(); i != _params->priorities.end(); ++i) {
// if it existed before
j = params_->priorities.find(i->first);
if (j != params_->priorities.end()) {
// copy its current arbitration message to
// the new index in the new message vector
tmp[i->second] = message_[j->second];
// remove copy of pointer to avoid confusion
message_[j->second] = NULL;
}
else {
// else build new entry
tmp[i->second] = getMessageInstance();
}
}
}
else
for (first = tmp.begin(); first != last; ++first)
*first = getMessageInstance();
// swap old and new message vectors
message_.swap(tmp);
// delete old messages
last = tmp.end();
for (first = tmp.begin(); first != last; ++first)
delete *first;
// create local copy of current priority mapping
// for next arbitration change
params_ = _params;
// new activations
calcActivation();
}
//----------------------------------------------------------------------------------------------------------------------
void Muscle::update(float dt)
{
double prevLength = m_length;
updateLengthAndMomentArm();
#if DEBUGGING
// Todo: Temporary check that the min/max muscle length calculations are correct
double delta = 0.00001;
bool lengthOK = (m_length - m_lengthMin) >= -delta && (m_length - m_lengthMax) < delta ;
if(!lengthOK)
{
std::cout << getName() << "\t";
std::cout << "Length Error: " << m_length << " " << m_lengthMin << " " << m_lengthMax << " " << m_lengthOpt;
std::cout << ". Angles:" << m_arm->getJointAngle(JT_elbow) << " " << m_arm->getJointAngle(JT_shoulder);
std::cout << ". Wraps: " << ((MuscleMonoWrap*)this)->getWraps() << std::endl;
m_length = clamp(m_length, m_lengthMin, m_lengthMax);
}
assert(lengthOK);
#endif
m_velocity = (m_length - prevLength) / dt; // lengthening(!) velocity.
// Dimensionless variables for Hill-model
m_lengthNorm = m_length / m_lengthOpt;
m_velocityNorm = m_velocity / m_maxVelocity;
// Length scaled to unit interval (0,1)
m_lengthUnit = lengthToUnitLength(m_length);
assert(m_lengthUnit >= 0.0 && m_lengthUnit <= 1.0);
// Muscle activation from neural excitation
m_activation = calcActivation(m_activation, m_excitation, dt);
// Calculate individual constitutive forces
m_passiveForceNorm = calcPassiveForceNorm(m_lengthNorm);
m_activeForceNorm = calcActiveForceNorm(m_lengthNorm);
m_velocityForceNorm = calcVelocityForceNorm(m_velocityNorm);
// Calculate overall response
m_force = (m_activation * m_maxForce * m_activeForceNorm * m_velocityForceNorm) + (m_maxForce * m_passiveForceNorm);
}
