void SSimRobotEntity::setInitPosition(Vector3d pos)
{
int jsize = m_allJoints.size();
for (int i = 0; i < jsize; i++) {
SSimJoint *sjoint = m_allJoints[i];
SSimRobotParts rparts = sjoint->robotParts;
//dBodyID body = m_allJoints[i]->robotParts.objParts.body;
dBodyID body = rparts.objParts.body;
dJointID joint = sjoint->joint;
//dGeomID geom = rparts.objParts.geoms[0];
if (i == 0) {
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
//dGeomSetPosition(geom,pos.x(),pos.y(),pos.z());
//const dReal *gpos = dGeomGetPosition(geom);
//LOG_MSG(("root body pos = (%f, %f, %f)", pos.x(), pos.y(), pos.z()));
//LOG_MSG(("root geom pos = (%f, %f, %f)", gpos[0], gpos[1], gpos[2]));
}
else {
// TODO: deal with a case in non-hinge joint
// gap between joint from root joint
dReal trans_x = sjoint->posFromRoot.x();
dReal trans_y = sjoint->posFromRoot.y();
dReal trans_z = sjoint->posFromRoot.z();
//LOG_MSG(("zure (%f, %f, %f)", trans_x, trans_y, trans_z));
//LOG_MSG(("zure of body(%f, %f, %f)", rparts.com.x(), rparts.com.y(), rparts.com.z()));
dBodySetPosition(body,
pos.x()+trans_x+rparts.com.x(),
pos.y()+trans_y+rparts.com.y(),
pos.z()+trans_z+rparts.com.z());
//const dReal *gpos = dGeomGetPosition(geom);
LOG_MSG(("body(id:%d) pos = (%f, %f, %f)",body, pos.x()+trans_x+rparts.com.x(), pos.y()+trans_y+rparts.com.y(), pos.z()+trans_z+rparts.com.z()));
//LOG_MSG(("geom pos = (%f, %f, %f)", gpos[0], gpos[1], gpos[2]));
// setting of joint position
int type = dJointGetType(joint);
if (type == dJointTypeHinge) {
dJointSetHingeAnchor(joint,
pos.x()+trans_x,
pos.y()+trans_y,
pos.z()+trans_z);
}
LOG_MSG(("joint(%d) pos = (%f, %f, %f)",joint, pos.x()+trans_x, pos.y()+trans_y, pos.z()+trans_z));
/*
dGeomSetPosition(geom,
pos.x()+trans_x+rparts.com.x(),
pos.y()+trans_y+rparts.com.y(),
pos.z()+trans_z+rparts.com.z());
*/
}
}
}
static void _soy_joints_hinge_anchor_set (soyjointsHinge* self, soyatomsPosition* anchor) {
struct dxJoint* _tmp0_;
soyatomsPosition* _tmp1_;
gfloat _tmp2_;
gfloat _tmp3_;
soyatomsPosition* _tmp4_;
gfloat _tmp5_;
gfloat _tmp6_;
soyatomsPosition* _tmp7_;
gfloat _tmp8_;
gfloat _tmp9_;
g_return_if_fail (self != NULL);
g_return_if_fail (anchor != NULL);
g_rw_lock_writer_lock (&soy_scenes__stepLock);
_tmp0_ = ((soyjointsJoint*) self)->joint;
_tmp1_ = anchor;
_tmp2_ = soy_atoms_position_get_x (_tmp1_);
_tmp3_ = _tmp2_;
_tmp4_ = anchor;
_tmp5_ = soy_atoms_position_get_y (_tmp4_);
_tmp6_ = _tmp5_;
_tmp7_ = anchor;
_tmp8_ = soy_atoms_position_get_z (_tmp7_);
_tmp9_ = _tmp8_;
dJointSetHingeAnchor ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_);
g_rw_lock_writer_unlock (&soy_scenes__stepLock);
}
// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
// ang2 = position angle
// ang = rotation angle
Robot::Wheel::Wheel(Robot* robot,int _id,float ang,float ang2,int wheeltexid)
{
id = _id;
rob = robot;
float rad = rob->cfg->robotSettings.RobotRadius - rob->cfg->robotSettings.WheelThickness / 2.0;
ang *= M_PI/180.0f;
ang2 *= M_PI/180.0f;
float x = rob->m_x;
float y = rob->m_y;
float z = rob->m_z;
float centerx = x+rad*cos(ang2);
float centery = y+rad*sin(ang2);
float centerz = z-rob->cfg->robotSettings.RobotHeight*0.5+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight;
cyl = new PCylinder(centerx,centery,centerz,rob->cfg->robotSettings.WheelRadius,rob->cfg->robotSettings.WheelThickness,rob->cfg->robotSettings.WheelMass,0.9,0.9,0.9,wheeltexid);
cyl->setRotation(-sin(ang),cos(ang),0,M_PI*0.5);
cyl->setBodyRotation(-sin(ang),cos(ang),0,M_PI*0.5,true); //set local rotation matrix
cyl->setBodyPosition(centerx-x,centery-y,centerz-z,true); //set local position vector
cyl->space = rob->space;
rob->w->addObject(cyl);
joint = dJointCreateHinge (rob->w->world,0);
dJointAttach (joint,rob->chassis->body,cyl->body);
const dReal *a = dBodyGetPosition (cyl->body);
dJointSetHingeAxis (joint,cos(ang),sin(ang),0);
dJointSetHingeAnchor (joint,a[0],a[1],a[2]);
motor = dJointCreateAMotor(rob->w->world,0);
dJointAttach(motor,rob->chassis->body,cyl->body);
dJointSetAMotorNumAxes(motor,1);
dJointSetAMotorAxis(motor,0,1,cos(ang),sin(ang),0);
dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax);
speed = 0;
}
Robot::Kicker::Kicker(Robot* robot)
{
rob = robot;
float x = rob->m_x;
float y = rob->m_y;
float z = rob->m_z;
float centerx = x+(rob->cfg->robotSettings.RobotCenterFromKicker+rob->cfg->robotSettings.KickerThickness);
float centery = y;
float centerz = z-(rob->cfg->robotSettings.RobotHeight)*0.5f+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight+rob->cfg->robotSettings.KickerZ;
box = new PBox(centerx,centery,centerz,rob->cfg->robotSettings.KickerThickness,rob->cfg->robotSettings.KickerWidth,rob->cfg->robotSettings.KickerHeight,rob->cfg->robotSettings.KickerMass,0.9,0.9,0.9);
box->setBodyPosition(centerx-x,centery-y,centerz-z,true);
box->space = rob->space;
rob->w->addObject(box);
joint = dJointCreateHinge (rob->w->world,0);
dJointAttach (joint,rob->chassis->body,box->body);
const dReal *aa = dBodyGetPosition (box->body);
dJointSetHingeAnchor (joint,aa[0],aa[1],aa[2]);
dJointSetHingeAxis (joint,0,-1,0);
dJointSetHingeParam (joint,dParamVel,0);
dJointSetHingeParam (joint,dParamLoStop,0);
dJointSetHingeParam (joint,dParamHiStop,0);
rolling = 0;
kicking = false;
}
void Hinge::createPhysics()
{
ASSERT(axis);
ASSERT(axis->motor);
//
axis->create();
//
PrimaryObject::createPhysics();
// find bodies to connect
Body* parentBody = dynamic_cast<Body*>(parent);
ASSERT(!parentBody || parentBody->body);
ASSERT(!children.empty());
Body* childBody = dynamic_cast<Body*>(*children.begin());
ASSERT(childBody);
ASSERT(childBody->body);
// create joint
joint = dJointCreateHinge(Simulation::simulation->physicalWorld, 0);
dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
//set hinge joint parameter
dJointSetHingeAnchor(joint, pose.translation.x, pose.translation.y, pose.translation.z);
Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
dJointSetHingeAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
if(axis->cfm != -1.f)
dJointSetHingeParam(joint, dParamCFM, dReal(axis->cfm));
if(axis->deflection)
{
const Axis::Deflection& deflection = *axis->deflection;
float minHingeLimit = deflection.min;
float maxHingeLimit = deflection.max;
if(minHingeLimit > maxHingeLimit)
minHingeLimit = maxHingeLimit;
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
float internalTolerance = (maxHingeLimit - minHingeLimit) * 0.1f;
if(dynamic_cast<ServoMotor*>(axis->motor))
{
minHingeLimit -= internalTolerance;
maxHingeLimit += internalTolerance;
}
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
dJointSetHingeParam(joint, dParamHiStop, dReal(maxHingeLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
if(deflection.stopCFM != -1.f)
dJointSetHingeParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
if(deflection.stopERP != -1.f)
dJointSetHingeParam(joint, dParamStopERP, dReal(deflection.stopERP));
}
// create motor
axis->motor->create(this);
OpenGLTools::convertTransformation(rotation, translation, transformation);
}
/*** 車輪の生成 ***/
void makeWheel()
{
dMass mass;
dReal r = 0.1;
dReal w = 0.024;
dReal m = 0.15;
dReal d = 0.01;
dReal tmp_z = Z;
dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0};
dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)};
dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0};
dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2};
dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
for (int i = 0; i < WHEEL_NUM; i++)
{
wheel[i].v = 0.0;
// make body, geom and set geom to body.
// The position and orientation of geom is abondoned.
wheel[i].body = dBodyCreate(world);
wheel[i].geom = dCreateCylinder(space,r, w);
dGeomSetBody(wheel[i].geom,wheel[i].body);
// set configure of body
dMassSetZero(&mass);
if (i < 2) {
dMassSetCylinderTotal(&mass,m, 1, r, w);
} else {
dMassSetCylinderTotal(&mass,m, 2, r, w);
}
dBodySetMass(wheel[i].body,&mass);
// set position and orientation of body.
dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]);
dMatrix3 R;
if (i >= 2) {
dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
} else {
dRFromAxisAndAngle(R,0,1,0,M_PI/2.0);
dBodySetRotation(wheel[i].body,R);
}
// make joint.
wheel[i].joint = dJointCreateHinge(world,0);
dJointAttach(wheel[i].joint,base.body,wheel[i].body);
if (i < 2) {
dJointSetHingeAxis(wheel[i].joint, 1, 0, 0);
} else {
dJointSetHingeAxis(wheel[i].joint, 0, -1, 0);
}
dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]);
}
}
/**
* This method is called if the joint should be attached.
* It creates the ODE-joint, calculates the current anchor-position
* and axis-orientation and attaches the Joint.
* @param obj1 first ODE-object to attach with
* @param obj2 second ODE-object to attach with
**/
void HingeJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
gmtl::Vec3f newAnchor, newAxis, scaleVec;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
TransformationData entityTrans;
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, obj1, obj2);
newAnchor = anchor;
newAxis = axis;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
scaleVec = entityTrans.scale;
// scale Anchor-offset by mainEntity-scale value
newAnchor[0] *= scaleVec[0];
newAnchor[1] *= scaleVec[1];
newAnchor[2] *= scaleVec[2];
// scale Axis by mainEntity-scale value because of possible distortion
newAxis[0] *= scaleVec[0];
newAxis[1] *= scaleVec[1];
newAxis[2] *= scaleVec[2];
gmtl::normalize(newAxis);
// get the Rotation of the mainEntity
entityRot = entityTrans.orientation;
// rotate Axis by mainEntity-rotation
newAxis *= entityRot;
// rotate Anchor-offset by mainEntity-rotation
newAnchor *= entityRot;
// transform new Anchor to world coordinates
newAnchor += entityTrans.position;
} // if
dJointSetHingeAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
dJointSetHingeAxis(joint, newAxis[0], newAxis[1], newAxis[2]);
// ODE parameters for minimizing failure in Hinge Joint
// It should not be correct this way but it works better than without it ;-)
dJointSetHingeParam(joint, dParamCFM, 0);
dJointSetHingeParam(joint, dParamStopERP, 0);
dJointSetHingeParam(joint, dParamStopCFM, 0);
oldAngle = 0;
// set the maximum and minimum Joint-angles (if set).
if (anglesSet)
setODEAngles();
} // attachJoint
void HingeJoint::applyAnchor(dReal x, dReal y, dReal z)
{
dJointSetHingeAnchor(m_joint, x, y, z);
// Set of max and min of joint angle
dJointSetHingeAxis (m_joint, m_axis.x(), m_axis.y(), m_axis.z());
dJointSetHingeParam(m_joint, dParamLoStop, -2.0*M_PI);
dJointSetHingeParam(m_joint, dParamHiStop, 2.0*M_PI);
}
bool ActiveHingeModel::initModel() {
// Create the 4 components of the hinge
hingeRoot_ = this->createBody(B_SLOT_A_ID);
dBodyID frame = this->createBody(B_FRAME_ID);
dBodyID servo = this->createBody(B_SERVO_ID);
hingeTail_ = this->createBody(B_SLOT_B_ID);
// Set the masses for the various boxes
float separation = 0;//inMm(0.1);
this->createBoxGeom(hingeRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
dReal xFrame = separation + FRAME_LENGTH / 2 + SLOT_THICKNESS / 2;
this->createBoxGeom(frame, MASS_FRAME, osg::Vec3(xFrame,
SERVO_POSITION_OFFSET, 0),
FRAME_LENGTH, FRAME_WIDTH, FRAME_HEIGHT);
dReal xServo = xFrame + (FRAME_ROTATION_OFFSET - (FRAME_LENGTH / 2))
+ SERVO_ROTATION_OFFSET - SERVO_LENGTH/2;
this->createBoxGeom(servo, MASS_SERVO, osg::Vec3(xServo,
SERVO_POSITION_OFFSET, 0),
SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT);
dReal xTail = xServo + SERVO_LENGTH / 2 + separation + SLOT_THICKNESS / 2;
this->createBoxGeom(hingeTail_, MASS_SLOT, osg::Vec3(xTail, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
// Create joints to hold pieces in position
// root <-> connectionPartA
this->fixBodies(hingeRoot_, frame, osg::Vec3(1, 0, 0));
// connectionPartA <(hinge)> connectionPArtB
dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint, frame, servo);
dJointSetHingeAxis(joint, 0, 1, 0);
dJointSetHingeAnchor(joint,
SLOT_THICKNESS / 2 + separation + FRAME_ROTATION_OFFSET, 0, 0);
// connectionPartB <-> tail
this->fixBodies(servo, hingeTail_, osg::Vec3(1, 0, 0));
// Create servo
this->motor_.reset(
new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
ServoMotor::DEFAULT_GAIN,
ioPair(this->getId(),0)));
return true;
}
/**
*@brief 位置設定の関数
* @param x 位置(X)
* @param y 位置(Y)
* @param z 位置(Z)
*/
void ODEJointObj::SetJointPosition(double px, double py, double pz)
{
ms->mu->lock();
if(JointType == 2)
{
dJointSetHingeAnchor(joint, px, py, pz);
SetPosition(bscale_x*px + offx, bscale_y*py + offy, bscale_z*pz + offz);
jx = px;
jy = py;
jz = pz;
}
ms->mu->unlock();
}
void CODEHingeJoint::setParams()
{
double anchorPos[3];
mpBodyAnchors[0]->getPosition(anchorPos);
dJointSetHingeAnchor(mID, anchorPos[0], anchorPos[1], anchorPos[2]);
dJointSetHingeAxis(mID, mAxisX, mAxisY, mAxisZ);
dJointSetHingeParam(mID, dParamLoStop, mLowerLimit);
dJointSetHingeParam(mID, dParamHiStop, mUpperLimit);
if (mStopERP >= 0)
dJointSetHingeParam(mID, dParamStopERP, mStopERP);
if (mStopCFM >= 0)
dJointSetHingeParam(mID, dParamStopCFM, mStopCFM);
}
void JOINT::Create_In_Simulator(dWorldID world, OBJECT *firstObject, OBJECT *secondObject) {
joint = dJointCreateHinge(world,0);
dJointAttach( joint , firstObject->Get_Body() , secondObject->Get_Body() );
dJointSetHingeAnchor(joint,x,y,z);
dJointSetHingeAxis(joint,normalX,normalY,normalZ);
dJointSetHingeParam(joint,dParamLoStop,lowStop);
dJointSetHingeParam(joint,dParamHiStop,highStop);
}
AppHalf::AppHalf(dWorldID world, dSpaceID space, AppDesignID app_design,
dBodyID main_body, dBodyID blade)
: ebMain(main_body), ebBlade(blade)
{
// NOTE: app_design must have a side called before this, so that A..E and K..Q are one of left or right
// create bodies with geometries
// create links and connect them
jA = dJointCreateHinge(world, 0);
dJointSetHingeAxis(jA, 0, 1, 0); // is -1 or 1 a matter? only for angle and anglerate sign, i think.
dReal *A = app_design->getA();
dJointSetHingeAnchor(jA, A[0], A[1], A[2]);
dJointConnect(jA, ebMain, bBeam);
}
static void set_phys_joint_anchor(dJointID j, const float v[3])
{
switch (dJointGetType(j))
{
case dJointTypeBall:
dJointSetBallAnchor (j, v[0], v[1], v[2]); break;
case dJointTypeHinge:
dJointSetHingeAnchor (j, v[0], v[1], v[2]); break;
case dJointTypeHinge2:
dJointSetHinge2Anchor (j, v[0], v[1], v[2]); break;
case dJointTypeUniversal:
dJointSetUniversalAnchor(j, v[0], v[1], v[2]); break;
default: break;
}
}
static void soy_joints_hinge_real_setup (soyjointsJoint* base, soyatomsPosition* anchor, soyatomsAxis* axis1, soyatomsAxis* axis2) {
soyjointsHinge * self;
struct dxJoint* _tmp0_;
soyatomsPosition* _tmp1_;
gfloat _tmp2_;
gfloat _tmp3_;
soyatomsPosition* _tmp4_;
gfloat _tmp5_;
gfloat _tmp6_;
soyatomsPosition* _tmp7_;
gfloat _tmp8_;
gfloat _tmp9_;
struct dxJoint* _tmp10_;
soyatomsAxis* _tmp11_;
gfloat _tmp12_;
gfloat _tmp13_;
soyatomsAxis* _tmp14_;
gfloat _tmp15_;
gfloat _tmp16_;
soyatomsAxis* _tmp17_;
gfloat _tmp18_;
gfloat _tmp19_;
self = (soyjointsHinge*) base;
_tmp0_ = ((soyjointsJoint*) self)->joint;
_tmp1_ = anchor;
_tmp2_ = soy_atoms_position_get_x (_tmp1_);
_tmp3_ = _tmp2_;
_tmp4_ = anchor;
_tmp5_ = soy_atoms_position_get_y (_tmp4_);
_tmp6_ = _tmp5_;
_tmp7_ = anchor;
_tmp8_ = soy_atoms_position_get_z (_tmp7_);
_tmp9_ = _tmp8_;
dJointSetHingeAnchor ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_);
_tmp10_ = ((soyjointsJoint*) self)->joint;
_tmp11_ = axis1;
_tmp12_ = soy_atoms_axis_get_x (_tmp11_);
_tmp13_ = _tmp12_;
_tmp14_ = axis1;
_tmp15_ = soy_atoms_axis_get_y (_tmp14_);
_tmp16_ = _tmp15_;
_tmp17_ = axis1;
_tmp18_ = soy_atoms_axis_get_z (_tmp17_);
_tmp19_ = _tmp18_;
dJointSetHingeAxis ((struct dxJoint*) _tmp10_, (dReal) _tmp13_, (dReal) _tmp16_, (dReal) _tmp19_);
}
/*** ロボットアームの生成 ***/
void makeArm()
{
dMass mass; // 質量パラメータ
dReal x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 x
dReal y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 重心 y
dReal z[NUM] = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0}; // 重心 z
dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50}; // 長さ
dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50}; // 質量
dReal r[NUM-1] = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; // 半径
dReal c_x[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 x
dReal c_y[NUM] = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00}; // 関節中心点 y
dReal c_z[NUM] = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9}; // 関節中心点 z
dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0}; // 関節回転軸 x
dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0}; // 関節回転軸 y
dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1}; // 関節回転軸 z
// リンクの生成
for (int i = 0; i < NUM-1; i++) {
rlink[i].body = dBodyCreate(world);
dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
dBodySetMass(rlink[i].body, &mass);
rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
dGeomSetBody(rlink[i].geom,rlink[i].body);
}
rlink[NUM-1].body = dBodyCreate(world);
dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
dBodySetMass(rlink[NUM-1].body, &mass);
rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);
// ジョイントの生成とリンクへの取り付け
joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント
dJointAttach(joint[0], rlink[0].body, 0);
dJointSetFixed(joint[0]);
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
}
}
/**
* Creates a new ODE_HingeJoint.
*
* @param body1 the first body to connect the joint to.
* @param body2 the second body to connect the joint to.
* @return the new ODE_HingeJoint.
*/
dJointID ODE_HingeJoint::createJoint(dBodyID body1, dBodyID body2) {
if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get())) {
Core::log("Invalid axes for ODE_HingeJoint.");
return 0;
}
//if one of the bodyIDs is null, the joint is connected to a static object.
dJointID newJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
dJointAttach(newJoint, body1, body2);
Vector3D anchor = mJointAxisPoint1->get() ;
Vector3D axis = mJointAxisPoint2->get() ;
axis.set(mJointAxisPoint2->getX() - mJointAxisPoint1->getX(), mJointAxisPoint2->getY() - mJointAxisPoint1->getY(), mJointAxisPoint2->getZ() - mJointAxisPoint1->getZ());
dJointSetHingeAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetHingeAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
return newJoint;
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
// create world
dInitODE2(0);
world = dWorldCreate();
dMass m;
dMassSetBox (&m,1,SIDE,SIDE,SIDE);
dMassAdjust (&m,MASS);
dQuaternion q;
dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);
body[0] = dBodyCreate (world);
dBodySetMass (body[0],&m);
dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
dBodySetQuaternion (body[0],q);
body[1] = dBodyCreate (world);
dBodySetMass (body[1],&m);
dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
dBodySetQuaternion (body[1],q);
hinge = dJointCreateHinge (world,0);
dJointAttach (hinge,body[0],body[1]);
dJointSetHingeAnchor (hinge,0,0,1);
dJointSetHingeAxis (hinge,1,-1,1.41421356);
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
sODEJoint* sODEJoint::AddHingeJoint( int id, dBodyID body1, dBodyID body2, float ax, float ay, float az, float x, float y, float z )
{
sODEJoint *pJoint = new sODEJoint( );
//modify prev and next pointers of any involved joints, make new joint the new head
pJoint->pNextJoint = pODEJointList;
if ( pODEJointList ) pODEJointList->pPrevJoint = pJoint;
pODEJointList = pJoint;
//create the ODE hinge joint
pJoint->oJoint = dJointCreateHinge( g_ODEWorld, 0 );
dJointAttach( pJoint->oJoint, body1, body2 );
dJointSetHingeAnchor( pJoint->oJoint, x,y,z );
dJointSetHingeAxis( pJoint->oJoint, ax,ay,az );
pJoint->iID = id;
return pJoint;
}
static void _soy_joints_hinge_anchor_set (soyjointsHinge* self, soyatomsPosition* anchor) {
struct dxJoint* _tmp0_ = NULL;
soyatomsPosition* _tmp1_ = NULL;
gfloat _tmp2_ = 0.0F;
gfloat _tmp3_ = 0.0F;
soyatomsPosition* _tmp4_ = NULL;
gfloat _tmp5_ = 0.0F;
gfloat _tmp6_ = 0.0F;
soyatomsPosition* _tmp7_ = NULL;
gfloat _tmp8_ = 0.0F;
gfloat _tmp9_ = 0.0F;
#line 51 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_return_if_fail (self != NULL);
#line 51 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_return_if_fail (anchor != NULL);
#line 52 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_rw_lock_writer_lock (&soy_scenes__stepLock);
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp0_ = ((soyjointsJoint*) self)->joint;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp1_ = anchor;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp2_ = soy_atoms_position_get_x (_tmp1_);
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp3_ = _tmp2_;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp4_ = anchor;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp5_ = soy_atoms_position_get_y (_tmp4_);
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp6_ = _tmp5_;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp7_ = anchor;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp8_ = soy_atoms_position_get_z (_tmp7_);
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
_tmp9_ = _tmp8_;
#line 53 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
dJointSetHingeAnchor ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_);
#line 56 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
g_rw_lock_writer_unlock (&soy_scenes__stepLock);
#line 379 "Hinge.c"
}
void createHingeLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID &joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius, dReal length,
dReal maxAngle, dReal minAngle,
dReal anchorXPos, dReal anchorYPos, dReal anchorZPos)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
//dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//hinge joint
joint = dJointCreateHinge(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetHingeAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetHingeAxis(joint, 0, 1, 0);
//Max and min angles
dJointSetHingeParam(joint, dParamHiStop, maxAngle);
dJointSetHingeParam(joint, dParamLoStop, minAngle);
}
void SkidSteeringVehicle::create() {
this->vehicleBody = dBodyCreate(this->environment->world);
this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
dBodySetMass(this->vehicleBody, &this->vehicleMass);
dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
dGeomSetBody(this->vehicleGeom, this->vehicleBody);
dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);
dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
dMatrix3 wheelR;
dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
dBodySetRotation(this->wheelBody[fr][lr], wheelR);
this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
}
}
this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
}
}
}
int main(int argc, char *argv[]) {
dsFunctions fn;
double x[NUM] = {0.00}, y[NUM] = {0.00}; // Center of gravity
double z[NUM] = { 0.05, 0.50, 1.50, 2.55};
double m[NUM] = {10.00, 2.00, 2.00, 2.00}; // mass
double anchor_x[NUM] = {0.00}, anchor_y[NUM] = {0.00};// anchors of joints
double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00};
double axis_x[NUM] = { 0.00, 0.00, 0.00, 0.00}; // axises of joints
double axis_y[NUM] = { 0.00, 0.00, 1.00, 1.00};
double axis_z[NUM] = { 1.00, 1.00, 0.00, 0.00};
fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop;
fn.command = &command;
fn.path_to_textures = "../../drawstuff/textures";
dInitODE(); // Initialize ODE
world = dWorldCreate(); // Create a world
dWorldSetGravity(world, 0, 0, -9.8);
for (int i = 0; i < NUM; i++) {
dMass mass;
link[i] = dBodyCreate(world);
dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position
dMassSetZero(&mass); // Set mass parameter to zero
dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]); // Calculate mass parameter
dBodySetMass(link[i], &mass); // Set mass
}
joint[0] = dJointCreateFixed(world, 0); // A fixed joint
dJointAttach(joint[0], link[0], 0); // Attach the joint between the ground and the base
dJointSetFixed(joint[0]); // Set the fixed joint
for (int j = 1; j < NUM; j++) {
joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint
dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint
dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]);
dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]);
}
dsSimulationLoop(argc, argv, 640, 570, &fn); // Simulation loop
dCloseODE();
return 0;
}
void createLeg( const dWorldID & _world ){
// read leg parameters there
lower_part.mass = 0.5;
lower_part.radius = 0.1;
lower_part.length = 0.5;
dReal lower_part_pos[3] = {0.0, 0.0, lower_part.length};
upper_part.mass = 0.5;
upper_part.radius = 0.1;
upper_part.length = 0.5;
dReal upper_part_pos[3] = {0.0, 0.0, upper_part.length + lower_part.length};
createPart(_world,lower_part, lower_part_pos);
createPart(_world,upper_part, upper_part_pos);
jointHinge = dJointCreateHinge(_world, 0); // Create a hinge joint
dJointAttach(jointHinge, lower_part.body, upper_part.body ); // Attach the joint
dJointSetHingeAnchor(jointHinge, 1.0,0.0,0.0);//anchor_x[j], anchor_y[j],anchor_z[j]);
}
bool ActiveWheelModel::initModel() {
// Create the 4 components of the hinge
wheelRoot_ = this->createBody(B_SLOT_ID);
dBodyID servo = this->createBody(B_SERVO_ID);
dBodyID wheel = this->createBody(B_WHEEL_ID);
// Set the masses for the various boxes
dMass mass;
this->createBoxGeom(wheelRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
dReal zServo = 0;//-SLOT_WIDTH / 2 + SERVO_Z_OFFSET + SERVO_HEIGHT / 2;
this->createBoxGeom(servo, MASS_SERVO, osg::Vec3(X_SERVO, 0, zServo),
SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT);
this->createCylinderGeom(wheel, MASS_WHEEL, osg::Vec3(X_WHEEL, 0, 0), 1,
getRadius(), WHEEL_THICKNESS);
// Create joints to hold pieces in position
// slot <slider> hinge
this->fixBodies(wheelRoot_, servo, osg::Vec3(1, 0, 0));
// servo <(hinge)> wheel
dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint, servo, wheel);
dJointSetHingeAxis(joint, 1, 0, 0);
dJointSetHingeAnchor(joint, X_WHEEL, 0, 0);
// Create servo
this->motor_.reset(
new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE,
ioPair(this->getId(),0)));
return true;
}
Else::AcrobotArticulatedBody
::AcrobotArticulatedBody( dWorldID argWorldID,
dSpaceID argSpaceID,
double argScale )
: ArticulatedBody( argWorldID, argSpaceID ),
myRadius( 0.1 * argScale ),
myLength( argScale ),
myJointGroupID( 0 )
{
// create body
gmtl::Vec3d dims( 2*myRadius, 2*myRadius, 2*myRadius + myLength );
const double density = 1.0;
const int zDirection = 3;
const double extraSpace = 0.1*myLength;
dMass mass;
dBodyID body = dBodyCreate( myWorldID );
dBodySetData( body, this );
// create cylinder aligned to z-axis
dGeomID geom = dCreateCapsule( mySpaceID, myRadius, myLength );
dGeomSetBody( geom, body );
dBodySetPosition( body, 0, 0, extraSpace + 0.5*myLength + myRadius );
dMassSetCappedCylinder( &mass, density, zDirection, myRadius, myLength );
dBodySetMass( body, &mass );
dJointID joint = dJointCreateHinge( myWorldID, myJointGroupID );
dJointAttach( joint, NULL, body );
dJointSetHingeAnchor( joint, 0, 0, extraSpace + myLength + myRadius );
dVector3 axis;
axis[0] = 0; axis[1] = 1; axis[2] = 0;
dJointSetHingeAxis( joint, axis[0], axis[1], axis[2] );
myBodies[0] = body;
myJoints[0] = joint;
myBodyDims[0] = dims;
myGeoms.push_back( geom );
}
//! A hinge requires a fixed anchor point and an axis
OscHingeODE::OscHingeODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase* parent,
OscObject *object1, OscObject *object2,
double x, double y, double z, double ax, double ay, double az)
: OscHinge(name, parent, object1, object2, x, y, z, ax, ay, az)
{
m_response = new OscResponse("response",this);
// create the constraint for object1
cVector3d anchor(x,y,z);
cVector3d axis(ax,ay,az);
dJointID odeJoint = dJointCreateHinge(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetHingeAnchor(odeJoint, anchor.x, anchor.y, anchor.z);
dJointSetHingeAxis(odeJoint, axis.x, axis.y, axis.z);
printf("Hinge joint created between %s and %s at anchor (%f,%f,%f), axis (%f,%f,%f)\n",
object1->c_name(), object2?object2->c_name():"world", x,y,z,ax,ay,az);
}
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
// chassis pin joints
const dReal *pos;
snow->jChassisUp= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body);
dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0);
pos= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jChassisUp,pos[0]
,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]);
const dReal *pos1;
snow->jChassisLow= dJointCreateHinge(world,0);
dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body);
dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0);
pos1= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jChassisLow, pos1[0]
, pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]);
// rotor ball joint
/*const dReal *p2;
jRotorUp= dJointCreateBall(world,0);
dJointAttach(jRotorUp, uplink.body, hlink.body);
p2= dBodyGetPosition(uplink.body);
dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]);
const dReal *p3;
jRotorLow= dJointCreateBall(world,0);
dJointAttach(jRotorLow, lowlink.body,hlink.body);
p3= dBodyGetPosition(lowlink.body);
dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/
const dReal *p2;
snow->jRotorUp= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body);
p2= dBodyGetPosition(snow->uplink.body);
dJointSetHingeAnchor(snow->jRotorUp, p2[0]
,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]);
dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0);
const dReal *p3;
snow->jRotorLow= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body);
p3= dBodyGetPosition(snow->lowlink.body);
dJointSetHingeAnchor(snow->jRotorLow, p3[0]
,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]);
dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0);
// rotor hinge joint
const dReal *pw= dBodyGetPosition(wnow->body);
snow->jRotorMid= dJointCreateHinge(world,0);
dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body);
dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0);
dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]);
// strut joint
const dReal *ps1, *ps2;
dReal angle= -shiftSign*strutAngle;
ps1= dBodyGetPosition(snow->upstrut.body);
ps2= dBodyGetPosition(snow->lowstrut.body);
snow->jStrutUp= dJointCreateBall(world,0);
dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body);
//dJointSetFixed(snow->jStrutUp);
dJointSetBallAnchor(snow->jStrutUp, ps1[0]
,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle))
,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle)));
snow->jStrutLow= dJointCreateBall(world,0);
dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body);
dJointSetBallAnchor(snow->jStrutLow, ps2[0]
,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle))
,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle)));
// struct sliding joint
snow->jStrutMid= dJointCreateSlider(world,0);
dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body);
dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle)));
// set joint feedback
wnow->feedback= new dJointFeedback;
dJointSetFeedback(snow->jStrutMid,wnow->feedback);
// suspension slider
/*snow->jLowSpring= dJointCreateLMotor(world,0);
dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body);
dJointSetLMotorNumAxes(snow->jLowSpring, 1);
dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.0);*/
}
bool ActiveCardanModel::initModel() {
// Create the 4 components of the hinge
cardanRoot_ = this->createBody(B_SLOT_A_ID);
dBodyID connectionPartA = this->createBody(B_CONNECTION_A_ID);
dBodyID crossPartA = this->createBody(B_CROSS_PART_A_ID);
dBodyID crossPartB = this->createBody(B_CROSS_PART_B_ID);
dBodyID connectionPartB = this->createBody(B_CONNECTION_B_ID);
cardanTail_ = this->createBody(B_SLOT_B_ID);
float separation = inMm(0.1);
this->createBoxGeom(cardanRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
dReal xPartA = SLOT_THICKNESS / 2 + separation
+ CONNNECTION_PART_LENGTH / 2;
this->createBoxGeom(connectionPartA, MASS_SERVO, osg::Vec3(xPartA, 0, 0),
CONNNECTION_PART_LENGTH, CONNNECTION_PART_WIDTH,
CONNECTION_PART_HEIGHT);
dReal xCrossPartA = xPartA + CONNECTION_PART_OFFSET;
this->createBoxGeom(crossPartA, MASS_CROSS / 3,
osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_WIDTH,
CROSS_HEIGHT);
this->createBoxGeom(crossPartB, MASS_CROSS / 3,
osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_HEIGHT,
CROSS_WIDTH);
dReal xPartB = xCrossPartA + CONNECTION_PART_OFFSET;
this->createBoxGeom(connectionPartB, MASS_SERVO, osg::Vec3(xPartB, 0, 0),
CONNNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT,
CONNNECTION_PART_WIDTH);
dReal xTail = xPartB + CONNNECTION_PART_LENGTH / 2 + separation
+ SLOT_THICKNESS / 2;
this->createBoxGeom(cardanTail_, MASS_SLOT, osg::Vec3(xTail, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);
// Cross Geometries
dBodyID crossPartAedge1 = this->createBody(B_CROSS_PART_A_EDGE_1_ID);
dBodyID crossPartAedge2 = this->createBody(B_CROSS_PART_A_EDGE_2_ID);
dBodyID crossPartBedge1 = this->createBody(B_CROSS_PART_B_EDGE_1_ID);
dBodyID crossPartBedge2 = this->createBody(B_CROSS_PART_B_EDGE_2_ID);
this->createBoxGeom(crossPartAedge1, MASS_CROSS / 12,
osg::Vec3(xCrossPartA - (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2), 0,
CROSS_HEIGHT / 2 + (CROSS_THICKNESS / 2)),
CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS);
this->createBoxGeom(crossPartAedge2, MASS_CROSS / 12,
osg::Vec3(xCrossPartA - (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2), 0,
-CROSS_HEIGHT / 2 - (CROSS_THICKNESS / 2)),
CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS);
this->createBoxGeom(crossPartBedge1, MASS_CROSS / 12,
osg::Vec3(xCrossPartA + (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2),
CROSS_HEIGHT / 2 - (CROSS_THICKNESS / 2), 0),
CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH);
this->createBoxGeom(crossPartBedge2, MASS_CROSS / 12,
osg::Vec3(xCrossPartA + (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2),
-CROSS_HEIGHT / 2 + (CROSS_THICKNESS / 2), 0),
CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH);
// Create joints to hold pieces in position
// root <-> connectionPartA
this->fixBodies(cardanRoot_, connectionPartA, osg::Vec3(1, 0, 0));
// connectionPartA <(hinge)> crossPartA
dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint, connectionPartA, crossPartA);
dJointSetHingeAxis(joint, 0, 0, 1);
dJointSetHingeAnchor(joint,
xPartA
+ ((CONNNECTION_PART_LENGTH / 2)
- (CONNNECTION_PART_LENGTH
- CONNNECTION_PART_ROTATION_OFFSET)), 0, 0);
// crossPartA <-> crossPartB
this->fixBodies(crossPartA, crossPartB, osg::Vec3(1, 0, 0));
// crossPartB <(hinge)> connectionPartB
dJointID joint2 = dJointCreateHinge(this->getPhysicsWorld(), 0);
dJointAttach(joint2, crossPartB, connectionPartB);
dJointSetHingeAxis(joint2, 0, 1, 0);
dJointSetHingeAnchor(joint2,
xPartB
- ((CONNNECTION_PART_LENGTH / 2)
- (CONNNECTION_PART_LENGTH
- CONNNECTION_PART_ROTATION_OFFSET)), 0, 0);
// connectionPartB <-> tail
this->fixBodies(connectionPartB, cardanTail_, osg::Vec3(1, 0, 0));
// Fix cross Parts
this->fixBodies(crossPartA, crossPartAedge1, osg::Vec3(1, 0, 0));
this->fixBodies(crossPartA, crossPartAedge2, osg::Vec3(1, 0, 0));
this->fixBodies(crossPartB, crossPartBedge1, osg::Vec3(1, 0, 0));
this->fixBodies(crossPartB, crossPartBedge2, osg::Vec3(1, 0, 0));
// Create motors
motor1_.reset(
new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
ServoMotor::DEFAULT_GAIN,
ioPair(this->getId(),0)));
motor2_.reset(
new ServoMotor(joint2, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
ServoMotor::DEFAULT_GAIN,
ioPair(this->getId(),1)));
return true;
}
