void SimScheduler::initRobotsJointMode()
{
dynamicsSimulator_->setCharacterAllJointModes( body_->name(),
DynamicsSimulator::HIGH_GAIN_MODE);
DblSequence wdata;
wdata.length(1);
wdata[0] = -1.0;
// The bush fill are torque controlled.
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"RLEG_BUSH_Z",
DynamicsSimulator::POSITION_GIVEN,
wdata );
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"RLEG_BUSH_ROLL",
DynamicsSimulator::POSITION_GIVEN,
wdata );
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"RLEG_BUSH_PITCH",
DynamicsSimulator::POSITION_GIVEN,
wdata );
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"LLEG_BUSH_Z",
DynamicsSimulator::POSITION_GIVEN,
wdata );
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"LLEG_BUSH_ROLL",
DynamicsSimulator::POSITION_GIVEN,
wdata );
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"LLEG_BUSH_PITCH",
DynamicsSimulator::POSITION_GIVEN,
wdata );
}
void SimScheduler::initRobotsPose()
{
DblSequence angle;
if (!strcmp(body_->name(),"sample"))
{
angle.length(29);
angle[0] = 0.0; angle[1] = -0.0360373;
angle[2] = 0.0; angle[3] = 0.0785047;
angle[4] = -0.0424675; angle[5] = 0.0;
angle[6] = 0.174533; angle[7] = -0.00349066;
angle[8] = 0.0; angle[9] = -1.5708;
angle[10] = 0.0; angle[11] = 0.0;
angle[12] = 0.0; angle[13] = 0.0;
angle[14] = -0.0360373; angle[15] = 0.0;
angle[16] = 0.0785047; angle[17] = -0.0424675; angle[18] = 0.0;
angle[19] = 0.174533; angle[20] = -0.00349066; angle[21] = 0.0; angle[22] = -1.5708;
angle[23] = 0.0; angle[24] = 0.0; angle[25] = 0.0;
angle[26] = 0.0; angle[27] = 0.0; angle[28] = 0.0;
}
else if (!strcmp(body_->name(),"HRP2JRL"))
{
angle.length(40);
angle[0] = 0.0 ; angle[1] = 0.0 ; angle[2] = -0.45379;
angle[3] = 0.87266; angle[4] =-0.41888 ; angle[5] = 0.0;
angle[6] = 0.0 ; angle[7] = 0.0 ; angle[8] = -0.45379;
angle[9] = 0.87266; angle[10] =-0.41888 ; angle[11] = 0.0;
angle[12] = 0.0 ; angle[13] = 0.0;
angle[14] = 0.0 ; angle[15] = 0.0;
angle[16] = 0.2618; angle[17] =-0.17453 ; angle[18] = 0.0;
angle[19] =-0.5236; angle[20] = 0.0 ; angle[21] = 0.0;
angle[22] = 0.17453;
angle[23] = 0.2618; angle[24] = 0.17453 ; angle[25] = 0.0;
angle[26] =-0.5236; angle[27] = 0.0 ; angle[28] = 0.0;
angle[29] = 0.17453;
angle[30] =-0.17453; angle[31] = 0.17453; angle[32] = -0.17453; angle[33] = 0.17453; angle[34] = -0.17453;
angle[35] =-0.17453; angle[36] = 0.17453; angle[37] = -0.17453; angle[38] = 0.17453; angle[39] = -0.17453;
}
dynamicsSimulator_->setCharacterAllLinkData( body_->name(),
DynamicsSimulator::JOINT_VALUE,
angle );
}
DblSequence* CollisionDetector_impl::scanDistanceWithRay(const DblArray3 p, const DblArray9 R, CORBA::Double step, CORBA::Double range)
{
DblSequence *distances = new DblSequence();
int scan_half = (int)(range/2/step);
distances->length(scan_half*2+1);
double local[3], a;
DblArray3 dir;
local[1] = 0;
for (int i = -scan_half; i<= scan_half; i++){
a = i*step;
local[0] = -sin(a); local[2] = -cos(a);
dir[0] = R[0]*local[0]+R[1]*local[1]+R[2]*local[2];
dir[1] = R[3]*local[0]+R[4]*local[1]+R[5]*local[2];
dir[2] = R[6]*local[0]+R[7]*local[1]+R[8]*local[2];
(*distances)[i+scan_half] = queryDistanceWithRay(p, dir);
#if 0
printf("angle = %8.3f, distance = %8.3f\n", a, (*distances)[i+scan_half]);
fflush(stdout);
#endif
}
return distances;
}
void SimScheduler::initRobotsFreeFlyerPosition()
{
// initial position and orientation
Vector3 waist_p;
Matrix33 waist_R;
if (!strcmp(body_->name(),"HRP2"))
waist_p << 0, 0, 0.705;
else
waist_p << 0, 0, 0.6487;
waist_R = Matrix33::Identity();
DblSequence trans;
trans.length(12);
for(int i=0; i<3; i++) trans[i] = waist_p(i);
for(int i=0; i<3; i++){
for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
}
dynamicsSimulator_->setCharacterLinkData( body_->name(),
"WAIST",
DynamicsSimulator::ABS_TRANSFORM,
trans );
}
inline double getLimitValue(DblSequence limitseq, double defaultValue)
{
return (limitseq.length() == 0) ? defaultValue : limitseq[0];
}
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;
}
