void ControlledRobotSimulator::GetLinkTorques(Vector& t) const
{
Vector tact(robot->drivers.size());
t.resize(robot->links.size());
GetActuatorTorques(tact);
for(size_t i=0;i<robot->drivers.size();i++)
switch(robot->drivers[i].type) {
case RobotJointDriver::Affine:
{
for (size_t j=0;j<robot->drivers[i].linkIndices.size();j++)
t[robot->drivers[i].linkIndices[j]] = tact[i]*robot->drivers[i].affScaling[j];
break;
}
default:
{
for (size_t j=0;j<robot->drivers[i].linkIndices.size();j++)
t[robot->drivers[i].linkIndices[j]] = tact[i];
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, std::string("owd"));
#ifdef OWD_RT
if(mlockall(MCL_CURRENT | MCL_FUTURE) == -1){
ROS_FATAL("owd: mlockall failed: ");
return NULL;
}
#endif
// read parameters and set wam options
ros::NodeHandle n("~");
std::string calibration_filename;
int canbus_number;
std::string hand_type;
bool forcetorque;
bool modified_j1;
int pub_freq; // DEPRECATED
int wam_pub_freq;
int hand_pub_freq;
int ft_pub_freq;
int tactile_pub_freq;
n.param("calibration_file",calibration_filename,std::string("wam_joint_calibrations"));
n.param("canbus_number",canbus_number,0);
n.param("hand_type",hand_type,std::string("none"));
n.param("forcetorque_sensor",forcetorque,false);
n.param("modified_j1",modified_j1,false);
int wam_freq_default=10;
int hand_freq_default=10;
if (n.getParam("publish_frequency",pub_freq)) {
ROS_WARN("Parameter publish_frequency has been deprecated. Please use wam_publish_frequency, hand_publish_frequency, ft_publish_frequency, and tactile_publish_frequency.");
// we'll use the deprecated value to set the defaults for the
// individual hand and wam frequencies, so that it will only be
// used if the newer params are not specified
wam_freq_default=hand_freq_default=pub_freq;
}
n.param("wam_publish_frequency",wam_pub_freq,wam_freq_default);
n.param("hand_publish_frequency",hand_pub_freq,hand_freq_default);
n.param("ft_publish_frequency",ft_pub_freq,10);
n.param("tactile_publish_frequency",tactile_pub_freq,10);
if (wam_pub_freq > 500) {
ROS_WARN("value of wam_publish_frequency exceeds maximum sensor rate; capping to 500Hz");
wam_pub_freq = 500;
}
if (hand_pub_freq > 40) {
ROS_WARN("value of hand_publish_frequency exceeds maximum sensor rate; capping to 40Hz");
hand_pub_freq = 40;
}
if (ft_pub_freq > 500) {
ROS_WARN("value of ft_publish_frequency exceeds maximum sensor rate; capping to 500Hz");
ft_pub_freq = 500;
}
if (tactile_pub_freq > 40) {
ROS_WARN("value of tactile_publish_frequency exceeds maximum sensor rate; capping to 40Hz");
tactile_pub_freq = 40;
}
ROS_DEBUG("Using CANbus number %d",canbus_number);
int BH_model(0);
bool tactile(false);
if (! hand_type.compare(0,3,"280")) {
BH_model=280;
if (! hand_type.compare("280+TACT")) {
ROS_DEBUG("Expecting tactile sensors on this hand");
tactile=true;
}
} else if (! hand_type.compare(0,3,"260")) {
BH_model=260;
} else if (! hand_type.compare(0,29,"darpa_arms_calibration_target")) {
BH_model=999;
}
OWD::WamDriver *wamdriver = new OWD::WamDriver(canbus_number,BH_model,forcetorque,tactile);
if (modified_j1) {
wamdriver->SetModifiedJ1(true);
}
try {
if (! wamdriver->Init(calibration_filename.c_str())) {
ROS_FATAL("WamDriver::Init() returned false; exiting.");
delete wamdriver;
exit(1);
}
} catch (int error) {
ROS_FATAL("Error during WamDriver::Init(); exiting.");
delete wamdriver;
exit(1);
} catch (char *errmsg) {
ROS_FATAL("Error during WamDriver::Init(): %s",errmsg);
delete wamdriver;
exit(1);
}
#ifndef BH280_ONLY
try {
wamdriver->AdvertiseAndSubscribe(n);
} catch (int error) {
ROS_FATAL("Error during WamDriver::AdvertiseAndSubscribe(); exiting.");
delete wamdriver;
exit(1);
}
#endif // BH280_ONLY
#ifndef OWDSIM
BHD_280 *bhd(NULL);
if (BH_model == 280) {
bhd= new BHD_280(wamdriver->bus);
}
FT *ft(NULL);
if (forcetorque) {
ft = new FT(wamdriver->bus);
}
Tactile *tact(NULL);
if (tactile) {
tact = new Tactile(wamdriver->bus);
}
#endif // OWDSIM
#ifdef BUILD_FOR_SEA
usleep(100000);
wamdriver->publishAllSeaSettings();
#endif // BUILD_FOR_SEA
ROS_DEBUG("Creating timer and spinner threads");
#ifndef BH280_ONLY
ros::Timer wam_timer = n.createTimer(ros::Rate(wam_pub_freq).expectedCycleTime(), &OWD::WamDriver::Pump, wamdriver);
#endif // BH280_ONLY
#ifndef OWDSIM
ros::Timer bhd_timer;
if (bhd) {
bhd_timer = n.createTimer(ros::Rate(hand_pub_freq).expectedCycleTime(), &BHD_280::Pump, bhd);
}
ros::Timer ft_timer;
if (ft) {
ft_timer = n.createTimer(ros::Rate(ft_pub_freq).expectedCycleTime(), &FT::Pump, ft);
}
ros::Timer tactile_timer;
if (tact) {
tactile_timer = n.createTimer(ros::Rate(tactile_pub_freq).expectedCycleTime(), &Tactile::Pump, tact);
}
#endif // OWDSIM
ros::MultiThreadedSpinner s(3);
ROS_DEBUG("Spinning");
ros::spin(s);
ROS_DEBUG("Done spinning; exiting");
#ifndef OWDSIM
while (wamdriver->owam->bus) {
usleep(10000);
}
#endif
return(0);
}
