int main(int argc, char* argv[])
{
VisitableA a;
VisitableB b;
PrivateVisitor p_a(a);
PrivateVisitor p_b(b);
}
int main()
{
int a[]={10,20,30,40,50,60,70,80};
int ele,i;
ele=sizeof(a)/sizeof(a[0]);
p_a(a,ele);
printf("\n");
return 0;
}
int main(void) {
Pin p_a(PIN_A, -70, 290, 3), p_b(PIN_B, 0, 290, 0),
p_c(PIN_C, 70, 290, 0);
// TODO: call solve with initial position
Robot robot(p_a, p_b, p_c);
robot.solveHannoiTower();
return 0;
}
TEST(TriangleTest, equality)
{
double p_a_x = 0, p_a_y = 1, p_a_z = 2;
double p_b_x = 3, p_b_y = 4, p_b_z = 5;
double p_c_x = 6, p_c_y = 7, p_c_z = 8;
Geom::Point3D p_a(p_a_x, p_a_y, p_a_z);
Geom::Point3D p_b(p_b_x, p_b_y, p_b_z);
Geom::Point3D p_c(p_c_x, p_c_y, p_c_z);
Geom::Triangle triangle1(p_a, p_b, p_c);
Geom::Triangle triangle2(p_a, p_b, p_c);
EXPECT_EQ(triangle1, triangle2);
Geom::Triangle triangle3(p_a, p_b, p_b);
EXPECT_NE(triangle1, triangle3);
}
double LipmConstHeightPlanner::forwardPass(const double *x0, Traj<1,1> &com) const
{
Eigen::Matrix<double,2,1> z;
Eigen::Matrix<double,1,1> u;
com = Traj<1,1>();
for (size_t i=0; i<_zmp_d.size(); i++) {
com.append(_zmp_d[i].time, _zmp_d[i].type, NULL, NULL, NULL, NULL);
}
com[0].x[0] = x0[0];
com[0].x[1] = x0[1];
#if 0
std::cout << _com.size() << " " << _zmp_d.size() << std::endl;
#endif
for (size_t i=0; i<_zmp_d.size()-1; i++) {
z(0) = com[i].x[0] - _zmp_d[i].x[0];
z(1) = com[i].x[1];
u = _K*z + _du[i];
com[i].u[0] = u(0);
z = _A*z + _B*u;
com[i+1].x[0] = z(0) + _zmp_d[i].x[0];
com[i+1].x[1] = z(1);
}
#if 0
std::fstream p_d("tmp/pd", std::fstream::out);
std::fstream p_a("tmp/pa", std::fstream::out);
std::fstream com_out("tmp/com", std::fstream::out);
for (size_t i=0; i<_zmp_d.size(); i++) {
p_d << _zmp_d[i].x[0] << std::endl;
p_a << com[i].u[0] + _zmp_d[i].x[0] << std::endl;
com_out << com[i].x[0] << std::endl;
}
p_d.close();
p_a.close();
com_out.close();
#endif
return 0;
}
TEST(TriangleTest, constructor)
{
double p_a_x = 0, p_a_y = 1, p_a_z = 2;
double p_b_x = 3, p_b_y = 4, p_b_z = 5;
double p_c_x = 6, p_c_y = 7, p_c_z = 8;
Geom::Point3D p_a(p_a_x, p_a_y, p_a_z);
Geom::Point3D p_b(p_b_x, p_b_y, p_b_z);
Geom::Point3D p_c(p_c_x, p_c_y, p_c_z);
Geom::Triangle triangle(p_a, p_b, p_c);
EXPECT_EQ(triangle.GetPointA(), p_a);
EXPECT_EQ(triangle.GetPointB(), p_b);
EXPECT_EQ(triangle.GetPointC(), p_c);
Geom::Point3D expectedExtentLower(0, 1, 2);
Geom::Point3D expectedExtentUpper(6, 7, 8);
Geom::GeometryExtent3D expectedExtent(expectedExtentLower, expectedExtentUpper);
EXPECT_EQ(expectedExtent, triangle.GetGeometryExtent());
}
