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); }