bool VfController::ReadConfig(const char* cfg_filename)
{
//YAML::Node doc;
//GetYamlDoc(cfg_filename, doc);
if (!M3Controller::ReadConfig(cfg_filename))
return false;
const YAML::Node& ik = doc["ik"];
double damp_max, epsilon;
ik["cart_mask"] >> cart_mask_str_;
ik["damp_max"] >> damp_max;
ik["epsilon"] >> epsilon;
Eigen::MatrixXd gains = MatrixXd::Zero(6,6);
ik["gain_x"] >> gains(0,0);
ik["gain_y"] >> gains(1,1);
ik["gain_z"] >> gains(2,2);
ik["gain_roll"] >> gains(3,3);
ik["gain_pitch"] >> gains(4,4);
ik["gain_yaw"] >> gains(5,5);
// Sample time
double dt = 1/static_cast<double>(RT_TASK_FREQUENCY);
std::string root_name = "T0"; //FIXME
std::string end_effector_name;
if(chain_name_ == "RIGHT_ARM")
end_effector_name = "palm_right";
else if(chain_name_ == "LEFT_ARM")
end_effector_name = "palm_left";
else
{
M3_ERR("Only RIGHT_ARM and LEFT_ARM are supported");
return false;
}
try
{
kin_ = new KDLClik (root_name,end_effector_name,damp_max,epsilon,gains,dt);
}
catch(const std::runtime_error& e)
{
M3_ERR("Failed to create kdl kinematics: ",e.what());
return false;
}
return true;
}
bool M3TorqueShm::LinkDependentComponents()
{
if (humanoid_name.size()!=0)
{
bot=dynamic_cast<M3Humanoid*>(factory->GetComponent(humanoid_name));
if (bot==NULL)
{
M3_ERR("M3Humanoid component %s declared for M3TorqueShm but could not be linked\n",
humanoid_name.c_str());
return false;
}
right_loadx6=(M3LoadX6*)factory->GetComponent(right_loadx6_name); //May be null if not on this robot model
left_loadx6=(M3LoadX6*)factory->GetComponent(left_loadx6_name);//May be null if not on this robot model
}
return true;
}
bool M3LedX2XNEcShm::LinkDependentComponents()
{
tmp_cnt = 0;
if (led_x2xn_name.size()!=0)
{
led_x2xn = dynamic_cast<M3LedX2XNEc*>(factory->GetComponent(led_x2xn_name));
if (led_x2xn==NULL)
{
M3_ERR("M3LedX2XNEc component %s declared for M3LedX2XNEcShm but could not be linked\n",
led_x2xn_name.c_str());
return false;
}
} else {
return false;
}
return true;
}
bool M3SerialPort::open(const std::string& port_name)
{
port =port_name;
fd = ::open(port.c_str(), O_RDWR | O_NOCTTY| O_NDELAY);
if (fd == -1)
{
/*
* Could not open the port.
*/
//perror("open_port: Unable to open /dev/ttyS0 - ");
M3_WARN("Unable to open serial port %s\n",port.c_str());
//string err = "Unable to open serial port: "+port;
//throw std::runtime_error(err);
return false;
}
fcntl(fd, F_SETFL, 0);
//fcntl(fd, F_SETFL, FNDELAY);//make return 0 if no byts
struct termios options;
/*
* Get the current options for the port...
*/
tcgetattr(fd, &options);
/*
* Set the baud rates to 19200...
*/
cfsetispeed(&options, B57600);
cfsetospeed(&options, B57600);
/*
* Enable the receiver and set local mode...
*/
options.c_cflag |= (CLOCAL | CREAD);
/* 8N1 */
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
//options.c_cc[VTIME] = 2; /* inter-character timer 0.2s timeout */
//options.c_cc[VMIN] = 1; /* blocking read until 1 chars received */
/*
* Set the new options for the port...
*/
tcsetattr(fd, TCSANOW, &options);
this->port = port;
#ifdef DEBUG
std::cerr << "Starting notifier on " << fd << std::endl;
#endif
stop_thread = false;
if( (rc1=pthread_create( &thread1, NULL, read_serial_thread, (void*)this)) )
{
M3_ERR("Thread creation failed: %d\n", rc1);
return false;
}
return true;
}
