bool agent::compute::discovery::check_ipmi_response(
const ipmi::Response& response,
const ipmi::Response::COMPLETION_CODE code) {
if (code == response.get_completion_code()) {
return true;
}
log_debug(GET_LOGGER("rpc"), "IPMI completion code: "
<< std::uint32_t(response.get_completion_code()));
return false;
}
void ControlledRobotSimulator::GetSensedVelocity(Config& dq)
{
JointVelocitySensor* s=sensors.GetTypedSensor<JointVelocitySensor>();
if(s==NULL){
LOG4CXX_ERROR(GET_LOGGER(ControlledRobotSimulator),"ControlledRobotSimulator::GetSensedVelocity: Warning, robot has no joint velocity sensor");
}
else {
//resize to the correct size
if(s->indices.empty() || s->dq.empty())
dq = s->dq;
else {
dq.resize(robot->dq.n);
dq.set(0.0);
for(size_t i=0;i<s->indices.size();i++)
dq[s->indices[i]] = s->dq[i];
}
}
}
void ControlledRobotSimulator::GetSensedConfig(Config& q)
{
JointPositionSensor* s = sensors.GetTypedSensor<JointPositionSensor>();
if(s==NULL){
LOG4CXX_ERROR(GET_LOGGER(ControlledRobotSimulator),"ControlledRobotSimulator::GetSensedConfig: Warning, robot has no joint position sensor");
}
else {
//resize to the correct size
if(s->indices.empty() || s->q.empty())
q = s->q;
else {
q.resize(robot->q.n);
q.set(0.0);
for(size_t i=0;i<s->indices.size();i++)
q[s->indices[i]] = s->q[i];
}
}
}
void ControlledRobotSimulator::GetCommandedConfig(Config& q)
{
Assert(command.actuators.size() == robot->drivers.size());
robot->q.set(0.0);
bool warned=false;
for(size_t i=0;i<command.actuators.size();i++) {
RobotJointDriver& d=robot->drivers[i];
if(command.actuators[i].mode == ActuatorCommand::PID)
robot->SetDriverValue(i,command.actuators[i].qdes);
else {
if(!warned)
LOG4CXX_ERROR(GET_LOGGER(ControlledRobotSimulator),"ControlledRobotSimulator::GetCommandedConfig: Can't get commanded config for non-PID drivers");
warned = true;
//robot->SetDriverValue(i,0.0);
}
}
q = robot->q;
}
void ControlledRobotSimulator::GetActuatorTorques(Vector& t) const
{
if(t.empty()) t.resize(robot->drivers.size());
if(t.n != (int)robot->drivers.size()) {
LOG4CXX_WARN(GET_LOGGER(ControlledRobotSimulator),"ControlledRobotSimulator::GetActuatorTorques: Warning, vector isn't sized to the number of drivers "<<robot->drivers.size()<<" (got "<<t.n);
if(t.n == (int)robot->links.size())
LOG4CXX_WARN(GET_LOGGER(ControlledRobotSimulator)," (Did you mean GetLinkTorques()?");
}
Assert(command.actuators.size() == robot->drivers.size());
t.resize(command.actuators.size());
for(size_t i=0;i<command.actuators.size();i++) {
const RobotJointDriver& d=robot->drivers[i];
Real q=oderobot->GetDriverValue(i);
Real dq=oderobot->GetDriverVelocity(i);
int link = d.linkIndices[0];
if(q < robot->qMin(link)) {
if(q + TwoPi >= robot->qMin(link) && q + TwoPi <= robot->qMax(link))
q += TwoPi;
}
else if(q > robot->qMax(link)) {
if(q - TwoPi <= robot->qMax(link) && q - TwoPi >= robot->qMin(link))
q -= TwoPi;
}
if(q < robot->qMin(link)-gJointLimitWarningThreshold || q > robot->qMax(link)+gJointLimitWarningThreshold) {
printf("Warning: joint angle %s out of bounds\n",robot->linkNames[link].c_str());
printf("q=%g, qmin=%g, qmax=%g (deg)\n",RtoD(q),RtoD(robot->qMin(link)),RtoD(robot->qMax(link)));
//getchar();
}
const ActuatorCommand& cmd=command.actuators[i];
switch(cmd.mode) {
case ActuatorCommand::OFF:
printf("Warning: actuator off?\n");
t(i) = 0;
break;
case ActuatorCommand::TORQUE:
//printf("Warning: direct torque?\n");
if(cmd.torque < d.tmin-gTorqueLimitWarningThreshold)
printf("Actuator %s limit exceeded: %g < %g\n",robot->LinkName(robot->drivers[i].linkIndices[0]).c_str(),cmd.torque,d.tmin);
else if(cmd.torque > d.tmax+gTorqueLimitWarningThreshold)
printf("Actuator %s limit exceeded: %g > %g\n",robot->LinkName(robot->drivers[i].linkIndices[0]).c_str(),cmd.torque,d.tmax);
t(i) = Clamp(cmd.torque,d.tmin,d.tmax);
break;
case ActuatorCommand::PID:
{
//TODO: simulate low level errors in the PID loop
Real cmdtorque = cmd.GetPIDTorque(q,dq);
if(cmdtorque < d.tmin-gTorqueLimitWarningThreshold)
printf("Actuator %s limit exceeded: %g < %g\n",robot->LinkName(robot->drivers[i].linkIndices[0]).c_str(),cmdtorque,d.tmin);
else if(cmdtorque > d.tmax+gTorqueLimitWarningThreshold)
printf("Actuator %s limit exceeded: %g > %g\n",robot->LinkName(robot->drivers[i].linkIndices[0]).c_str(),cmdtorque,d.tmax);
Real td=Clamp(cmdtorque,d.tmin,d.tmax);
//printf("%d: Current %g,%g, desired %g,%g, torque desired %g, clamped %g\n",i,q,dq,cmd.qdes,cmd.dqdes,cmd.GetPIDTorque(q,dq),td);
t(i) = td;
break;
}
case ActuatorCommand::LOCKED_VELOCITY:
t(i) = 0;
break;
}
}
}
