/** creates vehicle at desired position
@param pos struct Position with desired position
*/
void SchlangeVelocity::create(const osg::Matrix& pose){
Schlange::create(pose);
//*****************joint definition***********
for ( int n = 0; n < conf.segmNumber-1; n++ ) {
Pos p1(objects[n]->getPosition());
Pos p2(objects[n]->getPosition());
UniversalJoint* j = new UniversalJoint(objects[n], objects[n+1],
(objects[n]->getPosition() + objects[n+1]->getPosition())/2,
Axis(0,0,1)* pose, Axis(0,1,0)* pose);
j->init(odeHandle, osgHandle, true, conf.segmDia * 1.02);
// setting stops at universal joints
j->setParam(dParamLoStop, -conf.jointLimit*1.5);
j->setParam(dParamHiStop, conf.jointLimit*1.5);
j->setParam(dParamLoStop2, -conf.jointLimit*1.5);
j->setParam(dParamHiStop2, conf.jointLimit*1.5);
// making stops bouncy
j->setParam (dParamBounce, 0.9 );
j->setParam (dParamBounce2, 0.9 ); // universal
joints.push_back(j);
}
}
void Bar_chart::draw_lines() const{
if(m_data.size() == 0 || m_max == 0) return;
const int xoffset = 40; // ウインドウの左端からy軸までの距離
const int yoffset = 40; // ウインドウの右端からx軸までの距離
const int space = 40; // グラフの向こう側の領域
const int xlength = m_width - xoffset - space; // 軸の長さ
const int ylength = m_height - yoffset - space;
const int x_orig = xoffset; // 原点
const int y_orig = space + ylength;
const Point orig(x_orig, y_orig);
const double yscale = (double)ylength / m_max; // y軸の倍率
const int data_w = (xlength - space) / (m_data.size() * 2 - 1); // 棒の幅
// xy軸を描画
Axis(Axis::x, Point(point(0).x + space, point(0).y + y_orig), xlength).draw_lines();
Axis(Axis::y, Point(point(0).x + x_orig, point(0).y + ylength + space), ylength, 10, "one notch = " + to_string(m_max / 10)).draw_lines();
// グラフの長方形・ラベルを作成、描画
for(int i = 0; i < m_data.size(); ++i){
Rectangle r(Point(point(0).x + space + space + data_w * i * 2, point(0).y + space + ylength - m_data[i].data * yscale + 1), data_w, m_data[i].data * yscale);
r.set_fill_color(m_data[i].color);
r.draw_lines();
Text t(Point(r.point(0).x, r.point(0).y + m_data[i].data * yscale + 15), m_data[i].label);
t.set_color(m_data[i].color);
t.draw();
}
}
bool OISInputManager::mouseMoved( const OIS::MouseEvent& rEvent )
{
return InputManager::mouseMoved(
Axis( rEvent.state.X.abs, rEvent.state.X.rel ),
Axis( rEvent.state.Y.abs, rEvent.state.Y.rel ),
Axis( rEvent.state.Z.abs, rEvent.state.Z.rel ) );
}
std::vector<shared_ptr<Point>> Board::EmptyPoints()
{
std::vector<shared_ptr<Point>> va;
for (int row = boundary->GetUp(); row < boundary->GetDown(); row++)
for (int column = boundary->GetLeft(); column < boundary->GetRight(); column++)
if (board[Axis(row, column)]->PointColor == Empty)
va.push_back(board[Axis(row, column)]);
return va;
}
void GCodeReader::update_coordinates(GCodeLine &gline, std::pair<const char*, const char*> &command)
{
PROFILE_FUNC();
if (*command.first == 'G') {
int cmd_len = int(command.second - command.first);
if ((cmd_len == 2 && (command.first[1] == '0' || command.first[1] == '1')) ||
(cmd_len == 3 && command.first[1] == '9' && command.first[2] == '2')) {
for (size_t i = 0; i < NUM_AXES; ++ i)
if (gline.has(Axis(i)))
this->m_position[i] = gline.value(Axis(i));
}
}
}
void PureProjection::FromCartesianTGeodetic(const double &X,const double &Y,const double &Z, double &Lat, double &Lng)
{
double E = Flattening() * (2.0 - Flattening());
Lng = atan2(Y, X);
double P = sqrt(X * X + Y * Y);
double Theta = atan2(Z, (P * (1.0 - Flattening())));
double st = sin(Theta);
double ct = cos(Theta);
Lat = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct);
Lat /= (PI / 180);
Lng /= (PI / 180);
}
int sub_image (imageptr iptr, regionptr rptr, imageptr *optr)
{
int nx,ny,nz, nx1,ny1,nz1, ix,iy,iz, ix0,iy0,iz0;
size_t np, np1;
nx = Nx(iptr);
ny = Ny(iptr);
nz = Nz(iptr);
np = nx*ny*nz;
/* grab the bounding box */
ix0 = BLC(rptr)[0];
iy0 = BLC(rptr)[1];
iz0 = BLC(rptr)[2];
nx1 = TRC(rptr)[0] - ix0;
ny1 = TRC(rptr)[1] - iy0;
nz1 = TRC(rptr)[2] - iz0;
np1 = nx1*ny1*nz1;
*optr = (imageptr ) allocate(sizeof(image));
dprintf (DLEV,"copy_image:Allocated image @ %d size=%d * %d * %d",*optr,nx1,ny1,nz1);
Frame(*optr) = (real *) allocate(np1*sizeof(real));
dprintf (DLEV,"Frame allocated @ %d ",Frame(*optr));
Nx(*optr) = nx1;
Ny(*optr) = ny1;
Nz(*optr) = nz1;
Xmin(*optr) = Xmin(iptr) + ix0*Dx(iptr);
Ymin(*optr) = Ymin(iptr) + iy0*Dy(iptr);
Zmin(*optr) = Zmin(iptr) + iz0*Dz(iptr);
Dx(*optr) = Dx(iptr);
Dy(*optr) = Dy(iptr);
Dz(*optr) = Dz(iptr);
Namex(*optr) = mystrcpy(Namex(iptr));
Namey(*optr) = mystrcpy(Namey(iptr));
Namez(*optr) = mystrcpy(Namez(iptr));
Xref(*optr) = Xref(iptr) + ix0;
Yref(*optr) = Yref(iptr) + iy0;
Zref(*optr) = Zref(iptr) + iz0;
for (iz=0; iz<nz1; iz++)
for (iy=0; iy<ny1; iy++)
for (ix=0; ix<nx1; ix++)
CubeValue(*optr,ix,iy,iz) = CubeValue(iptr,ix-ix0,iy-iy0,iz-iy0);
Storage(*optr) = matdef[idef];
Axis(*optr) = Axis(iptr);
set_iarray(*optr);
return 1; /* succes return code */
}
Region Region::powerOfTwo() const
{
size_t vox = std::max({X.values.size(), Y.values.size(), Z.values.size()});
size_t n = 1 << (size_t)ceil(log(vox) / log(2));
// Calculate how much extra needs to be added to an axis
auto d = [=](const Axis& a){
return (a.bounds.upper() - a.bounds.lower()) *
(float(n) / a.values.size() - 1);
};
// Return the expanded axis bounds
auto a = [=](const Axis& a){
auto da = d(a);
return Axis({a.bounds.lower() - da/2,
a.bounds.upper() + da/2}, n);
};
auto r = Region(a(X), a(Y), a(Z));
assert(r.X.values.size() == n);
assert(r.Y.values.size() == n);
assert(r.Z.values.size() == n);
return r;
}
/*
* create new map from scratch, using %x and %y as position parameters
* 0..nx-1 and 0..ny-1
*/
local void do_create(int nx, int ny,int nz)
{
double m_min, m_max, total;
real fin[5], fout;
int ix, iy, iz;
int badvalues;
m_min = HUGE; m_max = -HUGE;
total = 0.0; /* count total intensity in new map */
badvalues = 0; /* count number of bad operations */
if (nz > 0) {
warning("cube");
if (!create_cube (&iptr, nx, ny, nz)) /* create default empty image */
error("Could not create 3D image from scratch");
#if 0
Axis(iptr) = 1; /* set linear axistype with a fits-style reference pixel */
#endif
wcs_f2i(3,crpix,crval,cdelt,iptr);
for (iz=0; iz<nz; iz++) {
fin[2] = iz-crpix[2]+1; /* crpix is 1 for first pixel (FITS convention) */
for (iy=0; iy<ny; iy++) {
fin[1] = iy-crpix[1]+1;
for (ix=0; ix<nx; ix++) {
fout = 0.0;
CubeValue(iptr,ix,iy,iz) = fout;
m_min = MIN(m_min,fout); /* and check for new minmax */
m_max = MAX(m_max,fout);
total += fout; /* add up totals */
}
}
}
} else {
warning("2d-map");
if (!create_image (&iptr, nx, ny))
error("Could not create 2D image from scratch");
wcs_f2i(2,crpix,crval,cdelt,iptr);
for (iy=0; iy<ny; iy++) {
fin[1] = iy;
for (ix=0; ix<nx; ix++) {
fout = 0.0;
MapValue(iptr,ix,iy) = fout;
m_min = MIN(m_min,fout); /* and check for new minmax */
m_max = MAX(m_max,fout);
total += fout; /* add up totals */
}
}
}
MapMin(iptr) = m_min;
MapMax(iptr) = m_max;
dprintf(1,"New min and max in map are: %f %f\n",m_min,m_max);
dprintf(1,"New total brightness/mass is %f\n",
total*Dx(iptr)*Dy(iptr));
if (badvalues)
warning ("There were %d bad operations in dofie",badvalues);
}
JointElement::JointElement(std::unordered_map<std::string, JointElement::JointInfo>& joints,
std::unordered_map<std::string, JointElement::JointInfo>& fixedJoints)
: iDynTree::XMLElement("joint")
, m_joints(joints)
, m_fixedJoints(fixedJoints)
, m_jointFrame(Transform::Identity())
, m_axis(Axis(Direction(1.0, 0.0, 0.0), Position(0.0, 0.0, 0.0))) { }
NS_IMETHODIMP
nsDOMMouseScrollEvent::GetAxis(int32_t* aResult)
{
NS_ENSURE_ARG_POINTER(aResult);
*aResult = Axis();
return NS_OK;
}
void
CloudEditorWidget::paintGL ()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
if (!cloud_ptr_)
return;
tool_ptr_ -> draw();
if (color_scheme_ == COLOR_BY_RGB)
cloud_ptr_->drawWithRGB();
else if (color_scheme_ == COLOR_BY_PURE)
cloud_ptr_->drawWithPureColor();
else
{
// Assumes that color_scheme_ contains COLOR_BY_[X,Y,Z] and the values
// match Axis::[X,Y,Z]
cloud_ptr_ -> setColorRampAxis(Axis(color_scheme_));
cloud_ptr_ -> drawWithTexture();
}
}
float
DOMSVGLength::GetValue(ErrorResult& aRv)
{
if (mVal) {
if (mIsAnimValItem) {
mSVGElement->FlushAnimations();
return mVal->GetAnimValue(mSVGElement);
}
return mVal->GetBaseValue(mSVGElement);
}
if (mIsAnimValItem && HasOwner()) {
Element()->FlushAnimations(); // May make HasOwner() == false
}
if (HasOwner()) {
float value = InternalItem().GetValueInUserUnits(Element(), Axis());
if (!IsFinite(value)) {
aRv.Throw(NS_ERROR_FAILURE);
}
return value;
} else if (mUnit == nsIDOMSVGLength::SVG_LENGTHTYPE_NUMBER ||
mUnit == nsIDOMSVGLength::SVG_LENGTHTYPE_PX) {
return mValue;
}
// else [SVGWG issue] Can't convert this length's value to user units
// ReportToConsole
aRv.Throw(NS_ERROR_FAILURE);
return 0.0f;
}
void R3Triad::
Draw(void) const
{
// Draw three rays
R3BeginLine();
R3LoadPoint(R3zero_point);
R3LoadPoint((R3zero_point + Axis(RN_X)));
R3EndLine();
R3BeginLine();
R3LoadPoint(R3zero_point);
R3LoadPoint((R3zero_point + Axis(RN_Y)));
R3EndLine();
R3BeginLine();
R3LoadPoint(R3zero_point);
R3LoadPoint((R3zero_point + Axis(RN_Z)));
R3EndLine();
}
void
DOMSVGLength::SetValue(float aUserUnitValue, ErrorResult& aRv)
{
if (mIsAnimValItem) {
aRv.Throw(NS_ERROR_DOM_NO_MODIFICATION_ALLOWED_ERR);
return;
}
if (mVal) {
mVal->SetBaseValue(aUserUnitValue, mSVGElement, true);
return;
}
// Although the value passed in is in user units, this method does not turn
// this length into a user unit length. Instead it converts the user unit
// value to this length's current unit and sets that, leaving this length's
// unit as it is.
if (HasOwner()) {
if (InternalItem().GetValueInUserUnits(Element(), Axis()) ==
aUserUnitValue) {
return;
}
float uuPerUnit = InternalItem().GetUserUnitsPerUnit(Element(), Axis());
if (uuPerUnit > 0) {
float newValue = aUserUnitValue / uuPerUnit;
if (IsFinite(newValue)) {
AutoChangeLengthNotifier notifier(this);
InternalItem().SetValueAndUnit(newValue, InternalItem().GetUnit());
return;
}
}
} else if (mUnit == nsIDOMSVGLength::SVG_LENGTHTYPE_NUMBER ||
mUnit == nsIDOMSVGLength::SVG_LENGTHTYPE_PX) {
mValue = aUserUnitValue;
return;
}
// else [SVGWG issue] Can't convert user unit value to this length's unit
// ReportToConsole
aRv.Throw(NS_ERROR_FAILURE);
}
QVector<double> StatGatherer :: Axis()
{
QVector<double> Axis(m_taille);
double pas=m_fin/m_taille;
double axe(0);
for (int i=0;i<m_taille;++i)
{
Axis[i]=axe;
axe+=pas;
}
return Axis;
}
vpRJoint::vpRJoint()
{
m_rQ = m_rDq = m_rDdq = m_rActuationTau = m_rSpringDamperTau = m_rImpulsiveTau = m_rQi = m_rK = m_rC = SCALAR_0;
m_rRestitution = SCALAR_1;
m_sS = se3(SCALAR_0, SCALAR_0, SCALAR_1, SCALAR_0, SCALAR_0, SCALAR_0);
m_bUsingDefaultAxis = true;
m_bHasUpperLimit = false;
m_bHasLowerLimit = false;
m_sO = SCALAR_0;
m_sT = SCALAR_0;
m_sVl = Axis(SCALAR_0);
}
void PureProjection::FromCartesianTGeodetic(const double &X, const double &Y, const double &Z, double &Lat_D, double &Lng_D)
{
double E = Flattening() * (2.0 - Flattening());
double Lng_R = atan2(Y, X);
double P = sqrt(X * X + Y * Y);
double Theta = atan2(Z, (P * (1.0 - Flattening())));
double st = sin(Theta);
double ct = cos(Theta);
double Lat_R = atan2(Z + E / (1.0 - Flattening()) * Axis() * st * st * st, P - E * Axis() * ct * ct * ct);
Lat_D = Lat_R * DEG2RAD;
Lng_D = Lng_R * DEG2RAD;
}
int create_image (imageptr *iptr, int nx, int ny)
{
*iptr = (imageptr ) allocate(sizeof(image));
dprintf (DLEV,"create_image:Allocated image @ %d size=%d * %d",*iptr,nx,ny);
Frame(*iptr) = (real *) allocate(nx*ny*sizeof(real));
dprintf (DLEV,"Frame allocated @ %d ",Frame(*iptr));
Axis(*iptr) = 0; /* old style axis with no reference pixel */
Nx(*iptr) = nx; /* old style ONE map, no cube */
Ny(*iptr) = ny;
Nz(*iptr) = 1;
Xmin(*iptr) = 0.0; /* start lower left corner at 0.0 */
Ymin(*iptr) = 0.0;
Zmin(*iptr) = 0.0;
Dx(*iptr) = 1.0; /* unity pixels */
Dy(*iptr) = 1.0;
Dz(*iptr) = 1.0;
Xref(*iptr) = 0.0;
Yref(*iptr) = 0.0;
Zref(*iptr) = 0.0;
MapMin(*iptr) = 0.0;
MapMax(*iptr) = 0.0;
BeamType(*iptr) = 0;
Beamx(*iptr) = 0.0; /* name beams */
Beamy(*iptr) = 0.0;
Beamz(*iptr) = 0.0;
Namex(*iptr) = NULL; /* no axis names */
Namey(*iptr) = NULL;
Namez(*iptr) = NULL;
Unit(*iptr) = NULL; /* no units */
Time(*iptr) = 0.0;
Storage(*iptr) = matdef[idef];
Axis(*iptr) = 0;
Mask(*iptr) = NULL;
set_iarray((*iptr));
return 1; /* succes return code */
}
int copy_image (imageptr iptr, imageptr *optr)
{
int nx,ny,nz;
size_t np;
nx = Nx(iptr);
ny = Ny(iptr);
nz = Nz(iptr);
np = nx*ny*nz;
*optr = (imageptr ) allocate(sizeof(image));
dprintf (DLEV,"copy_image:Allocated image @ %d size=%d * %d * %d",*optr,nx,ny,nz);
Frame(*optr) = (real *) allocate(np*sizeof(real));
dprintf (DLEV,"Frame allocated @ %d ",Frame(*optr));
Nx(*optr) = nx;
Ny(*optr) = ny;
Nz(*optr) = nz;
Xmin(*optr) = Xmin(iptr);
Ymin(*optr) = Ymin(iptr);
Zmin(*optr) = Zmin(iptr);
Dx(*optr) = Dx(iptr);
Dy(*optr) = Dy(iptr);
Dz(*optr) = Dz(iptr);
Namex(*optr) = mystrcpy(Namex(iptr));
Namey(*optr) = mystrcpy(Namey(iptr));
Namez(*optr) = mystrcpy(Namez(iptr));
Xref(*optr) = Xref(iptr);
Yref(*optr) = Yref(iptr);
Zref(*optr) = Zref(iptr);
Storage(*optr) = matdef[idef];
Axis(*optr) = Axis(iptr);
set_iarray(*optr);
return 1; /* succes return code */
}
int write_image (stream outstr, imageptr iptr)
{
put_history(outstr);
put_set (outstr,ImageTag);
put_set (outstr,ParametersTag);
put_data (outstr,NxTag, IntType, &(Nx(iptr)), 0);
put_data (outstr,NyTag, IntType, &(Ny(iptr)), 0);
put_data (outstr,NzTag, IntType, &(Nz(iptr)), 0);
put_data (outstr,XminTag,RealType, &(Xmin(iptr)), 0);
put_data (outstr,YminTag,RealType, &(Ymin(iptr)), 0);
put_data (outstr,ZminTag,RealType, &(Zmin(iptr)), 0);
put_data (outstr,DxTag, RealType, &(Dx(iptr)), 0);
put_data (outstr,DyTag, RealType, &(Dy(iptr)), 0);
put_data (outstr,DzTag, RealType, &(Dz(iptr)), 0);
put_data (outstr,XrefTag,RealType, &(Xref(iptr)), 0);
put_data (outstr,YrefTag,RealType, &(Yref(iptr)), 0);
put_data (outstr,ZrefTag,RealType, &(Zref(iptr)), 0);
put_data (outstr,MapMinTag, RealType, &(MapMin(iptr)), 0);
put_data (outstr,MapMaxTag, RealType, &(MapMax(iptr)), 0);
put_data (outstr,BeamTypeTag, IntType, &(BeamType(iptr)), 0);
put_data (outstr,BeamxTag, RealType, &(Beamx(iptr)), 0);
put_data (outstr,BeamyTag, RealType, &(Beamy(iptr)), 0);
put_data (outstr,BeamzTag, RealType, &(Beamz(iptr)), 0);
if (Namex(iptr))
put_string (outstr,NamexTag,Namex(iptr));
if (Namey(iptr))
put_string (outstr,NameyTag,Namey(iptr));
if (Namez(iptr))
put_string (outstr,NamezTag,Namez(iptr));
if (Unit(iptr))
put_string (outstr,UnitTag,Unit(iptr));
put_data (outstr,TimeTag, RealType, &(Time(iptr)), 0);
put_string(outstr,StorageTag,matdef[idef]);
put_data (outstr,AxisTag, IntType, &(Axis(iptr)), 0);
put_tes (outstr, ParametersTag);
put_set (outstr,MapTag);
if (Nz(iptr)==1)
put_data (outstr,MapValuesTag,RealType,
Frame(iptr),Nx(iptr),Ny(iptr),0);
else
put_data (outstr,MapValuesTag,RealType,
Frame(iptr),Nx(iptr),Ny(iptr),Nz(iptr),0);
put_tes (outstr, MapTag);
put_tes (outstr, ImageTag);
return 1;
}
void SymmetryMod::ModifyPolyObject (TimeValue t, ModContext &mc, PolyObject *pobj, INode *inode) {
MNMesh &mesh = pobj->GetMesh();
if (mUseRampageWeldMath)
mesh.SetFlag(MN_MESH_USE_MAX2012_WELD_MATH,TRUE);
// Luna task 747
// We cannot support specified normals in Symmetry at this time.
mesh.ClearSpecifiedNormals();
Interval iv = FOREVER;
int axis, slice, weld, flip;
float thresh;
mp_pblock->GetValue (kSymAxis, t, axis, iv);
mp_pblock->GetValue (kSymFlip, t, flip, iv);
mp_pblock->GetValue (kSymSlice, t, slice, iv);
mp_pblock->GetValue (kSymWeld, t, weld, iv);
mp_pblock->GetValue (kSymThreshold, t, thresh, iv);
if (thresh<0) thresh=0;
// Get transform from mp_mirror controller:
Matrix3 tm = CompMatrix (t, NULL, &mc, &iv);
Matrix3 itm = Inverse (tm);
// Get DotProd(N,x)=off plane definition from transform
Point3 Axis(0,0,0);
Axis[axis] = flip ? -1.0f : 1.0f;
Point3 or = tm.GetTrans();
Point3 N = Normalize(tm*Axis - or);
float off = DotProd (N, or);
if (slice) SlicePolyObject (mesh, N, off);
MirrorPolyObject (mesh, axis, tm, itm);
if (weld) {
WeldPolyObject (mesh, N, off, thresh);
RemovePolySpurs (mesh);
}
pobj->UpdateValidity (GEOM_CHAN_NUM, iv);
pobj->UpdateValidity (TOPO_CHAN_NUM, iv);
pobj->UpdateValidity (VERT_COLOR_CHAN_NUM, iv);
pobj->UpdateValidity (TEXMAP_CHAN_NUM, iv);
pobj->UpdateValidity (SELECT_CHAN_NUM, iv);
}
void initialize(void)
{
//VP_BASIC_RENDERER_WORLD(world);
VP_FRAMEWORK_WORLD(world)
VP_FRAMEWORK_CAMERA(-49.9825, 15.968, 15.5653, -109.286, 78.4562, 0.767533)
vpMaterial::GetDefaultMaterial()->SetRestitution(0.2);
vpMaterial::GetDefaultMaterial()->SetDynamicFriction(1.0);
ground.SetGround();
ground.AddGeometry(new vpBox(Vec3(20, 20, 1)));
ground.AddGeometry(new vpCapsule(0.5, 11.0), Vec3(0, 0, 5));
ground.AddGeometry(new vpCapsule(0.5, 6.0), SE3(Vec3(0, -2.5, 10))*RotX(0.5*M_PI));
for ( int i = 0; i < NUM_CHAIN-1; i++ )
{
chain[i].SetJoint(&J[i], Vec3(10.0 / NUM_CHAIN, 0, 0));
chain[i+1].SetJoint(&J[i], Vec3(-10.0 / NUM_CHAIN, 0, 0));
//chain[i].AddGeometry(new vpBox(Vec3(20.0 / NUM_CHAIN, 0.2, 0.2)));
chain[i].AddGeometry(new vpCapsule(0.1, 20.0 / NUM_CHAIN + 0.2), RotY(0.5*M_PI));
for ( int j = -3; j < 3; j++ )
if ( i+j >= 0 && i+j <= NUM_CHAIN-1 ) world.IgnoreCollision(&chain[i], &chain[i+j]);
J[i].SetDamping(SpatialDamper(20));
J[i].SetElasticity(SpatialSpring(1500));
J[i].SetOrientation(Exp(Axis(0.01, 0.01, 0.01)));
chain[i].SetInertia(Inertia(.1));
}
chain[NUM_CHAIN-1].SetInertia(Inertia(1));
chain[NUM_CHAIN-1].AddGeometry(new vpCapsule(0.1, 20.0 / NUM_CHAIN + 0.2), RotY(0.5*M_PI));
chain[NUM_CHAIN/2].SetFrame(Vec3(0, -2, 15));
world.AddBody(&chain[NUM_CHAIN/2]);
world.AddBody(&ground);
world.SetGravity(Vec3(0.0, 0.0, -10.0));
world.Initialize();
world.SetIntegrator(VP::IMPLICIT_EULER_FAST);
world.SetTimeStep(0.005);
world.BackupState();
vpTimer timer;
timer.Tic();
for ( int i = 0; i < 1000; i++ ) world.StepAhead();
cout << timer.Toc() << chain[10].GetGenVelocity() << endl;
}
void PureProjection::FromGeodeticToCartesian(double Lat,double Lng,double Height, double &X, double &Y, double &Z)
{
Lat = (PI / 180) * Lat;
Lng = (PI / 180) * Lng;
double B = Axis() * (1.0 - Flattening());
double ee = 1.0 - (B / Axis()) * (B / Axis());
double N = (Axis() / sqrt(1.0 - ee * sin(Lat) * sin(Lat)));
X = (N + Height) * cos(Lat) * cos(Lng);
Y = (N + Height) * cos(Lat) * sin(Lng);
Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat);
}
/**
* @brief PureProjection::FromGeodeticToCartesian
* @param Lat_D
* @param Lng_D
* @param Height
* @param X
* @param Y
* @param Z
*/
void PureProjection::FromGeodeticToCartesian(double Lat_D, double Lng_D, double Height, double &X, double &Y, double &Z)
{
double Lat_R = Lat_D * DEG2RAD;
double Lng_R = Lng_D * DEG2RAD;
double B = Axis() * (1.0 - Flattening());
double ee = 1.0 - (B / Axis()) * (B / Axis());
double N = (Axis() / sqrt(1.0 - ee * sin(Lat_R) * sin(Lat_R)));
X = (N + Height) * cos(Lat_R) * cos(Lng_R);
Y = (N + Height) * cos(Lat_R) * sin(Lng_R);
Z = (N * (B / Axis()) * (B / Axis()) + Height) * sin(Lat_R);
}
void Board::initialize()
{
for (int i = 0; i < range; i++)
{
allRow[i] = std::make_shared<Line>(Row);
allColumn[i] = std::make_shared<Line>(Column);
}
for (int i = 0; i < range * 2 - 1; i++)
{
allLeftOblique[i] = std::make_shared<Line>(LeftOblique);
allRightOblique[i] = std::make_shared<Line>(RightOblique);
}
for (int _row = 0; _row < range; _row++)
{
for (int _column = 0; _column < range; _column++)
{
auto point = std::make_shared<Point>(_row, _column);
board[Axis(_row, _column)] = point;
allRow[_row]->AddPoint(board[Axis(_row, _column)]);
allColumn[_column]->AddPoint(board[Axis(_row, _column)]);
}
}
//左上到右下，每一行左下到右上
for (int oblique = 0; oblique < range; oblique++)
for (int row = oblique, column = 0; row >= 0; --row, ++column)
allLeftOblique[oblique]->AddPoint(board[Axis(row, column)]);
for (int oblique = range; oblique < range * 2 - 1; oblique++)
for (int row = range - 1, column = oblique - range + 1; column < range; --row, ++column)
allLeftOblique[oblique]->AddPoint(board[Axis(row, column)]);
//右上到左下，每一行左上到右下
for (int oblique = 0; oblique < range; oblique++)
for (int row = 0, column = 14 - oblique; column < range; ++row, ++column)
allRightOblique[oblique]->AddPoint(board[Axis(row, column)]);
for (int oblique = range; oblique < range * 2 - 1; oblique++)
for (int row = oblique - range + 1, column = 0; row < range; ++row, ++column)
allRightOblique[oblique]->AddPoint(board[Axis(row, column)]);
for (auto &pair : allRow)
totalLine.push_back(pair.second);
for (auto &pair : allColumn)
totalLine.push_back(pair.second);
for (auto &pair : allLeftOblique)
totalLine.push_back(pair.second);
for (auto &pair : allRightOblique)
totalLine.push_back(pair.second);
}
cImgListRender::cImgListRender(cTextureList* pTxList, bool isCenter)
: m_pPlane(NULL)
, m_pTxList(pTxList)
, m_onMt(false)
{
D3DXVECTOR3 Axis(0, 1, 0);
D3DXMatrixTranslation(&m_Mat[T], 0, 0, 0);
D3DXMatrixIdentity(&m_Mat[R]);
D3DXMatrixScaling(&m_Mat[S], 1.f, 1.f, 1.f);
// 보드 생성
m_pPlane = new cPlane;
m_pPlane->Create(0, 0, static_cast<float>(pTxList->GetTexture(0)->Width())/32.f
, static_cast<float>(pTxList->GetTexture(0)->Height())/32.f, 1, 1
, true, isCenter);
}
void
DOMSVGLength::ConvertToSpecifiedUnits(uint16_t aUnit, ErrorResult& aRv)
{
if (mIsAnimValItem) {
aRv.Throw(NS_ERROR_DOM_NO_MODIFICATION_ALLOWED_ERR);
return;
}
if (mVal) {
mVal->ConvertToSpecifiedUnits(aUnit, mSVGElement);
return;
}
if (!SVGLength::IsValidUnitType(aUnit)) {
aRv.Throw(NS_ERROR_DOM_NOT_SUPPORTED_ERR);
return;
}
if (HasOwner()) {
if (InternalItem().GetUnit() == aUnit) {
return;
}
float val = InternalItem().GetValueInSpecifiedUnit(
aUnit, Element(), Axis());
if (IsFinite(val)) {
AutoChangeLengthNotifier notifier(this);
InternalItem().SetValueAndUnit(val, aUnit);
return;
}
} else {
SVGLength len(mValue, mUnit);
float val = len.GetValueInSpecifiedUnit(aUnit, nullptr, 0);
if (IsFinite(val)) {
mValue = val;
mUnit = aUnit;
return;
}
}
// else [SVGWG issue] Can't convert unit
// ReportToConsole
aRv.Throw(NS_ERROR_FAILURE);
}
/*
* CREATE_CUBE: create a blank cube Nx by Ny by Nz in size
*/
int create_cube (imageptr *iptr, int nx, int ny, int nz)
{
*iptr = (imageptr ) allocate(sizeof(image));
dprintf (DLEV,"CREATE_CUBE: Allocated image @ cube %d size=%d * %d * %d",
*iptr,nx,ny,nz);
if (*iptr == NULL)
return 0; /* no memory available */
Frame(*iptr) = (real *) allocate(nx*ny*nz*sizeof(real));
dprintf (DLEV,"Frame allocated @ %d ",Frame(*iptr));
Nx(*iptr) = nx; /* cube dimension */
Ny(*iptr) = ny;
Nz(*iptr) = nz;
Xmin(*iptr) = 0.0; /* start lower left corner at 0.0 */
Ymin(*iptr) = 0.0;
Zmin(*iptr) = 0.0;
Xref(*iptr) = 0.0;
Yref(*iptr) = 0.0;
Zref(*iptr) = 0.0;
Dx(*iptr) = 1.0; /* unity pixels */
Dy(*iptr) = 1.0;
Dz(*iptr) = 1.0;
MapMin(*iptr) = 0.0;
MapMax(*iptr) = 0.0;
BeamType(*iptr) = 0;
Beamx(*iptr) = 0.0; /* name beams */
Beamy(*iptr) = 0.0;
Beamz(*iptr) = 0.0;
Namex(*iptr) = NULL; /* no axis names yet */
Namey(*iptr) = NULL;
Namez(*iptr) = NULL;
Unit(*iptr) = NULL; /* no units */
Time(*iptr) = 0.0;
Storage(*iptr) = matdef[idef];
Axis(*iptr) = 0;
Mask(*iptr) = NULL;
set_iarray(*iptr);
return 1; /* succes return code */
}
/** creates vehicle at desired position
@param pos struct Position with desired position
*/
void Hexabot::create(const osg::Matrix& pose){
if (created) {
destroy();
}
#ifdef DEBUG_MODE
cout << "hello, This is the debug mode" << endl;
cout << "@@@@ First, We create the robot!! @@@@" << endl;
#endif
/*********************************************************************************:
* The ordinal configuration
* Environment, color, feature etc/
***********************************************************************************/
// we want legs colliding with other legs, so we set internal collision
// flag to "false".
odeHandle.createNewSimpleSpace(parentspace,false);
// Hexabot color ;-)
osgHandle.color = Color(0/255., 30./255., 0./255., 1.0f);
// Joint color ;-)
OsgHandle dynaHandle(osgHandle);
dynaHandle.color = Color(204/255., 51/255., 0./255., 1.0f);
//dynaHandle.color = Color(0/255., 0/255., 0./255., 1.0f);
// Tibia Color (colored by leg)
OsgHandle tibiaHandle[6] = {osgHandle, osgHandle, osgHandle, osgHandle, osgHandle, osgHandle};
tibiaHandle[0].color = Color(0./255., 0./255., 0./255., 1.0f);
tibiaHandle[1].color = Color(153./255., 102./255., 0./255., 1.0f);
tibiaHandle[2].color = Color(153./255., 204./255., 0./255., 1.0f);
tibiaHandle[3].color = Color(153./255., 0./255., 30./255., 1.0f);
tibiaHandle[4].color = Color(153./255., 102./255., 30./255., 1.0f);
tibiaHandle[5].color = Color(153./255., 204./255., 30./255., 1.0f);
// change Material substance
OdeHandle odeHandleBody(odeHandle);
odeHandleBody.substance.toMetal(3.0);
// get a representation of the origin
// By multipling this parameter to pose, we can get only the position data of pose
const Pos nullpos(0, 0, 0);
/*********************************************************************************:
* Create Body Structure
* Trunk, Leg etc.
***********************************************************************************/
/*********************************************
* Main Frame */
// Make the boxes connect each other to make rectangle
/**********************************************/
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Body frame starts!! @@@@" << endl;
#endif
// make the rectangle boxes to make hexagon (they are connected each other and make hexagon)
// make 2 rectangle plate objects
for(int i=0; i< 2;i++){
trunk.tPlate[i] = new Box( conf.body.length, conf.body.width, conf.body.height);
trunk.tPlate[i]->getOSGPrimitive()->setTexture("Images/wood.rgb");
// This function (setMass) seems not to be implemented
//trunk.tPlate[i]->setMass(conf.body.mass / 6.);
}
// First object should be initialized (I choose the lower rectangle one)
trunk.tPlate[0]->init(odeHandle, conf.body.mass / 2., osgHandle);
trunk.tPlate[0]->setPose( pose );
// add it to Object
objects.push_back(trunk.tPlate[0]);
// just for memorlizing transForm
trunk.tUpTrans = new ImpTransform2(trunk.tPlate[0], trunk.tPlate[1],Matrix::rotate(0., Vec3(0, 0, 1)) *Matrix::translate(0, 0, conf.dyna.height));
trunk.tTrans = trunk.tUpTrans;
trunk.tTrans->init(odeHandle, conf.body.mass / 2., osgHandle);
// add it to Object
objects.push_back(trunk.tTrans);
/*********************************************
LEGs **sholder, coxa femur tibia
//
**********************************************/
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Leg frame starts!! @@@@" << endl;
#endif
// Useful Parameter to make robot model
// Transition Matrix from origin of this robot to center of the robot
// notice: the orgin of the robot is on the center of the lower hexagonal plate
osg::Matrix trans_rO_to_rC = Matrix::translate( 0., 0., conf.dyna.height / 2.);
// Trans Matrix from center of the robot to Shoulder Dynamixel center
// The horizontal length between robot center and Shoulder Dynamixel center
double length_rC_to_sC_x = (conf.jLength.length_x_center_to_TCJ - conf.dyna.length_axis_to_center);
// Matrix
osg::Matrix trans_rC_to_sC_x = Matrix::translate( length_rC_to_sC_x, 0, 0);
// The horizontal length between Shoulder Dynamixel center and Shoulder Dynamixel center
// Matrix
osg::Matrix trans_sC_to_sC_y = Matrix::translate(0, conf.jLength.length_y_TCJ_to_TCJ, 0);
// Trans Matrix from center of the robot to Coxa Dynamixel center
// The horizontal length between robot center and Coxa Dynamixel center
double length_rC_to_cC = length_rC_to_sC_x + conf.jLength.length_TCJ_to_CTJ;
// Matrix
osg::Matrix trans_rC_to_cC = Matrix::translate( length_rC_to_cC, 0, 0);
// Trans Matrix from center of the robot to Femur Dynamixel center
// The horizontal length between robot center and Femur Dynamixel center
double length_rC_to_fC_x = conf.jLength.length_x_center_to_TCJ + conf.jLength.length_TCJ_to_CTJ;
double length_rC_to_fC_z = conf.jLength.length_CTJ_to_FTJ - conf.dyna.length_axis_to_center;
//double length_rC_to_fC = length_rC_to_cC + conf.jLength.length_CTJ_to_FTJ;
// Matrix
osg::Matrix trans_rC_to_fC = Matrix::translate( length_rC_to_fC_x, 0, length_rC_to_fC_z);
// Trans Matrix from center of the robot to Foot Plate center
// The horizontal length between robot center and Foot Plate center
double length_rC_to_tC_x = length_rC_to_fC_x + conf.dyna.length_from_axis_to_tip;
double length_rC_to_tC_z = conf.jLength.length_CTJ_to_FTJ + (-conf.foot.length + conf.dyna.width)/2.;
// Matrix
osg::Matrix trans_rC_to_tC = Matrix::translate( length_rC_to_tC_x, 0, length_rC_to_tC_z);
// Trans Matrix from center of the robot to center of the foot sphere
// Matrix
osg::Matrix trans_rC_to_fsC = Matrix::translate( length_rC_to_fC_x, 0, length_rC_to_tC_z - conf.foot.length/2. - conf.foot.footRadius );
// Trans Matrix from center of the tibia to center of the foot sphere
// Matrix
osg::Matrix trans_tC_to_fsC = Matrix::translate( - conf.dyna.length_from_axis_to_tip, 0, - conf.foot.length/2. - conf.foot.footRadius);
// Trans Matrix from center of the robot to TC joint Center
// Matrix
osg::Matrix trans_rC_to_TCj = Matrix::translate( conf.jLength.length_x_center_to_TCJ, 0, 0);
// Trans Matrix from center of the robot to CT joint Center
// Matrix
osg::Matrix trans_rC_to_CTj = Matrix::translate( conf.jLength.length_x_center_to_TCJ + conf.jLength.length_TCJ_to_CTJ, 0, 0);
// Trans Matrix from center of the robot to FT joint Center
// Matrix
osg::Matrix trans_rC_to_FTj = Matrix::translate( conf.jLength.length_x_center_to_TCJ + conf.jLength.length_TCJ_to_CTJ, 0, + conf.jLength.length_CTJ_to_FTJ);
for(int i = 0; i < LEG_POS_MAX; i++){
// Name of the Leg
LegPos leg = LegPos(i);
// Rotation Matrix : change the rotation direction according to leg number
// By using this, the direction of leg can be changed.. It is used to adapt to the location of the leg
Matrix legRotate;
Matrix legTrans;
if(leg == L1){
legRotate = Matrix::rotate(M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, conf.jLength.length_y_TCJ_to_TCJ + conf.y_trans_center, 0);
}
else if( leg == L2){
legRotate = Matrix::rotate(M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, conf.y_trans_center, 0);
}
else if(leg == L3 ){
legRotate = Matrix::rotate(M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, -conf.jLength.length_y_TCJ_to_TCJ +conf.y_trans_center, 0);
}else if(leg == L4){
legRotate = Matrix::rotate(-M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, -conf.jLength.length_y_TCJ_to_TCJ - conf.y_trans_center, 0);
}else if(leg == L5){
legRotate = Matrix::rotate(-M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, -conf.y_trans_center, 0);
}else{
legRotate = Matrix::rotate(-M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, conf.jLength.length_y_TCJ_to_TCJ - conf.y_trans_center, 0);
}
// Shoulder ******************************************
// We connect the sholder dynamixel to main Body
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Shoulder starts!! @@@@" << endl;
#endif
// sholder Dynamixel Box
Box* dBox = new Box( conf.dyna.length, conf.dyna.width, conf.dyna.height);
dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
//dBox->setMass(conf.dyna.mass);
// trans (locate the object on the hexagon plane)
// first, I turn the object to leg's direction and translate it to desired position ()
// mention that this pose matrix is multipled vector from right hand <- it is different from usual one!!
ImpTransform2* dTrans = new ImpTransform2(trunk.tPlate[0], dBox,
trans_rC_to_sC_x * trans_rO_to_rC * legTrans * legRotate);
dTrans->init(odeHandle, conf.dyna.mass, dynaHandle);
// save to the leg struct
legs[leg].shoulderBox = dBox;
legs[leg].shoulder = dTrans;
// add it to object
objects.push_back(legs[leg].shoulder);
// Coxa **********************************************
// We make the first joint and link of Hexabot
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Coxa starts!! @@@@" << endl;
#endif
// Make the Dyanmixel for link
// Coxa (1st link) Dynamixel Box
Box* link1dBox = new Box( conf.dyna.length, conf.dyna.width, conf.dyna.height);
link1dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
link1dBox->init(odeHandle, conf.dyna.mass , dynaHandle);
// about the pose or something
// set the pose
// pose:: pose represent the attitude and position of the object by quotanion
// we can use the function rotate and translate as if it is attitude change 4*4 matrix
//
// I should say about mechanism of the pose in my understanding.
// Pose ; this is the 4 by 4 matrix and it represents the rotation and translation
// It can be used as if it is normal 4 by 4 rotation matrix.
// But what you should notice is that this matrix is for Row Vector (1 by 4 vector)
// So, the row of the calculating is inverse to normal one!!
// You should be careful about it
Matrix l1Pose = Matrix::rotate( M_PI / 2., Vec3(1, 0, 0)) * /* this rotation is used to turn the dymamixel to optimal attitude!! (because, we change the axis of joints) */
trans_rC_to_cC * trans_rO_to_rC * legTrans* legRotate * pose; // first, I turn the object to leg's direction and translate it to desired position ()
link1dBox->setPose(l1Pose);
// save it to leg struct
legs[leg].coxa = link1dBox;
// add it to object
objects.push_back(legs[leg].coxa);
// TC joints *******************************:::::::
// create the joint of dynamixel on Body (from 1st link to body)
// calculate the joint pose (attitude and position) by using trans and rotate function
osg::Matrix j1Pose = trans_rC_to_TCj * trans_rO_to_rC * legTrans* legRotate * pose;
// To make a joint, we need the position vector only, so multiple nullpos to the pose to get the vector.
// the attitude is determined by axis (it is z axis)
HingeJoint* TCj = new HingeJoint( trunk.tPlate[0], link1dBox, nullpos * j1Pose, Axis(0,0,1) * legRotate );
// ^ Notice: When we want to rotate the Axis() like Pose, we have to multiple the rotation matrix from RIGHT SIDE
// Not Left side, it is very different from real calculation
TCj->init(odeHandle, osgHandle, true, conf.rate * 0.04);
// add the joint to the joint vector
joints.push_back(TCj);
// create motor (by using servo motor, we will do position control)
//OneAxisServo* servo1 = new OneAxisServoVel(odeHandle, TCj, conf.servoParam.TC_angle_MIN, conf.servoParam.TC_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
//Debug
OneAxisServo* servo1;
#ifdef HEXABOT_MAIN_VER_1_6
servo1 = new OneAxisServoVel(odeHandle, TCj, conf.servoParam.TC_angle_MIN, conf.servoParam.TC_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
#else
servo1 = new OneAxisServoCentered(TCj, conf.servoParam.TC_angle_MIN, conf.servoParam.TC_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.integ, conf.servoParam.maxVel, 1.0);
#endif
// save it to leg struct
legs[leg].tcJoint = TCj;
legs[leg].tcServo = servo1;
// add it to servo Map
servos[getMotorName(leg, TC)] = servo1;
// create torque sensor
// In this time, I do not use scaller just get real value
motorTorqSensors[getMotorName(leg, TC)] = new TorqueSensor(1., 1.);
// initialize (own is Null, it was not used in simulation)
motorTorqSensors[getMotorName(leg, TC)]->init(NULL, TCj);
// Femur **********************************************
// We make the second joint and link of Hexabot (CT joint and femur)
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Femur starts!! @@@@" << endl;
#endif
// Make the Dyanmixel for link
// Femur (2nd link) Dynamixel Box
Box* link2dBox = new Box( conf.dyna.length, conf.dyna.width, conf.dyna.height);
link2dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
link2dBox->init(odeHandle, conf.dyna.mass , dynaHandle);
// set the pose
// pose:: pose represent the attitude and position of the object by quotanion
// we can use the function rotate and translate as if it is attitude change 4*4 matrix
osg::Matrix l2Pose = Matrix::rotate( M_PI / 2., Vec3(0, 0, 1)) * Matrix::rotate( M_PI / 2., Vec3(1, 0, 0)) * // this rotation is used to turn the dymamixel to optimal attitude!! (because, we change the axis of joints)
trans_rC_to_fC * trans_rO_to_rC * legTrans * legRotate * pose; // first, I turn the object to leg's direction and translate it to desired position ()
link2dBox->setPose(l2Pose);
// save it to leg struct
legs[leg].femur = link2dBox;
// add it to object
objects.push_back(legs[leg].femur);
// CT joint :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
// create the joint of dynamixel on Body (from 2nd link to 1st Link)
// calculate the joint pose (attitude and position) by using trans and rotate function
osg::Matrix j2Pose = trans_rC_to_CTj * trans_rO_to_rC * legTrans* legRotate * pose;
// To make a joint, we need the position vector only, so multiple nullpos to the pose to get the vector.
// the attitude is determined by axis (it is z axis)
HingeJoint* CTj = new HingeJoint( legs[leg].coxa, link2dBox, nullpos * j2Pose, Axis(0,1,0) * legRotate);
// ^ Notice: When we want to rotate the Axis() like Pose, we have to multiple the rotation matrix from RIGHT SIDE
// Not Left side, it is very different from real calculation
CTj->init(odeHandle, osgHandle, true, conf.rate * 0.04);
// add the joint to the joint vector
joints.push_back(CTj);
// create motor (by using servo motor, we will do position control)
// OneAxisServo* servo2 = new OneAxisServoVel(odeHandle, CTj, conf.servoParam.CT_angle_MIN, conf.servoParam.CT_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
//Debug
OneAxisServo* servo2;
#ifdef HEXABOT_MAIN_VER_1_6
servo2 = new OneAxisServoVel(odeHandle, CTj, conf.servoParam.CT_angle_MIN, conf.servoParam.CT_angle_MAX, conf.servoParam.power, conf.servoParam.damp,conf.servoParam.maxVel, 1.0);
#else
servo2 = new OneAxisServoCentered(CTj, conf.servoParam.CT_angle_MIN, conf.servoParam.CT_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.integ, conf.servoParam.maxVel, 1.0);
#endif
// save it to leg struct
legs[leg].ctJoint = CTj;
legs[leg].ctServo = servo2;
// add it to servo Map
servos[Hexabot::getMotorName(leg, CT)] = servo2;
// create torque sensor
// In this time, I do not use scaller just get real value
motorTorqSensors[getMotorName(leg, CT)] = new TorqueSensor(1., 1.);
// initialize (own is Null, it was not used in simulation)
motorTorqSensors[getMotorName(leg, CT)]->init(NULL, CTj);
// Tibia **********************************************
// We make the third joint and link of Hexabot (FT joint and femur)
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Tibia starts!! @@@@" << endl;
#endif
// Make the Plate
// Tibia (3rd link) Plate
Box* link3dBox = new Box( conf.foot.height, conf.foot.width, conf.foot.length);
link3dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
link3dBox->init(odeHandle, conf.foot.mass , tibiaHandle[leg]);
// set the pose
// pose:: pose represent the attitude and position of the object by quotanion
// we can use the function rotate and translate as if it is attitude change 4*4 matrix
osg::Matrix l3Pose = trans_rC_to_tC * trans_rO_to_rC * legTrans* legRotate * pose; // first, I turn the object to leg's direction and translate it to desired position ()
link3dBox->setPose(l3Pose);
// save it to leg struct
legs[leg].tibia = link3dBox;
// add it to object
objects.push_back(legs[leg].tibia);
//Make Foot
// Sphere
Sphere* foot = new Sphere( conf.foot.footRadius );
foot->getOSGPrimitive()->setTexture("Images/wood.rgb");
// Set the substance of foot
OdeHandle rubberHandle(odeHandle);
const Substance FootSubstance(3.0, 0.0, 500.0, 0.1);//500
rubberHandle.substance = FootSubstance;
legs[leg].footSphere = foot;
//translate
legs[leg].foot = new ImpTransform2(legs[leg].tibia, foot, trans_tC_to_fsC);
// initialize
legs[leg].foot->init(rubberHandle, 0.0, osgHandle);
//add it to objects
objects.push_back(legs[leg].foot);
// FT joint :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
// create the joint of dynamixel on Femur (from 3rd link to 2nd Link)
// calculate the joint pose (attitude and position) by using trans and rotate function
osg::Matrix j3Pose = trans_rC_to_FTj * trans_rO_to_rC * legTrans* legRotate * pose;
// To make a joint, we need the position vector only, so multiple nullpos to the pose to get the vector.
// the attitude is determined by axis (it is z axis)
HingeJoint* FTj = new HingeJoint( legs[leg].femur, link3dBox, nullpos * j3Pose, Axis(0,1,0) * legRotate);
// ^ Notice: When we want to rotate the Axis() like Pose, we have to multiple the rotation matrix from RIGHT SIDE
// Not Left side, it is very different from real calculation
FTj->init(odeHandle, osgHandle, true, conf.rate * 0.04);
// add the joint to the joint vector
joints.push_back(FTj);
// create motor (by using servo motor, we will do position control)
// OneAxisServo* servo3 = new OneAxisServoVel(odeHandle, FTj, conf.servoParam.FT_angle_MIN, conf.servoParam.FT_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
//Debug
OneAxisServo* servo3;
#ifdef HEXABOT_MAIN_VER_1_6
servo3 = new OneAxisServoVel(odeHandle, FTj, conf.servoParam.FT_angle_MIN, conf.servoParam.FT_angle_MAX, conf.servoParam.power, conf.servoParam.damp,conf.servoParam.maxVel, 1.0);
#else
servo3 = new OneAxisServoCentered(FTj, conf.servoParam.FT_angle_MIN, conf.servoParam.FT_angle_MAX, conf.servoParam.power, conf.servoParam.damp,conf.servoParam.integ, conf.servoParam.maxVel, 1.0);
#endif
// save it to leg struct
legs[leg].ftJoint = FTj;
legs[leg].ftServo = servo3;
// add it to servo Map
servos[Hexabot::getMotorName(leg, FT)] = servo3;
// create torque sensor
// In this time, I do not use scaller just get real value
motorTorqSensors[getMotorName(leg, FT)] = new TorqueSensor(1., 1.);
// initialize (own is Null, it was not used in simulation)
motorTorqSensors[getMotorName(leg, FT)]->init(NULL, FTj);
// Leg contact Sensors (Foot toe)
legContactSensors[leg] = new ContactSensor(false, 100, conf.foot.footRadius*1.1, false, true, Color(0,5,0));
//make the sphere a little bit larger than real foot to detect touching 1.01
//legContactSensors[leg]->update();
// We set the contact sensor at the point of foot sphere
//legContactSensors[leg]->init(odeHandle, osgHandle, legs[leg].tibia, true, trans_tC_to_fsC);
// this is changed to adopt the new version of lpzrobots
legContactSensors[leg]->setInitData(odeHandle, osgHandle, TRANSM(0, 0, 0));//trans_tC_to_fsC);
legContactSensors[leg]->init(legs[leg].foot);
//legContactSensors[leg]->update();
//odeHandle.addIgnoredPair(legs[leg].foot, legContactSensors[leg]->getTransformObject());
//legContactSensors[LegPos(i)]->setInitData(odeHandle, osgHandle, TRANSM(0, 0, -(0.5) * l4));
//legContactSensors[LegPos(i)]->init(foot);
}
#ifdef DEBUG_MODE
cout << "@@@@ Creation Phase ends!! @@@@" << endl;
#endif
created=true;
}
