SUPPRESS_OFFSET_WARNING_BEGIN
int BackgroundFilterParameters::staticInit()
{
ReflectionNaming &nameing = naming();
nameing = ReflectionNaming(
"Background Filter Parameters",
"Background Filter Parameters",
""
);
fields().push_back(
new IntField
(
BackgroundFilterParameters::THRESHOLD_ID,
offsetof(BackgroundFilterParameters, mThreshold),
100,
"Threshold",
"Threshold",
"Threshold"
)
);
return 0;
}
SUPPRESS_OFFSET_WARNING_BEGIN
int ThickeningParameters::staticInit()
{
ReflectionNaming &nameing = naming();
nameing = ReflectionNaming(
"Thickening Parameters",
"Thickening Parameters",
""
);
fields().push_back(
new IntField
(
ThickeningParameters::POWER_ID,
offsetof(ThickeningParameters, mPower),
5000,
"Power",
"Power",
"Power"
)
);
return 0;
}
int initializeOpenHRPModel (char* _filename)
{
int rtmargc=0;
char** argv=NULL;
//std::vector<char *> rtmargv;
//rtmargv.push_back(argv[0]);
rtmargc++;
RTC::Manager* manager;
//manager = RTC::Manager::init(rtmargc, rtmargv.data());
manager = RTC::Manager::init(rtmargc, argv);
std::string nameServer = manager->getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
std::string filename(_filename);
if (!loadBodyFromModelLoader(m_robot, filename.c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()),
true)){
std::cerr << print_prefix << " Failed to load model[" << filename << "]" << std::endl;
return 1;
} else {
std::cerr << print_prefix << " Success to load model[" << filename << "]" << std::endl;
}
return 0;
};
SUPPRESS_OFFSET_WARNING_BEGIN
int InputFilterParameters::staticInit()
{
ReflectionNaming &nameing = naming();
nameing = ReflectionNaming(
"Input Filter Parameters",
"Input Filter Parameters",
""
);
fields().push_back(
new EnumField
(
InputFilterParameters::INPUT_TYPE_ID,
offsetof(InputFilterParameters, mInputType),
0,
"Input Type",
"Input Type",
"Input Type",
new EnumReflection(2
, new EnumOption(0,"Left Frame")
, new EnumOption(1,"Right Frame")
)
)
);
return 0;
}
RTC::ReturnCode_t ForwardKinematics::onInitialize()
{
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
coil::Properties& ref = getProperties();
bindParameter("sensorAttachedLink", m_sensorAttachedLinkName, ref["conf.default.sensorAttachedLink"].c_str());
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("q", m_qIn);
addInPort("sensorRpy", m_sensorRpyIn);
addInPort("qRef", m_qRefIn);
addInPort("basePosRef", m_basePosRefIn);
addInPort("baseRpyRef", m_baseRpyRefIn);
// Set OutPort buffer
// Set service provider to Ports
m_ForwardKinematicsServicePort.registerProvider("service0", "ForwardKinematicsService", m_service0);
addPort(m_ForwardKinematicsServicePort);
m_service0.setComp(this);
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
RTC::Properties& prop = getProperties();
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
m_refBody = hrp::BodyPtr(new hrp::Body());
if (!loadBodyFromModelLoader(m_refBody, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
m_actBody = hrp::BodyPtr(new hrp::Body());
if (!loadBodyFromModelLoader(m_actBody, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
m_refLink = m_refBody->rootLink();
m_actLink = m_actBody->rootLink();
return RTC::RTC_OK;
}
RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onInitialize()
{
m_SequencePlayerServicePort.registerConsumer("service0", "SequencePlayerService", m_service0);
addPort(m_SequencePlayerServicePort);
RTC::Properties& prop = getProperties();
body = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0)
{
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
std::string modelfile = m_pManager->getConfig()["model"];
if (!loadBodyFromModelLoader(body, modelfile.c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext())))
{
std::cerr << "[HrpsysJointTrajectoryBridge] failed to load model[" << prop["model"] << "]" << std::endl;
}
bool ret = false;
while (!ret)
{
try
{
bodyinfo = hrp::loadBodyInfo(modelfile.c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()));
ret = loadBodyFromBodyInfo(body, bodyinfo);
}
catch (CORBA::SystemException& ex)
{
std::cerr << "[HrpsysJointTrajectoryBridge] loadBodyInfo(): CORBA::SystemException " << ex._name() << std::endl;
sleep(1);
}
catch (...)
{
std::cerr << "[HrpsysJointTrajectoryBridge] loadBodyInfo(): failed to load model[" << modelfile << "]"
<< std::endl;
;
sleep(1);
}
}
if (body == NULL)
{
return RTC::RTC_ERROR;
}
body->calcForwardKinematics();
ROS_INFO_STREAM("[HrpsysJointTrajectoryBridge] @Initilize name : " << getInstanceName() << " done");
return RTC::RTC_OK;
}
Monitor::Monitor(CORBA::ORB_var orb, const std::string &i_hostname,
int i_port, int i_interval, LogManager<TimedRobotState> *i_log) :
m_orb(orb),
m_rhCompName("RobotHardware0"),
m_shCompName("StateHolder0"),
m_interval(i_interval),
m_log(i_log)
{
char buf[128];
sprintf(buf, "%s:%d", i_hostname.c_str(), i_port);
RTC::CorbaNaming naming(orb, buf);
m_naming = CosNaming::NamingContext::_duplicate(naming.getRootContext());
}
int find_type(char dirname[], char name[]){ // 0 directory 1 file -1 error
struct stat statbuf;
char newname[255];
strcpy(newname, naming(dirname, name));
if (stat(newname, &statbuf) < 0)
{ //perror(newname);
return -1;
}
if ((statbuf.st_mode & S_IFMT) == S_IFDIR )
{ return 0; // directory encountered
}
else
{ return 1; // file encountered
}
}
void PyLink::addShapeFromFile(std::string url)
{
RTC::Manager* manager = &RTC::Manager::instance();
std::string nameServer = manager->getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(manager->getORB(), nameServer.c_str());
OpenHRP::ModelLoader_var modelloader = hrp::getModelLoader(CosNaming::NamingContext::_duplicate(naming.getRootContext()));
OpenHRP::ModelLoader::ModelLoadOption opt;
opt.readImage = true;
opt.AABBdata.length(0);
opt.AABBtype = OpenHRP::ModelLoader::AABB_NUM;
OpenHRP::BodyInfo_var binfo = modelloader->getBodyInfoEx(url.c_str(), opt);
OpenHRP::LinkInfoSequence_var lis = binfo->links();
loadShapeFromLinkInfo(this, lis[0], binfo, createPyShape);
}
RTC::ReturnCode_t StateHolder::onInitialize()
{
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("currentQIn", m_currentQIn);
addInPort("qIn", m_qIn);
addInPort("tqIn", m_tqIn);
addInPort("basePosIn", m_basePosIn);
addInPort("baseRpyIn", m_baseRpyIn);
addInPort("zmpIn", m_zmpIn);
addInPort("optionalDataIn", m_optionalDataIn);
// Set OutPort buffer
addOutPort("qOut", m_qOut);
addOutPort("tqOut", m_tqOut);
addOutPort("basePosOut", m_basePosOut);
addOutPort("baseRpyOut", m_baseRpyOut);
addOutPort("baseTformOut", m_baseTformOut);
addOutPort("basePoseOut", m_basePoseOut);
addOutPort("zmpOut", m_zmpOut);
addOutPort("optionalDataOut", m_optionalDataOut);
// Set service provider to Ports
m_StateHolderServicePort.registerProvider("service0", "StateHolderService", m_service0);
m_TimeKeeperServicePort.registerProvider("service1", "TimeKeeperService", m_service1);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_StateHolderServicePort);
addPort(m_TimeKeeperServicePort);
// </rtc-template>
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
std::cout << "StateHolder: dt = " << m_dt << std::endl;
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
OpenHRP::BodyInfo_var binfo;
binfo = hrp::loadBodyInfo(prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()));
OpenHRP::LinkInfoSequence_var lis = binfo->links();
const OpenHRP::LinkInfo& li = lis[0];
hrp::Vector3 p, axis;
p << li.translation[0], li.translation[1], li.translation[2];
axis << li.rotation[0], li.rotation[1], li.rotation[2];
hrp::Matrix33 R = hrp::rodrigues(axis, li.rotation[3]);
hrp::Vector3 rpy = hrp::rpyFromRot(R);
m_baseTform.data.length(12);
double *T = m_baseTform.data.get_buffer();
T[0] = R(0,0); T[1] = R(0,1); T[ 2] = R(0,2); T[ 3] = p[0];
T[4] = R(0,0); T[5] = R(0,1); T[ 6] = R(0,2); T[ 7] = p[1];
T[8] = R(0,0); T[9] = R(0,1); T[10] = R(0,2); T[11] = p[2];
m_basePos.data.x = m_basePose.data.position.x = p[0];
m_basePos.data.y = m_basePose.data.position.y = p[1];
m_basePos.data.z = m_basePose.data.position.z = p[2];
m_baseRpy.data.r = m_basePose.data.orientation.r = rpy[0];
m_baseRpy.data.p = m_basePose.data.orientation.p = rpy[1];
m_baseRpy.data.y = m_basePose.data.orientation.y = rpy[2];
m_zmp.data.x = m_zmp.data.y = m_zmp.data.z = 0.0;
// Setting for wrench data ports (real + virtual)
std::vector<std::string> fsensor_names;
// find names for real force sensors
for ( int k = 0; k < lis->length(); k++ ) {
OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
for ( int l = 0; l < sensors.length(); l++ ) {
if ( std::string(sensors[l].type) == "Force" ) {
fsensor_names.push_back(std::string(sensors[l].name));
}
}
}
int npforce = fsensor_names.size();
// find names for virtual force sensors
coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
int nvforce = virtual_force_sensor.size()/10;
for (unsigned int i=0; i<nvforce; i++){
fsensor_names.push_back(virtual_force_sensor[i*10+0]);
}
// add ports for all force sensors
int nforce = npforce + nvforce;
m_wrenches.resize(nforce);
m_wrenchesIn.resize(nforce);
m_wrenchesOut.resize(nforce);
for (unsigned int i=0; i<nforce; i++){
m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenches[i]);
m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
m_wrenches[i].data.length(6);
m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
registerInPort(std::string(fsensor_names[i]+"In").c_str(), *m_wrenchesIn[i]);
registerOutPort(std::string(fsensor_names[i]+"Out").c_str(), *m_wrenchesOut[i]);
}
return RTC::RTC_OK;
}
SUPPRESS_OFFSET_WARNING_BEGIN
int DistortionApplicationParameters::staticInit()
{
ReflectionNaming &nameing = naming();
nameing = ReflectionNaming(
"Distortion Application Parameters",
"Distortion Application Parameters",
""
);
fields().push_back(
new BoolField
(
DistortionApplicationParameters::FORCE_SCALE_ID,
offsetof(DistortionApplicationParameters, mForceScale),
false,
"Force Scale",
"Force Scale",
"Force Scale"
)
);
fields().push_back(
new BoolField
(
DistortionApplicationParameters::ADOPT_SCALE_ID,
offsetof(DistortionApplicationParameters, mAdoptScale),
false,
"Adopt Scale",
"Adopt Scale",
"Adopt Scale"
)
);
fields().push_back(
new EnumField
(
DistortionApplicationParameters::RESIZE_POLICY_ID,
offsetof(DistortionApplicationParameters, mResizePolicy),
3,
"Resize policy",
"Resize policy",
"Resize policy",
new EnumReflection(4
, new EnumOption(0,"No Change")
, new EnumOption(1,"Force Size")
, new EnumOption(2,"To Fit Result")
, new EnumOption(3,"To No Gaps")
)
)
);
fields().push_back(
new IntField
(
DistortionApplicationParameters::NEW_H_ID,
offsetof(DistortionApplicationParameters, mNewH),
0,
"New H",
"New H",
"New H"
)
);
fields().push_back(
new IntField
(
DistortionApplicationParameters::NEW_W_ID,
offsetof(DistortionApplicationParameters, mNewW),
0,
"New W",
"New W",
"New W"
)
);
return 0;
}
SUPPRESS_OFFSET_WARNING_BEGIN
int BaseParameters::staticInit()
{
ReflectionNaming &nameing = naming();
nameing = ReflectionNaming(
"Base Parameters",
"Base parameters",
""
);
fields().push_back(
new EnumField
(
BaseParameters::ROTATION_ID,
offsetof(BaseParameters, mRotation),
0,
"rotation",
"rotation",
"rotation",
NULL
)
);
fields().push_back(
new BoolField
(
BaseParameters::MIRROR_ID,
offsetof(BaseParameters, mMirror),
false,
"mirror",
"mirror",
"mirror"
)
);
fields().push_back(
new BoolField
(
BaseParameters::SWAPCAMERAS_ID,
offsetof(BaseParameters, mSwapCameras),
false,
"swapCameras",
"swapCameras",
"swapCameras"
)
);
fields().push_back(
new BoolField
(
BaseParameters::FILTERLOCK_ID,
offsetof(BaseParameters, mFilterLock),
false,
"filterLock",
"filterLock",
"filterLock"
)
);
fields().push_back(
new BoolField
(
BaseParameters::ENABLEFILTERGRAPH_ID,
offsetof(BaseParameters, mEnableFilterGraph),
false,
"enableFilterGraph",
"enableFilterGraph",
"enableFilterGraph"
)
);
fields().push_back(
new DoubleField
(
BaseParameters::DOWNSAMPLE_ID,
offsetof(BaseParameters, mDownsample),
1.5,
"downsample",
"downsample",
"downsample"
)
);
fields().push_back(
new IntField
(
BaseParameters::H_ID,
offsetof(BaseParameters, mH),
640,
"h",
"h",
"h"
)
);
fields().push_back(
new IntField
(
BaseParameters::W_ID,
offsetof(BaseParameters, mW),
480,
"w",
"w",
"w"
)
);
fields().push_back(
new BoolField
(
BaseParameters::AUTOH_ID,
offsetof(BaseParameters, mAutoH),
true,
"autoH",
"autoH",
"autoH"
)
);
fields().push_back(
new BoolField
(
BaseParameters::AUTOW_ID,
offsetof(BaseParameters, mAutoW),
true,
"autoW",
"autoW",
"autoW"
)
);
fields().push_back(
new IntField
(
BaseParameters::X_ID,
offsetof(BaseParameters, mX),
0,
"x",
"x",
"x"
)
);
fields().push_back(
new IntField
(
BaseParameters::Y_ID,
offsetof(BaseParameters, mY),
0,
"y",
"y",
"y"
)
);
fields().push_back(
new EnumField
(
BaseParameters::INTERPOLATIONTYPE_ID,
offsetof(BaseParameters, mInterpolationType),
0,
"InterpolationType",
"InterpolationType",
"InterpolationType",
NULL
)
);
return 0;
}
RTC::ReturnCode_t ReferenceForceUpdater::onInitialize()
{
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qRef", m_qRefIn);
addInPort("basePosIn", m_basePosIn);
addInPort("baseRpyIn",m_baseRpyIn);
addInPort("rpy",m_rpyIn);
// Set service provider to Ports
m_ReferenceForceUpdaterServicePort.registerProvider("service0", "ReferenceForceUpdaterService", m_ReferenceForceUpdaterService);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_ReferenceForceUpdaterServicePort);
// Get dt
RTC::Properties& prop = getProperties(); // get properties information from .wrl file
coil::stringTo(m_dt, prop["dt"].c_str());
// Make m_robot instance
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), // load robot model for m_robot
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
// Setting for wrench data ports (real + virtual)
std::vector<std::string> fsensor_names;
// find names for real force sensors
unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
for (unsigned int i=0; i<npforce; i++){
fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
}
// load virtual force sensors
readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
unsigned int nvforce = m_vfs.size();
for (unsigned int i=0; i<nvforce; i++){
for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
if (it->second.id == (int)i) fsensor_names.push_back(it->first);
}
}
// add ports for all force sensors (real force, input and output of ref_force)
unsigned int nforce = npforce + nvforce;
m_force.resize(nforce);
m_forceIn.resize(nforce);
m_ref_force_in.resize(nforce);
m_ref_force_out.resize(nforce);
m_ref_forceIn.resize(nforce);
m_ref_forceOut.resize(nforce);
std::cerr << "[" << m_profile.instance_name << "] create force sensor ports" << std::endl;
for (unsigned int i=0; i<nforce; i++){
// actual inport
m_forceIn[i] = new InPort<TimedDoubleSeq>(fsensor_names[i].c_str(), m_force[i]);
m_force[i].data.length(6);
registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]);
// ref inport
m_ref_force_in[i].data.length(6);
for (unsigned int j=0; j<6; j++) m_ref_force_in[i].data[j] = 0.0;
m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force_in[i]);
registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]);
std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl;
// ref Outport
m_ref_force_out[i].data.length(6);
for (unsigned int j=0; j<6; j++) m_ref_force_out[i].data[j] = 0.0;
m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"Out").c_str(), m_ref_force_out[i]);
registerOutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), *m_ref_forceOut[i]);
std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl;
}
// setting from conf file
// rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle)
coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
if (end_effectors_str.size() > 0) {
size_t prop_num = 10;
size_t num = end_effectors_str.size()/prop_num;
for (size_t i = 0; i < num; i++) {
std::string ee_name, ee_target, ee_base;
coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
ee_trans eet;
for (size_t j = 0; j < 3; j++) {
coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
}
double tmpv[4];
for (int j = 0; j < 4; j++ ) {
coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
}
eet.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
eet.target_name = ee_target;
ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
ReferenceForceUpdaterParam rfu_param;
//set rfu param
rfu_param.update_count = round((1/rfu_param.update_freq)/m_dt);
if (( ee_name != "rleg" ) && ( ee_name != "lleg" ))
m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name , rfu_param));
ee_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
ref_force.push_back(hrp::Vector3::Zero());
//ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt)));
ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
if (( ee_name != "lleg" ) && ( ee_name != "rleg" )) transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl;
std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl;
std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
}
}
// check if the dof of m_robot match the number of joint in m_robot
unsigned int dof = m_robot->numJoints();
for ( unsigned int i = 0 ; i < dof; i++ ){
if ( (int)i != m_robot->joint(i)->jointId ) {
std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl;
return RTC::RTC_ERROR;
}
}
loop = 0;
transition_interpolator_ratio.reserve(transition_interpolator.size());
for (unsigned int i=0; i<transition_interpolator.size(); i++ ) transition_interpolator_ratio[i] = 0;
return RTC::RTC_OK;
}
RTC::ReturnCode_t CollisionDetector::onInitialize()
{
std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qRef", m_qRefIn);
addInPort("qCurrent", m_qCurrentIn);
addInPort("servoStateIn", m_servoStateIn);
// Set OutPort buffer
addOutPort("q", m_qOut);
addOutPort("beepCommand", m_beepCommandOut);
// Set service provider to Ports
m_CollisionDetectorServicePort.registerProvider("service0", "CollisionDetectorService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_CollisionDetectorServicePort);
// </rtc-template>
//RTC::Properties& prop = getProperties();
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
if ( prop["collision_viewer"] == "true" ) {
m_use_viewer = true;
}
#ifdef USE_HRPSYSUTIL
m_glbody = new GLbody();
m_robot = hrp::BodyPtr(m_glbody);
#else
m_robot = hrp::BodyPtr(new hrp::Body());
#endif // USE_HRPSYSUTIL
//
OpenHRP::BodyInfo_var binfo;
binfo = hrp::loadBodyInfo(prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()));
if (CORBA::is_nil(binfo)){
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
<< std::endl;
return RTC::RTC_ERROR;
}
#ifdef USE_HRPSYSUTIL
if (!loadBodyFromBodyInfo(m_robot, binfo, true, GLlinkFactory)) {
#else
if (!loadBodyFromBodyInfo(m_robot, binfo, true, hrplinkFactory)) {
#endif // USE_HRPSYSUTIL
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
}
#ifdef USE_HRPSYSUTIL
loadShapeFromBodyInfo(m_glbody, binfo);
#endif // USE_HRPSYSUTIL
if ( prop["collision_model"] == "AABB" ) {
convertToAABB(m_robot);
} else if ( prop["collision_model"] == "convex hull" ||
prop["collision_model"] == "" ) { // set convex hull as default
convertToConvexHull(m_robot);
}
setupVClipModel(m_robot);
if ( prop["collision_pair"] != "" ) {
std::cerr << "[" << m_profile.instance_name << "] prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
std::istringstream iss(prop["collision_pair"]);
std::string tmp;
while (getline(iss, tmp, ' ')) {
size_t pos = tmp.find_first_of(':');
std::string name1 = tmp.substr(0, pos), name2 = tmp.substr(pos+1);
if ( m_robot->link(name1)==NULL ) {
std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name1 << std::endl;
std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
for (unsigned int i=0; i < m_robot->numLinks(); i++) {
std::cerr << " " << m_robot->link(i)->name;
}
std::cerr << std::endl;
continue;
}
if ( m_robot->link(name2)==NULL ) {
std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name2 << std::endl;
std::cerr << "[" << m_profile.instance_name << "] please choose one of following :";
for (unsigned int i=0; i < m_robot->numLinks(); i++) {
std::cerr << " " << m_robot->link(i)->name;
}
std::cerr << std::endl;
continue;
}
std::cerr << "[" << m_profile.instance_name << "] check collisions between " << m_robot->link(name1)->name << " and " << m_robot->link(name2)->name << std::endl;
m_pair[tmp] = new CollisionLinkPair(new VclipLinkPair(m_robot->link(name1), m_VclipLinks[m_robot->link(name1)->index],
m_robot->link(name2), m_VclipLinks[m_robot->link(name2)->index], 0));
}
}
if ( prop["collision_loop"] != "" ) {
coil::stringTo(m_collision_loop, prop["collision_loop"].c_str());
std::cerr << "[" << m_profile.instance_name << "] set collision_loop: " << m_collision_loop << std::endl;
}
#ifdef USE_HRPSYSUTIL
if ( m_use_viewer ) {
m_scene.addBody(m_robot);
GLlink::drawMode(GLlink::DM_COLLISION);
}
#endif // USE_HRPSYSUTIL
// true (1) do not move when collide,
// false(0) move even if collide
m_curr_collision_mask.resize(m_robot->numJoints()); // collision_mask used to select output 0: passthough reference data, 1 output safe data
std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0);
m_init_collision_mask.resize(m_robot->numJoints()); // collision_mask defined in .conf as [collisoin_mask] 0: move even if collide, 1: do not move when collide
std::fill(m_init_collision_mask.begin(), m_init_collision_mask.end(), 1);
if ( prop["collision_mask"] != "" ) {
std::cerr << "[" << m_profile.instance_name << "] prop[collision_mask] ->" << prop["collision_mask"] << std::endl;
coil::vstring mask_str = coil::split(prop["collision_mask"], ",");
if (mask_str.size() == m_robot->numJoints()) {
for (size_t i = 0; i < m_robot->numJoints(); i++) {coil::stringTo(m_init_collision_mask[i], mask_str[i].c_str()); }
for (size_t i = 0; i < m_robot->numJoints(); i++) {
if ( m_init_collision_mask[i] == 0 ) {
std::cerr << "[" << m_profile.instance_name << "] CollisionDetector will not control " << m_robot->joint(i)->name << std::endl;
}
}
}else{
std::cerr << "[" << m_profile.instance_name << "] ERROR size of collision_mask is differ from robot joint number .. " << mask_str.size() << ", " << m_robot->numJoints() << std::endl;
}
}
if ( prop["use_limb_collision"] != "" ) {
std::cerr << "[" << m_profile.instance_name << "] prop[use_limb_collision] -> " << prop["use_limb_collision"] << std::endl;
if ( prop["use_limb_collision"] == "true" ) {
m_use_limb_collision = true;
std::cerr << "[" << m_profile.instance_name << "] Enable use_limb_collision" << std::endl;
}
}
// setup collision state
m_state.angle.length(m_robot->numJoints());
m_state.collide.length(m_robot->numLinks());
// allocate memory for outPorts
m_q.data.length(m_robot->numJoints());
m_recover_time = 0;
m_safe_posture = true;
m_have_safe_posture = false;
i_dt = 1.0;
default_recover_time = 2.5/m_dt;
m_recover_jointdata = new double[m_robot->numJoints()];
m_lastsafe_jointdata = new double[m_robot->numJoints()];
m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
m_link_collision = new bool[m_robot->numLinks()];
for(unsigned int i=0; i<m_robot->numJoints(); i++){
m_q.data[i] = 0;
}
m_servoState.data.length(m_robot->numJoints());
for(unsigned int i = 0; i < m_robot->numJoints(); i++) {
m_servoState.data[i].length(1);
int status = 0;
status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
status |= 1<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
status |= 1<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
status |= 0<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
status |= 0<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
m_servoState.data[i][0] = status;
}
collision_beep_freq = static_cast<int>(1.0/(3.0*m_dt)); // 3 times / 1[s]
m_beepCommand.data.length(bc.get_num_beep_info());
return RTC::RTC_OK;
}
RTC::ReturnCode_t CollisionDetector::onFinalize()
{
delete[] m_recover_jointdata;
delete[] m_lastsafe_jointdata;
delete m_interpolator;
delete[] m_link_collision;
return RTC::RTC_OK;
}
/*
RTC::ReturnCode_t CollisionDetector::onStartup(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t CollisionDetector::onShutdown(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
RTC::ReturnCode_t CollisionDetector::onActivated(RTC::UniqueId ec_id)
{
std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
m_have_safe_posture = false;
return RTC::RTC_OK;
}
RTC::ReturnCode_t RemoveForceSensorLinkOffset::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qCurrent", m_qCurrentIn);
addInPort("rpy", m_rpyIn);
// Set OutPort buffer
// Set service provider to Ports
m_RemoveForceSensorLinkOffsetServicePort.registerProvider("service0", "RemoveForceSensorLinkOffsetService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_RemoveForceSensorLinkOffsetServicePort);
// </rtc-template>
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
}
int nforce = m_robot->numSensors(hrp::Sensor::FORCE);
m_force.resize(nforce);
m_forceOut.resize(nforce);
m_forceIn.resize(nforce);
for (size_t i = 0; i < nforce; i++) {
hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
m_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("off_"+s->name).c_str(), m_force[i]);
m_forceIn[i] = new InPort<TimedDoubleSeq>(s->name.c_str(), m_force[i]);
m_force[i].data.length(6);
registerInPort(s->name.c_str(), *m_forceIn[i]);
registerOutPort(std::string("off_"+s->name).c_str(), *m_forceOut[i]);
m_forcemoment_offset_param.insert(std::pair<std::string, ForceMomentOffsetParam>(s->name, ForceMomentOffsetParam()));
}
return RTC::RTC_OK;
}
RTC::ReturnCode_t CollisionDetector::onActivated(RTC::UniqueId ec_id)
{
std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
if ( prop["collision_viewer"] == "true" ) {
m_use_viewer = true;
}
m_glbody = new GLbody();
m_robot = hrp::BodyPtr(m_glbody);
//
OpenHRP::BodyInfo_var binfo;
binfo = hrp::loadBodyInfo(prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext()));
if (CORBA::is_nil(binfo)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
return RTC::RTC_ERROR;
}
if (!loadBodyFromBodyInfo(m_robot, binfo, true, GLlinkFactory)) {
std::cerr << "failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}
loadShapeFromBodyInfo(m_glbody, binfo);
convertToConvexHull(m_robot);
//convertToAABB(m_robot);
setupVClipModel(m_robot);
if ( prop["collision_pair"] != "" ) {
std::cerr << "prop[collision_pair] ->" << prop["collision_pair"] << std::endl;
std::istringstream iss(prop["collision_pair"]);
std::string tmp;
while (getline(iss, tmp, ' ')) {
size_t pos = tmp.find_first_of(':');
std::string name1 = tmp.substr(0, pos), name2 = tmp.substr(pos+1);
std::cerr << "check collisions between " << m_robot->link(name1)->name << " and " << m_robot->link(name2)->name << std::endl;
m_pair[tmp] = new VclipLinkPair(m_robot->link(name1), m_VclipLinks[m_robot->link(name1)->index],
m_robot->link(name2), m_VclipLinks[m_robot->link(name2)->index], 0);
}
}
if ( m_pair.size() == 0 ) {
std::cerr << "failed to setup collisions" << std::endl;
return RTC::RTC_ERROR;
}
if ( m_use_viewer ) {
m_scene.addBody(m_robot);
}
// allocate memory for outPorts
m_q.data.length(m_robot->numJoints());
m_recover_time = 0;
m_safe_posture = true;
i_dt = 1.0;
default_recover_time = 2.5/m_dt;
m_recover_jointdata = (double *)malloc(sizeof(double)*m_robot->numJoints());
m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
return RTC::RTC_OK;
}
RTC::ReturnCode_t SequencePlayer::onInitialize()
{
std::cout << "SequencePlayer::onInitialize()" << std::endl;
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qInit", m_qInitIn);
addInPort("basePosInit", m_basePosInitIn);
addInPort("baseRpyInit", m_baseRpyInitIn);
addInPort("zmpRefInit", m_zmpRefInitIn);
// Set OutPort buffer
addOutPort("qRef", m_qRefOut);
addOutPort("tqRef", m_tqRefOut);
addOutPort("zmpRef", m_zmpRefOut);
addOutPort("accRef", m_accRefOut);
addOutPort("basePos", m_basePosOut);
addOutPort("baseRpy", m_baseRpyOut);
addOutPort("optionalData", m_optionalDataOut);
// Set service provider to Ports
m_SequencePlayerServicePort.registerProvider("service0", "SequencePlayerService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_SequencePlayerServicePort);
// </rtc-template>
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
RTC::Properties& prop = getProperties();
coil::stringTo(dt, prop["dt"].c_str());
m_robot = hrp::BodyPtr(new Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
}
unsigned int dof = m_robot->numJoints();
// Setting for wrench data ports (real + virtual)
std::vector<std::string> fsensor_names;
// find names for real force sensors
unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
for (unsigned int i=0; i<npforce; i++){
fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
}
// find names for virtual force sensors
coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
unsigned int nvforce = virtual_force_sensor.size()/10;
for (unsigned int i=0; i<nvforce; i++){
fsensor_names.push_back(virtual_force_sensor[i*10+0]);
}
// add ports for all force sensors
unsigned int nforce = npforce + nvforce;
m_wrenches.resize(nforce);
m_wrenchesOut.resize(nforce);
for (unsigned int i=0; i<nforce; i++){
m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Ref").c_str(), m_wrenches[i]);
m_wrenches[i].data.length(6);
registerOutPort(std::string(fsensor_names[i]+"Ref").c_str(), *m_wrenchesOut[i]);
}
if (prop.hasKey("seq_optional_data_dim")) {
coil::stringTo(optional_data_dim, prop["seq_optional_data_dim"].c_str());
} else {
optional_data_dim = 1;
}
m_seq = new seqplay(dof, dt, nforce, optional_data_dim);
m_qInit.data.length(dof);
for (unsigned int i=0; i<dof; i++) m_qInit.data[i] = 0.0;
Link *root = m_robot->rootLink();
m_basePosInit.data.x = root->p[0]; m_basePosInit.data.y = root->p[1]; m_basePosInit.data.z = root->p[2];
hrp::Vector3 rpy = hrp::rpyFromRot(root->R);
m_baseRpyInit.data.r = rpy[0]; m_baseRpyInit.data.p = rpy[1]; m_baseRpyInit.data.y = rpy[2];
m_zmpRefInit.data.x = 0; m_zmpRefInit.data.y = 0; m_zmpRefInit.data.z = 0;
// allocate memory for outPorts
m_qRef.data.length(dof);
m_tqRef.data.length(dof);
m_optionalData.data.length(optional_data_dim);
return RTC::RTC_OK;
}
RTC::ReturnCode_t ImpedanceController::onInitialize()
{
std::cout << "ImpedanceController::onInitialize()" << std::endl;
bindParameter("debugLevel", m_debugLevel, "0");
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qCurrent", m_qCurrentIn);
addInPort("qRef", m_qRefIn);
addInPort("rpy", m_rpyIn);
addInPort("rpyRef", m_rpyRefIn);
// Set OutPort buffer
addOutPort("q", m_qOut);
// Set service provider to Ports
m_ImpedanceControllerServicePort.registerProvider("service0", "ImpedanceControllerService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_ImpedanceControllerServicePort);
// </rtc-template>
// <rtc-template block="bind_config">
// Bind variables and configuration variable
// </rtc-template>
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
}
coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
int nvforce = virtual_force_sensor.size()/10;
int nforce = npforce + nvforce;
m_force.resize(nforce);
m_forceIn.resize(nforce);
m_ref_force.resize(nforce);
m_ref_forceOut.resize(nforce);
for (unsigned int i=0; i<npforce; i++){
hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
m_forceIn[i] = new InPort<TimedDoubleSeq>(s->name.c_str(), m_force[i]);
m_force[i].data.length(6);
registerInPort(s->name.c_str(), *m_forceIn[i]);
m_ref_force[i].data.length(6);
for (unsigned int j=0; j<6; j++) m_ref_force[i].data[j] = 0.0;
m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("ref_"+s->name).c_str(), m_ref_force[i]);
registerOutPort(std::string("ref_"+s->name).c_str(), *m_ref_forceOut[i]);
std::cerr << s->name << std::endl;
}
for (unsigned int i=0; i<nvforce; i++){
std::string name = virtual_force_sensor[i*10+0];
VirtualForceSensorParam p;
hrp::dvector tr(7);
for (int j = 0; j < 7; j++ ) {
coil::stringTo(tr[j], virtual_force_sensor[i*10+3+j].c_str());
}
p.p = hrp::Vector3(tr[0], tr[1], tr[2]);
p.R = Eigen::AngleAxis<double>(tr[6], hrp::Vector3(tr[3],tr[4],tr[5])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
p.parent_link_name = virtual_force_sensor[i*10+2];
m_sensors[name] = p;
std::cerr << "virtual force sensor : " << name << std::endl;
std::cerr << " T, R : " << p.p[0] << " " << p.p[1] << " " << p.p[2] << std::endl << p.R << std::endl;
std::cerr << " parent : " << p.parent_link_name << std::endl;
m_forceIn[i+npforce] = new InPort<TimedDoubleSeq>(name.c_str(), m_force[i+npforce]);
m_force[i+npforce].data.length(6);
registerInPort(name.c_str(), *m_forceIn[i+npforce]);
m_ref_force[i+npforce].data.length(6);
for (unsigned int j=0; j<6; j++) m_ref_force[i].data[j] = 0.0;
m_ref_forceOut[i+npforce] = new OutPort<TimedDoubleSeq>(std::string("ref_"+name).c_str(), m_ref_force[i+npforce]);
registerOutPort(std::string("ref_"+name).c_str(), *m_ref_forceOut[i+npforce]);
std::cerr << name << std::endl;
}
for (unsigned int i=0; i<m_forceIn.size(); i++){
abs_forces.insert(std::pair<std::string, hrp::Vector3>(m_forceIn[i]->name(), hrp::Vector3::Zero()));
abs_moments.insert(std::pair<std::string, hrp::Vector3>(m_forceIn[i]->name(), hrp::Vector3::Zero()));
}
unsigned int dof = m_robot->numJoints();
for ( int i = 0 ; i < dof; i++ ){
if ( i != m_robot->joint(i)->jointId ) {
std::cerr << "jointId is not equal to the index number" << std::endl;
return RTC::RTC_ERROR;
}
}
// allocate memory for outPorts
m_q.data.length(dof);
qrefv.resize(dof);
loop = 0;
return RTC::RTC_OK;
}
RTC::ReturnCode_t TorqueFilter::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qCurrent", m_qCurrentIn);
addInPort("tauIn", m_tauInIn);
// Set OutPort buffer
addOutPort("tauOut", m_tauOutOut);
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "] in "
<< m_profile.instance_name << std::endl;
return RTC::RTC_ERROR;
}
// init outport
m_tauOut.data.length(m_robot->numJoints());
// set gravity compensation flag
coil::stringTo(m_is_gravity_compensation, prop["gravity_compensation"].c_str());
if (m_debugLevel > 0) {
std::cerr << m_profile.instance_name << " : gravity compensation flag: " << m_is_gravity_compensation << std::endl;
}
// set torque offset
// offset = -(gear torque in neutral pose)
m_torque_offset.resize(m_robot->numJoints());
coil::vstring torque_offset = coil::split(prop["torque_offset"], ",");
if ( m_torque_offset.size() == torque_offset.size() && m_debugLevel > 0) {
for(int i = 0; i < m_robot->numJoints(); i++){
coil::stringTo(m_torque_offset[i], torque_offset[i].c_str());
std::cerr << "[" << m_profile.instance_name << "]" << "offset[" << m_robot->joint(i)->name << "]: " << m_torque_offset[i] << std::endl;
}
} else {
if (m_debugLevel > 0) {
std::cerr << "[" << m_profile.instance_name << "]" << "Size of torque_offset is not correct." << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "joints: " << m_robot->numJoints() << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "offsets: " << torque_offset.size() << std::endl;
}
}
// make filter
// filter_dim, fb_coeffs[0], ..., fb_coeffs[filter_dim], ff_coeffs[0], ..., ff_coeffs[filter_dim]
coil::vstring torque_filter_params = coil::split(prop["torque_filter_params"], ","); // filter values
int filter_dim = 0;
std::vector<double> fb_coeffs, ff_coeffs;
bool use_default_flag = false;
// check size of toruqe_filter_params
if ( torque_filter_params.size() > 0 ) {
coil::stringTo(filter_dim, torque_filter_params[0].c_str());
if (m_debugLevel > 0) {
std::cerr << "[" << m_profile.instance_name << "]" << "filter dim: " << filter_dim << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "torque filter param size: " << torque_filter_params.size() << std::endl;
}
} else {
use_default_flag = true;
if (m_debugLevel > 0) {
std::cerr<< "[" << m_profile.instance_name << "]" << "There is no torque_filter_params. Use default values." << std::endl;
}
}
if (!use_default_flag && ((filter_dim + 1) * 2 + 1 != torque_filter_params.size()) ) {
if (m_debugLevel > 0) {
std::cerr<< "[" << m_profile.instance_name << "]" << "Size of torque_filter_params is not correct. Use default values." << std::endl;
}
use_default_flag = true;
}
// define parameters
if (use_default_flag) {
// ex) 2dim butterworth filter sampling = 200[hz] cutoff = 5[hz]
// octave$ [a, b] = butter(2, 5/200)
// fb_coeffs[0] = 1.00000; <- b0
// fb_coeffs[1] = 1.88903; <- -b1
// fb_coeffs[2] = -0.89487; <- -b2
// ff_coeffs[0] = 0.0014603; <- a0
// ff_coeffs[1] = 0.0029206; <- a1
// ff_coeffs[2] = 0.0014603; <- a2
filter_dim = 2;
fb_coeffs.resize(filter_dim+1);
fb_coeffs[0] = 1.00000;
fb_coeffs[1] = 1.88903;
fb_coeffs[2] =-0.89487;
ff_coeffs.resize(filter_dim+1);
ff_coeffs[0] = 0.0014603;
ff_coeffs[1] = 0.0029206;
ff_coeffs[2] = 0.0014603;
} else {
fb_coeffs.resize(filter_dim + 1);
ff_coeffs.resize(filter_dim + 1);
for (int i = 0; i < filter_dim + 1; i++) {
coil::stringTo(fb_coeffs[i], torque_filter_params[i + 1].c_str());
coil::stringTo(ff_coeffs[i], torque_filter_params[i + (filter_dim + 2)].c_str());
}
}
if (m_debugLevel > 0) {
for (int i = 0; i < filter_dim + 1; i++) {
std::cerr << "[" << m_profile.instance_name << "]" << "fb[" << i << "]: " << fb_coeffs[i] << std::endl;
std::cerr << "[" << m_profile.instance_name << "]" << "ff[" << i << "]: " << ff_coeffs[i] << std::endl;
}
}
// make filter instance
for(int i = 0; i < m_robot->numJoints(); i++){
m_filters.push_back(IIRFilter(filter_dim, fb_coeffs, ff_coeffs, std::string(m_profile.instance_name)));
}
return RTC::RTC_OK;
}
RTC::ReturnCode_t SequencePlayer::onInitialize()
{
std::cout << "SequencePlayer::onInitialize()" << std::endl;
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qInit", m_qInitIn);
addInPort("basePosInit", m_basePosInitIn);
addInPort("baseRpyInit", m_baseRpyInitIn);
// Set OutPort buffer
addOutPort("qRef", m_qRefOut);
addOutPort("zmpRef", m_zmpRefOut);
addOutPort("accRef", m_accRefOut);
addOutPort("basePos", m_basePosOut);
addOutPort("baseRpy", m_baseRpyOut);
// Set service provider to Ports
m_SequencePlayerServicePort.registerProvider("service0", "SequencePlayerService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_SequencePlayerServicePort);
// </rtc-template>
// <rtc-template block="bind_config">
// Bind variables and configuration variable
// </rtc-template>
RTC::Properties& prop = getProperties();
double dt;
coil::stringTo(dt, prop["dt"].c_str());
m_robot = new Body();
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
}
unsigned int dof = m_robot->numJoints();
m_seq = new seqplay(dof, dt);
m_qInit.data.length(dof);
for (unsigned int i=0; i<dof; i++) m_qInit.data[i] = 0.0;
m_basePosInit.data.x = m_basePosInit.data.y = m_basePosInit.data.z = 0.0;
m_baseRpyInit.data.r = m_baseRpyInit.data.p = m_baseRpyInit.data.y = 0.0;
// allocate memory for outPorts
m_qRef.data.length(dof);
return RTC::RTC_OK;
}
RTC::ReturnCode_t TorqueController::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("tauCurrent", m_tauCurrentInIn);
addInPort("tauMax", m_tauMaxInIn);
addInPort("qCurrent", m_qCurrentInIn);
addInPort("qRef", m_qRefInIn); // for naming rule of hrpsys_config.py
// Set OutPort buffer
addOutPort("q", m_qRefOutOut); // for naming rule of hrpsys_config.py
// Set service provider to Ports
m_TorqueControllerServicePort.registerProvider("service0", "TorqueControllerService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_TorqueControllerServicePort);
// </rtc-template>
// read property settings
RTC::Properties& prop = getProperties();
// get dt
coil::stringTo(m_dt, prop["dt"].c_str());
// make rtc manager settings
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
// set robot model
m_robot = hrp::BodyPtr(new hrp::Body());
std::cerr << prop["model"].c_str() << std::endl;
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
}
// make torque controller settings
coil::vstring motorTorqueControllerParamsFromConf = coil::split(prop["torque_controller_params"], ",");
hrp::dvector tdcParamKe(m_robot->numJoints()), tdcParamKd(m_robot->numJoints()), tdcParamKi(m_robot->numJoints()), tdcParamT(m_robot->numJoints());
if (motorTorqueControllerParamsFromConf.size() == 2 * m_robot->numJoints()) {
std::cerr << "use TwoDofController" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofController
coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[2 * i].c_str());
coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[2 * i + 1].c_str());
m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamT[i], m_dt));
}
if (m_debugLevel > 0) {
std::cerr << "torque controller parames:" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) {
std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
}
}
} else if (motorTorqueControllerParamsFromConf.size() == 3 * m_robot->numJoints()) { // use TwoDofControllerPDModel
std::cerr << "use TwoDofControllerPDModel" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofControllerPDModel
coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[3 * i].c_str());
coil::stringTo(tdcParamKd[i], motorTorqueControllerParamsFromConf[3 * i + 1].c_str());
coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[3 * i + 2].c_str());
m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamKd[i], tdcParamT[i], m_dt));
}
if (m_debugLevel > 0) {
std::cerr << "torque controller parames:" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) {
std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamKd[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
}
}
} else if (motorTorqueControllerParamsFromConf.size() == 4 * m_robot->numJoints()) { // use TwoDofControllerDynamicsModel
std::cerr << "use TwoDofControllerDynamicsModel" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) { // use TwoDofControllerDynamicsModel
coil::stringTo(tdcParamKe[i], motorTorqueControllerParamsFromConf[4 * i].c_str());
coil::stringTo(tdcParamKd[i], motorTorqueControllerParamsFromConf[4 * i + 1].c_str());
coil::stringTo(tdcParamKi[i], motorTorqueControllerParamsFromConf[4 * i + 2].c_str());
coil::stringTo(tdcParamT[i], motorTorqueControllerParamsFromConf[4 * i + 3].c_str());
m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamKd[i], tdcParamKi[i], tdcParamT[i], m_dt));
}
if (m_debugLevel > 0) {
std::cerr << "torque controller parames:" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) {
std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamKd[i] << " " << tdcParamKi[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
}
}
} else { // default
std::cerr << "[WARNING] torque_controller_params is not correct number, " << motorTorqueControllerParamsFromConf.size() << ". Use default controller." << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) {
tdcParamKe[i] = 400.0;
tdcParamT[i] = 0.04;
m_motorTorqueControllers.push_back(MotorTorqueController(m_robot->joint(i)->name, tdcParamKe[i], tdcParamT[i], m_dt));
}
if (m_debugLevel > 0) {
std::cerr << "torque controller parames:" << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) {
std::cerr << m_robot->joint(i)->name << ":" << tdcParamKe[i] << " " << tdcParamT[i] << " " << m_dt << std::endl;
}
}
}
// allocate memory for outPorts
m_qRefOut.data.length(m_robot->numJoints());
return RTC::RTC_OK;
}
RTC::ReturnCode_t ThermoEstimator::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("tauIn", m_tauInIn);
addInPort("servoStateIn", m_servoStateInIn);
// Set OutPort buffer
addOutPort("tempOut", m_tempOutOut);
addOutPort("servoStateOut", m_servoStateOutOut);
// Set service provider to Ports
// Set service consumers to Ports
// Set CORBA Service Ports
// </rtc-template>
// set parmeters
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "]"
<< std::endl;
}
// init outport
m_tempOut.data.length(m_robot->numJoints());
m_servoStateIn.data.length(m_robot->numJoints());
m_servoStateOut.data.length(m_robot->numJoints());
// set temperature of environment
if (prop["ambient_tmp"] != "") {
coil::stringTo(m_ambientTemp, prop["ambient_tmp"].c_str());
} else {
m_ambientTemp = 25.0;
}
std::cerr << m_profile.instance_name << ": ambient temperature: " << m_ambientTemp << std::endl;
// set motor heat parameters
m_motorHeatParams.resize(m_robot->numJoints());
coil::vstring motorHeatParamsFromConf = coil::split(prop["motor_heat_params"], ",");
if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) {
std::cerr << "[WARN]: size of motorHeatParams is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl;
} else {
std::cerr << "motorHeatParams is " << std::endl;
for (int i = 0; i < m_robot->numJoints(); i++) {
m_motorHeatParams[i].temperature = m_ambientTemp;
std::cerr << motorHeatParamsFromConf[2 * i].c_str() << " " << motorHeatParamsFromConf[2 * i + 1].c_str() << std::endl;
coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str());
coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str());
}
}
return RTC::RTC_OK;
}
RTC::ReturnCode_t GraspController::onInitialize()
{
std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
// <rtc-template block="bind_config">
// Bind variables and configuration variable
bindParameter("debugLevel", m_debugLevel, "0");
// </rtc-template>
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("qRef", m_qRefIn); // for naming rule of hrpsys_config.py
addInPort("qCurrent", m_qCurrentIn);
addInPort("qIn", m_qIn);
// Set OutPort buffer
addOutPort("q", m_qOut); // for naming rule of hrpsys_config.py
// Set service provider to Ports
m_GraspControllerServicePort.registerProvider("service0", "GraspControllerService", m_service0);
// Set service consumers to Ports
// Set CORBA Service Ports
addPort(m_GraspControllerServicePort);
// </rtc-template>
m_robot = hrp::BodyPtr(new hrp::Body());
RTC::Properties& prop = getProperties();
coil::stringTo(m_dt, prop["dt"].c_str());
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
<< std::endl;
return RTC::RTC_ERROR;
}
// <name> : <joint1>, <direction1>, <joint2>, <direction2>, <joint1>, <direction2>, <name> : <joint1>, <direction1>
// check num of grasp
coil::vstring grasp_joint_params = coil::split(prop["grasp_joint_groups"], ",");
std::string grasp_name;
GraspJoint grasp_joint;
std::vector<GraspJoint> grasp_joints;
for(int i = 0, f = 0; i < grasp_joint_params.size(); i++ ){
coil::vstring grasp_joint_group_names = coil::split(grasp_joint_params[i], ":");
if ( grasp_joint_group_names.size() > 1 ) {
if ( grasp_name != "" ) {
GraspParam grasp_param;
grasp_param.time = 0;
grasp_param.joints = grasp_joints;
grasp_param.time = 1; // stop
m_grasp_param[grasp_name] = grasp_param;
grasp_joints.clear();
}
// initialize
grasp_name = grasp_joint_group_names[0];
if ( !! m_robot->link(grasp_joint_group_names[1]) ) {
grasp_joint.id = m_robot->link(std::string(grasp_joint_group_names[1].c_str()))->jointId;
}
f = 0;
i++;
}
if ( f == 0 ) {
coil::stringTo(grasp_joint.dir,grasp_joint_params[i].c_str());
grasp_joints.push_back(grasp_joint);
f = 1 ;
} else {
if ( !! m_robot->link(grasp_joint_params[i]) ) {
grasp_joint.id = m_robot->link(grasp_joint_params[i])->jointId;
}
f = 0 ;
}
}
// finalize
if ( grasp_name != "" ) {
GraspParam grasp_param;
grasp_param.time = 0;
grasp_param.joints = grasp_joints;
grasp_param.time = 1; // stop
m_grasp_param[grasp_name] = grasp_param;
}
//
if ( m_debugLevel ) {
std::map<std::string, GraspParam >::iterator it = m_grasp_param.begin();
while ( it != m_grasp_param.end() ) {
std::cerr << "[" << m_profile.instance_name << "] " << it->first << " : ";
for ( int i = 0 ; i < it->second.joints.size(); i++ ) {
std::cerr << "id = " << it->second.joints[i].id << ", dir = " << it->second.joints[i].dir << ", ";
}
std::cerr << std::endl;
it++;
}
}
return RTC::RTC_OK;
}
void problem_init(int argc, char* argv[]){
/* Setup constants */
boxsize = 3; // in AU
integrator = WHFAST;
integrator_whfast_corrector = 0;
integrator_whfast_synchronize_manually = 0;
tmax = atof(argv[2]); // in year/(2*pi)
Keplername = argv[1]; //Kepler system being investigated, Must be first string after ./nbody!
p_suppress = 0; //If = 1, suppress all print statements
double RT = 0.06; //Resonance Threshold - if abs(P2/2*P1 - 1) < RT, then close enough to resonance
double timefac = 25.0; //Number of kicks per orbital period (of closest planet)
/* Migration constants */
mig_forces = 1; //If ==0, no migration.
K = 100; //tau_a/tau_e ratio. I.e. Lee & Peale (2002)
e_ini = atof(argv[3]); //atof(argv[3]); //initial eccentricity of the planets
afac = atof(argv[4]); //Factor to increase 'a' of OUTER planets by.
double iptmig_fac = atof(argv[5]); //reduction factor of inner planet's t_mig (lower value = more eccentricity)
double Cin = atof(argv[6]); //migration speed of outer planet inwards. Nominal is 6
/* Tide constants */
tides_on = 1; //If ==0, then no tidal torques on planets.
tide_force = 0; //if ==1, implement tides as *forces*, not as e' and a'.
double k2fac = atof(argv[7]); //multiply k2 by this factor
inner_only = 0; //if =1, allow only the inner planet to evolve under tidal influence
//k2fac_check(Keplername,&k2fac); //For special systems, make sure that if k2fac is set too high, it's reduced.
#ifdef OPENGL
display_wire = 1;
#endif // OPENGL
init_box();
printf("%f k2fac \n \n \n",k2fac);
//Naming
naming(Keplername, txt_file, K, iptmig_fac, e_ini, k2fac, tide_force);
// Initial vars
if(p_suppress == 0) printf("You have chosen: %s \n",Keplername);
double Ms,Rs,a,mp,rp,k2,Q,max_t_mig=0, P_temp;
int char_val;
//Star & Planet 1
readplanets(Keplername,txt_file,&char_val,&_N,&Ms,&Rs,&mp,&rp,&P_temp,p_suppress);
if(mig_forces == 0 && p_suppress == 0) printf("--> Migration is *off* \n");
if(tides_on == 0 && p_suppress == 0) printf("--> Tides are *off* \n");
struct particle star; //Star MUST be the first particle added.
star.x = 0; star.y = 0; star.z = 0;
star.vx = 0; star.vy = 0; star.vz = 0;
star.ax = 0; star.ay = 0; star.az = 0;
star.m = Ms;
star.r = Rs;
particles_add(star);
//timestep - units of 2pi*yr (required)
dt = 2.*M_PI*P_temp/(365.*timefac);
if(p_suppress == 0) printf("The timestep used for this simulation is (2pi*years): %f \n",dt);
//Arrays, Extra slot for star, calloc sets values to 0 already.
tau_a = calloc(sizeof(double),_N+1); //migration speed of semi-major axis
tau_e = calloc(sizeof(double),_N+1); //migration (damp) speed of eccentricity
lambda = calloc(sizeof(double),_N+1); //resonant angle for each planet
omega = calloc(sizeof(double),_N+1); //argument of periapsis for each planet
expmigfac = calloc(sizeof(double),_N+1);
t_mig = calloc(sizeof(double),_N+1);
t_damp = calloc(sizeof(double),_N+1);
phi_i = calloc(sizeof(int),_N+1); //phi index (for outputting resonance angles)
if(tide_force == 1){
//tidetau_a = calloc(sizeof(double),_N+1);
tidetauinv_e = calloc(sizeof(double),_N+1);
}
double P[_N+1]; //array of period values, only needed locally
P[1] = P_temp;
if(inner_only == 1) planets_with_tides = 1; else planets_with_tides = _N+1;
//planet 1
calcsemi(&a,Ms,P[1]); //I don't trust archive values. Make it all consistent
migration(Keplername,tau_a, t_mig, t_damp, &expmigfac[1], 0, &max_t_mig, P, 1, RT, Ms, mp, iptmig_fac, a, afac, p_suppress, Cin);
struct particle p = tools_init_orbit2d(Ms, mp, a*afac, e_ini, 0, 0.);
tau_e[1] = tau_a[1]/K;
assignk2Q(&k2, &Q, k2fac, rp);
p.Q=Q;
p.k2=k2;
p.r = rp;
particles_add(p);
//print/writing stuff
printf("System Properties: # planets=%d, Rs=%f, Ms=%f \n",_N, Rs, Ms);
printwrite(1,txt_file,a,P[1],e_ini,mp,rp,k2/Q,tau_a[1],t_mig[1],t_damp[1],afac,p_suppress);
//outer planets (i=0 is star)
for(int i=2;i<_N+1;i++){
extractplanets(&char_val,&mp,&rp,&P[i],p_suppress);
calcsemi(&a,Ms,P[i]);
migration(Keplername,tau_a, t_mig, t_damp, &expmigfac[i], &phi_i[i], &max_t_mig, P, i, RT, Ms, mp, iptmig_fac, a, afac, p_suppress, Cin);
struct particle p = tools_init_orbit2d(Ms, mp, a*afac, e_ini, 0, i*M_PI/4.);
tau_e[i] = tau_a[i]/K;
assignk2Q(&k2, &Q, k2fac, rp);
p.Q = Q;
p.k2 = k2;
p.r = rp;
particles_add(p);
printwrite(i,txt_file,a,P[i],e_ini,mp,rp,k2/Q,tau_a[i],t_mig[i],t_damp[i],afac,p_suppress);
}
//tidal delay
if(max_t_mig < 20000)tide_delay = 20000.; else tide_delay = max_t_mig + 20000.; //Have at least 30,000 years grace before turning on tides.
double tide_delay_output = 0;
if(tides_on == 1) tide_delay_output = tide_delay;
FILE *write;
write=fopen(txt_file, "a");
fprintf(write, "%f \n",tide_delay_output);
fclose(write);
tide_print = 0;
problem_additional_forces = problem_migration_forces; //Set function pointer to add dissipative forces.
integrator_force_is_velocitydependent = 1;
if (integrator != WH){ // The WH integrator assumes a heliocentric coordinate system.
tools_move_to_center_of_momentum();
}
}