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;
}
}
}
void Camera::setFov(float fov) {
vfov = fov;
hfov = RtoD(2.0f*atanf(cam_aspect*tanf(DtoR(0.5f*(vfov)))));
update(0.0f);
}
