void SimScheduler::initParallelMecanisms()
{
DblSequence3 aLink1LocalPos,aLink2LocalPos;
aLink1LocalPos.length(3); aLink2LocalPos.length(3);
aLink1LocalPos[0] = 0.0; aLink1LocalPos[1] = 0.004; aLink1LocalPos[2] = -0.11;
aLink2LocalPos[0] = 0.0; aLink2LocalPos[1] = 0.0; aLink2LocalPos[2] = 0.06 ;
DblSequence3 aJointAxis;
aJointAxis.length(3);
aJointAxis[0] = 1.0; aJointAxis[1] = 0.0; aJointAxis[2] = 0.0;
// Close the kinematic chain of the Right hand - gripper
dynamicsSimulator_->registerExtraJoint(body_->name(), "RARM_JOINT5",
body_->name(), "RHAND_JOINT1",
aLink1LocalPos, aLink2LocalPos,
ExtraJointType::EJ_XYZ,
aJointAxis,
"RL6RL1");
aLink1LocalPos[0] = 0.0; aLink1LocalPos[1] = -0.004; aLink1LocalPos[2] = -0.11;
aLink2LocalPos[0] = 0.0; aLink2LocalPos[1] = 0.0; aLink2LocalPos[2] = 0.06 ;
dynamicsSimulator_->registerExtraJoint(body_->name(), "RARM_JOINT5",
body_->name(), "RHAND_JOINT4",
aLink1LocalPos, aLink2LocalPos,
ExtraJointType::EJ_XYZ,
aJointAxis,
"RH2RL4");
// Close the kinematic chain of the Left hand - gripper
aLink1LocalPos[0] = 0.0; aLink1LocalPos[1] = -0.004; aLink1LocalPos[2] = -0.11;
aLink2LocalPos[0] = 0.0; aLink2LocalPos[1] = 0.0; aLink2LocalPos[2] = 0.06 ;
dynamicsSimulator_->registerExtraJoint(body_->name(), "LARM_JOINT5",
body_->name(), "LHAND_JOINT1",
aLink1LocalPos, aLink2LocalPos,
ExtraJointType::EJ_XYZ,
aJointAxis,
"RL6RL1");
aLink1LocalPos[0] = 0.0; aLink1LocalPos[1] = 0.004; aLink1LocalPos[2] = -0.11;
aLink2LocalPos[0] = 0.0; aLink2LocalPos[1] = 0.0; aLink2LocalPos[2] = 0.06 ;
dynamicsSimulator_->registerExtraJoint(body_->name(), "LARM_JOINT5",
body_->name(), "LHAND_JOINT4",
aLink1LocalPos, aLink2LocalPos,
ExtraJointType::EJ_XYZ,
aJointAxis,
"RH2RL4");
}
void SimScheduler::initDynamicsSimulator()
{
DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
dynamicsSimulatorFactory =
checkCorbaServer <DynamicsSimulatorFactory, DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxt_);
if (CORBA::is_nil(dynamicsSimulatorFactory)) {
std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
}
// Create dynamics simulator
dynamicsSimulator_ = dynamicsSimulatorFactory->create();
// Register robots.
cout << "** Dynamics server setup ** " << endl;
cout << "Character :" << floor_->name() << endl;
dynamicsSimulator_->registerCharacter(floor_->name(), floor_);
cout << "Character :" << body_->name() << endl;
dynamicsSimulator_->registerCharacter(body_->name(), body_);
// Enable sensor and gravity.
dynamicsSimulator_->init(timeStep_,
DynamicsSimulator::RUNGE_KUTTA,
DynamicsSimulator::ENABLE_SENSOR);
DblSequence3 g;
g.length(3);
g[0] = 0.0;
g[1] = 0.0;
double world_gravity = 9.8; // default gravity acceleration [m/s^2]
g[2] = world_gravity;
dynamicsSimulator_->setGVector(g);
initRobotsFreeFlyerPosition();
initRobotsPose();
initRobotsJointMode();
dynamicsSimulator_->calcWorldForwardKinematics();
initCollisions();
initParallelMecanisms();
dynamicsSimulator_->initSimulation();
}
int main(int argc, char* argv[])
{
int i;
string url = "file://";
double ddp = 0.001;
double timeK = 10.0;
// -urlでモデルのURLを指定
for(i=0; i < argc - 1; i++)
{
if( strcmp(argv[i], "-url") == 0)
{
url += argv[++i];
} else if( strcmp(argv[i], "-ddp") == 0)
{
ddp = strtod( argv[++i], NULL);
} else if( strcmp(argv[i], "-timeK") == 0)
{
timeK = strtod( argv[++i], NULL);
}
}
string robot_url = url+"sample.wrl";
string floor_url = url+"floor.wrl";
const char *ROBOT_URL = robot_url.c_str();
const char *FLOOR_URL = floor_url.c_str();
//////////////////////////////////////////////////////////////////////
// CORBA初期化
CORBA::ORB_var orb;
orb = CORBA::ORB_init(argc, argv);
// ROOT POA
CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
// POAマネージャへの参照を取得
PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
// NamingServiceの参照取得
CosNaming::NamingContext_var cxT;
{
CORBA::Object_var nS = orb->resolve_initial_references("NameService");
cxT = CosNaming::NamingContext::_narrow(nS);
}
/////////////////////////////////////////////////////////////////////////
// DynamicsSimulatorの取得
DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
dynamicsSimulatorFactory =
checkCorbaServer <DynamicsSimulatorFactory,
DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxT);
if (CORBA::is_nil(dynamicsSimulatorFactory))
{
std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
}
DynamicsSimulator_var dynamicsSimulator
= dynamicsSimulatorFactory->create();
// モデルの読み込み・登録
BodyInfo_var robotInfo = loadBodyInfo(ROBOT_URL, argc, argv);
dynamicsSimulator->registerCharacter("robot", robotInfo);
// 床の読み込み・登録
BodyInfo_var floorInfo = loadBodyInfo(FLOOR_URL, argc, argv);
dynamicsSimulator->registerCharacter("floor", floorInfo);
/////////////////////////////////////////////////////////////////////////
// DynamicsSimulatorの初期化
dynamicsSimulator->init(0.002,
DynamicsSimulator::RUNGE_KUTTA,
DynamicsSimulator::ENABLE_SENSOR);
// 重力ベクトルの設定
DblSequence3 gVector;
gVector.length(3);
gVector[0] = gVector[1] = 0;
gVector[2] = 9.8;
dynamicsSimulator->setGVector(gVector);
// 関節駆動モードの設定
dynamicsSimulator->setCharacterAllJointModes(
"robot", DynamicsSimulator::TORQUE_MODE);
// 初期姿勢
double init_pos[] = {0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02, 0.00E+00,
1.75E-01, -3.49E-03, 0, -1.57E+00, 0.00E+00, 0.00E+00,
0.00E+00, 0.00E+00, -3.60E-02, 0, 7.85E-02, -4.25E-02,
0.00E+00, 1.75E-01, 3.49E-03, 0, -1.57E+00, 0.00E+00,
0.00E+00, 0.00E+00, 0, 0, 0};
// 初期姿勢を関節角にセット
DblSequence q;
q.length(DOF);
for (int i=0; i<DOF; i++)
{
q[i] = init_pos[i];
}
dynamicsSimulator->setCharacterAllLinkData(
"robot", DynamicsSimulator::JOINT_VALUE, q);
// 順運動学計算
dynamicsSimulator->calcWorldForwardKinematics();
// 干渉チェックペアの設定−両手
DblSequence6 dc, sc;
dc.length(0);
sc.length(0);
dynamicsSimulator->registerCollisionCheckPair(
"robot",
"RARM_WRIST_R",
"robot",
"LARM_WRIST_R",
0.5,
0.5,
dc,
sc,
0.0,
0.0);
dynamicsSimulator->initSimulation();
/////////////////////////////////////////////////////////////////////////
// OnlineViewerの取得
OnlineViewer_var onlineViewer = getOnlineViewer(argc, argv);
// OnlineViewerに対するモデルロード
try
{
onlineViewer->load("robot", ROBOT_URL);
onlineViewer->load("floor", FLOOR_URL);
onlineViewer->setLogName("clap");
onlineViewer->clearLog();
}
catch (CORBA::SystemException& ex)
{
std::cerr << "Failed to connect GrxUI." << endl;
return 1;
}
/////////////////////////////////////////////////////////////////////////
// 逆運動学計算の準備
double RARM_p[] = {0.197403, -0.210919, 0.93732};
double RARM_R[] = {0.174891, -0.000607636, -0.984588,
0.00348999, 0.999994, 2.77917e-06,
0.984582, -0.00343669, 0.174892};
double LARM_p[] = {0.197403, 0.210919, 0.93732};
double LARM_R[] = {0.174891, 0.000607636, -0.984588,
-0.00348999, 0.999994, -2.77917e-06,
0.984582, 0.00343669, 0.174892};
double dp = 0.0;
/////////////////////////////////////////////////////////////////////////
WorldState_var state;
while (1)
{
// 逆運動学計算
LinkPosition link;
link.p[0] = RARM_p[0];
link.p[1] = RARM_p[1] + dp;
link.p[2] = RARM_p[2];
for (int i=0; i<9; i++)
link.R[i] = RARM_R[i];
dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
CORBA::string_dup("CHEST"),
CORBA::string_dup("RARM_WRIST_R"),
link);
link.p[0] = LARM_p[0];
link.p[1] = LARM_p[1] - dp;
link.p[2] = LARM_p[2];
for (int i=0; i<9; i++)
link.R[i] = LARM_R[i];
dynamicsSimulator->calcCharacterInverseKinematics(CORBA::string_dup("robot"),
CORBA::string_dup("CHEST"),
CORBA::string_dup("LARM_WRIST_R"),
link);
dynamicsSimulator->calcWorldForwardKinematics();
dp += ddp;
// OnlineViewer更新
try
{
dynamicsSimulator->getWorldState(state);
state->time = dp*timeK;
onlineViewer->update(state);
}
catch (CORBA::SystemException& ex)
{
std::cerr << "OnlineViewer could not be updated." << endl;
std::cerr << "ex._rep_id() = " << ex._rep_id() << endl;
return 1;
}
if(state->time >= 200.0){
std::cout << state->time << endl;
}
// 干渉チェック
dynamicsSimulator->checkCollision(true);
if (state->collisions.length() > 0)
{
if (state->collisions[0].points.length() > 0)
{
break;
}
}
}
return 0;
}
