namespace robogen {
const float LightSensorModel::MASS = inGrams(2);
const float LightSensorModel::SENSOR_BASE_WIDTH = inMm(34);
const float LightSensorModel::SENSOR_BASE_THICKNESS = inMm(1.5);
const float LightSensorModel::SENSOR_DISPLACEMENT = inMm(8);
LightSensorModel::LightSensorModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id, bool internalSensor) :
PerceptiveComponent(odeWorld, odeSpace, id),
internalSensor_(internalSensor) {
}
LightSensorModel::~LightSensorModel() {
}
bool LightSensorModel::initModel() {
// Create the 4 components of the hinge
sensorRoot_ = this->createBody(B_SENSOR_BASE_ID);
this->createBoxGeom(sensorRoot_, MASS, osg::Vec3(0, 0, 0),
SENSOR_BASE_THICKNESS, SENSOR_BASE_WIDTH, SENSOR_BASE_WIDTH);
this->sensor_.reset(new LightSensor(this->getCollisionSpace(),
this->getBodies(), this->getId()));
return true;
}
bool LightSensorModel::isInternal() {
return internalSensor_;
}
dBodyID LightSensorModel::getRoot() {
return sensorRoot_;
}
dBodyID LightSensorModel::getSlot(unsigned int /*i*/) {
return sensorRoot_;
}
osg::Vec3 LightSensorModel::getSlotPosition(unsigned int i) {
if (i > 1) {
std::cout << "[LightSensorModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Vec3 slotPos;
if (i == SLOT_A) {
osg::Vec3 curPos = this->getPosition(sensorRoot_);
osg::Vec3 slotAxis = this->getSlotAxis(i);
slotPos = curPos + slotAxis * (SENSOR_BASE_THICKNESS / 2);
}
return slotPos;
}
osg::Vec3 LightSensorModel::getSlotAxis(unsigned int i) {
if (i > 1) {
std::cout << "[LightSensorModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->getAttitude(this->sensorRoot_);
axis.set(-1, 0, 0);
}
return quat * axis;
}
osg::Vec3 LightSensorModel::getSlotOrientation(unsigned int i) {
if (i > 1) {
std::cout << "[LightSensorModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->getAttitude(this->sensorRoot_);
axis.set(0, 1, 0);
}
return quat * axis;
}
void LightSensorModel::getSensors(
std::vector<boost::shared_ptr<Sensor> >& sensors) {
sensors.resize(1);
sensors[0] = sensor_;
}
void LightSensorModel::updateSensors(boost::shared_ptr<Environment>& /*env*/) {
// Axis
osg::Quat quat = this->getAttitude(this->sensorRoot_);
osg::Vec3 curPos = this->getPosition(sensorRoot_);
osg::Vec3 axis(1, 0, 0);
osg::Vec3 sensorPos = curPos + quat * axis * SENSOR_DISPLACEMENT;
this->sensor_->update(sensorPos, this->getAttitude(sensorRoot_));
}
}
namespace robogen {
const float ActiveCardanModel::MASS_SERVO = inGrams(7);
const float ActiveCardanModel::MASS_SLOT = inGrams(2);
const float ActiveCardanModel::MASS_CROSS = inGrams(3);
const float ActiveCardanModel::SLOT_WIDTH = inMm(34);
const float ActiveCardanModel::SLOT_THICKNESS = inMm(1.5);
const float ActiveCardanModel::CONNNECTION_PART_WIDTH = inMm(12.5);
const float ActiveCardanModel::CONNNECTION_PART_LENGTH = inMm(24.5);
const float ActiveCardanModel::CONNECTION_PART_HEIGHT = inMm(21);
const float ActiveCardanModel::CROSS_WIDTH = inMm(10);
const float ActiveCardanModel::CROSS_HEIGHT = inMm(34);
const float ActiveCardanModel::CROSS_THICKNESS = inMm(3);
const float ActiveCardanModel::CONNNECTION_PART_ROTATION_OFFSET = inMm(20.5); // Left to right
// Offset of the cross center from the rotation axis
const float ActiveCardanModel::CROSS_CENTER_OFFSET = inMm(17);
// Offset of the cross center respect to the center of the connection part
const float ActiveCardanModel::CONNECTION_PART_OFFSET = inMm(19.875);
ActiveCardanModel::ActiveCardanModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id) :
ActuatedComponent(odeWorld, odeSpace, id) {
}
ActiveCardanModel::~ActiveCardanModel() {
}
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;
}
dBodyID ActiveCardanModel::getRoot() {
return cardanRoot_;
}
dBodyID ActiveCardanModel::getSlot(unsigned int i) {
if (i == SLOT_A) {
return cardanRoot_;
} else {
return cardanTail_;
}
}
osg::Vec3 ActiveCardanModel::getSlotPosition(unsigned int i) {
if (i > 2) {
std::cout << "[ActiveCardanModel] Invalid slot: " << i << std::endl;
assert(i <= 2);
}
osg::Vec3 slotPos;
if (i == SLOT_A) {
osg::Vec3 curPos = this->getPosition(cardanRoot_);
osg::Vec3 slotAxis = this->getSlotAxis(i);
slotPos = curPos + slotAxis * (SLOT_THICKNESS / 2);
} else {
osg::Vec3 curPos = this->getPosition(cardanTail_);
osg::Vec3 slotAxis = this->getSlotAxis(i);
slotPos = curPos + slotAxis * (SLOT_THICKNESS / 2);
}
return slotPos;
}
osg::Vec3 ActiveCardanModel::getSlotAxis(unsigned int i) {
if (i > 2) {
std::cout << "[ActiveCardanModel] Invalid slot: " << i << std::endl;
assert(i <= 2);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->getAttitude(this->cardanRoot_);
axis.set(-1, 0, 0);
} else if (i == SLOT_B) {
quat = this->getAttitude(this->cardanTail_);
axis.set(1, 0, 0);
}
return quat * axis;
}
osg::Vec3 ActiveCardanModel::getSlotOrientation(unsigned int i) {
if (i > 2) {
std::cout << "[ActiveCardanModel] Invalid slot: " << i << std::endl;
assert(i <= 2);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->getAttitude(this->cardanRoot_);
axis.set(0, 1, 0);
} else if (i == SLOT_B) {
quat = this->getAttitude(this->cardanTail_);
axis.set(0, 1, 0);
}
return quat * axis;
}
void ActiveCardanModel::getMotors(
std::vector<boost::shared_ptr<Motor> >& motors) {
motors.resize(2);
motors[0] = motor1_;
motors[1] = motor2_;
}
}
namespace robogen {
const float PassiveWheelModel::MASS_SLOT = inGrams(2);
const float PassiveWheelModel::MASS_AXEL = inGrams(2); //TODO verify
const float PassiveWheelModel::MASS_WHEEL = inGrams(4);
const float PassiveWheelModel::SLOT_WIDTH = inMm(34);
const float PassiveWheelModel::SLOT_THICKNESS = inMm(1.5);
const float PassiveWheelModel::WHEEL_THICKNESS = inMm(3);
const float PassiveWheelModel::WHEEL_AXEL_OFFSET = inMm(4.625);
const float PassiveWheelModel::AXEL_RADIUS = inMm(5);
const float PassiveWheelModel::AXEL_LENGTH = inMm(9.25);
PassiveWheelModel::PassiveWheelModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id, float radius) :
Model(odeWorld, odeSpace, id), radius_(radius) {
}
PassiveWheelModel::~PassiveWheelModel() {
}
float PassiveWheelModel::getRadius() const {
return radius_;
}
bool PassiveWheelModel::initModel() {
// Create the 2 components of the wheel,
// now created directly with calls to this->add___
wheelRoot_ = this->addBox(MASS_SLOT, osg::Vec3(0, 0, 0),
SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, B_SLOT_ID);
dReal xAxel = SLOT_THICKNESS / 2 + AXEL_LENGTH / 2;
boost::shared_ptr<SimpleBody> axel = this->addCylinder(MASS_AXEL,
osg::Vec3(xAxel, 0, 0), 1,
AXEL_RADIUS, AXEL_LENGTH, B_AXEL_ID);
dReal xWheel = SLOT_THICKNESS/2 + WHEEL_AXEL_OFFSET;;
boost::shared_ptr<SimpleBody> wheel = this->addCylinder(MASS_WHEEL,
osg::Vec3(xWheel, 0, 0), 1,
getRadius(), WHEEL_THICKNESS, B_WHEEL_ID);
// Create joints to hold pieces in position
this->fixBodies(wheelRoot_, axel);
this->attachWithHinge(axel, wheel, osg::Vec3( 1, 0, 0),
osg::Vec3(xWheel, 0, 0) );
return true;
}
boost::shared_ptr<SimpleBody> PassiveWheelModel::getRoot() {
return wheelRoot_;
}
boost::shared_ptr<SimpleBody> PassiveWheelModel::getSlot(unsigned int i) {
if (i > 1) {
std::cout << "[PassiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
return wheelRoot_;
}
osg::Vec3 PassiveWheelModel::getSlotPosition(unsigned int i) {
if (i > 1) {
std::cout << "[PassiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Vec3 curPos = this->wheelRoot_->getPosition();
osg::Vec3 slotAxis = this->getSlotAxis(i);
return curPos + slotAxis * (SLOT_THICKNESS / 2);
}
osg::Vec3 PassiveWheelModel::getSlotAxis(unsigned int i) {
if (i > 1) {
std::cout << "[PassiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat = this->wheelRoot_->getAttitude();
osg::Vec3 axis(-1, 0, 0);
return quat * axis;
}
osg::Vec3 PassiveWheelModel::getSlotOrientation(unsigned int i) {
if (i > 1) {
std::cout << "[PassiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat = this->wheelRoot_->getAttitude();
osg::Vec3 axis(0, 1, 0);
return quat * axis;
}
}
namespace robogen {
// mass of just the brick
const float CoreComponentModel::BRICK_MASS = inGrams(10.2);//inGrams(14.9);
// mass of brick with electronics (including battery)
const float CoreComponentModel::CORE_MASS = inGrams(10.2 + 34.3);//inGrams(14.9 + 40.5);
const float CoreComponentModel::HEIGHT = inMm(35.5);
const float CoreComponentModel::WIDTH = inMm(41);//inMm(46.5);
const float CoreComponentModel::SLOT_THICKNESS = inMm(1.5);
CoreComponentModel::CoreComponentModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id, bool hasSensors) :
PerceptiveComponent(odeWorld, odeSpace, id), hasSensors_(hasSensors) {
if (hasSensors) {
sensor_.reset(new ImuSensor());
}
}
CoreComponentModel::~CoreComponentModel() {
}
bool CoreComponentModel::initModel() {
coreComponent_ = this->addBox(hasSensors_ ? CORE_MASS : BRICK_MASS,
osg::Vec3(0, 0, 0), WIDTH, WIDTH,
HEIGHT, B_CORE_COMPONENT_ID);
return true;
}
boost::shared_ptr<SimpleBody> CoreComponentModel::getRoot() {
return coreComponent_;
}
boost::shared_ptr<SimpleBody> CoreComponentModel::getSlot(unsigned int /*i*/) {
return coreComponent_;
}
osg::Vec3 CoreComponentModel::getSlotPosition(unsigned int i) {
if (i >= NUM_SLOTS) {
std::cout << "[CoreComponentModel] Invalid slot: " << i << std::endl;
assert(i < NUM_SLOTS);
}
osg::Vec3 curPos = this->getRootPosition();
// want the slot to be the coordinates specifying the edge of where the
// adjoining part touches
osg::Vec3 slotAxis = this->getSlotAxis(i) *
(WIDTH / 2 - SLOT_THICKNESS);
curPos = curPos + slotAxis;
return curPos;
}
osg::Vec3 CoreComponentModel::getSlotAxis(unsigned int i) {
if (i >= NUM_SLOTS) {
std::cout << "[CoreComponentModel] Invalid slot: " << i << std::endl;
assert(i < NUM_SLOTS);
}
osg::Quat quat = this->getRootAttitude();
osg::Vec3 axis;
if (i == LEFT_FACE_SLOT) {
// Left face
axis.set(-1, 0, 0);
} else if (i == RIGHT_FACE_SLOT) {
// Right face
axis.set(1, 0, 0);
} else if (i == FRONT_FACE_SLOT) {
// Front face
axis.set(0, -1, 0);
} else if (i == BACK_FACE_SLOT) {
// Back face
axis.set(0, 1, 0);
}
#ifndef ENFORCE_PLANAR
else if (i == TOP_FACE_SLOT) {
// Top face
axis.set(0, 0, 1);
} else if (i == BOTTOM_FACE_SLOT) {
// Bottom face
axis.set(0, 0, -1);
}
#endif
return quat * axis;
}
osg::Vec3 CoreComponentModel::getSlotOrientation(unsigned int i) {
if (i >= NUM_SLOTS) {
std::cout << "[CoreComponentModel] Invalid slot: " << i << std::endl;
assert(i < NUM_SLOTS);
}
osg::Quat quat = this->getRootAttitude();
osg::Vec3 tangent;
if (i == LEFT_FACE_SLOT) {
// Left face
tangent.set(0, 1, 0);
} else if (i == RIGHT_FACE_SLOT) {
// Right face
tangent.set(0, 1, 0);
} else if (i == FRONT_FACE_SLOT) {
// Front face
tangent.set(0, 0, 1);
} else if (i == BACK_FACE_SLOT) {
// Back face
tangent.set(0, 0, 1);
}
#ifndef ENFORCE_PLANAR
else if (i == TOP_FACE_SLOT) {
// Top face
tangent.set(1, 0, 0);
} else if (i == BOTTOM_FACE_SLOT) {
// Bottom face
tangent.set(1, 0, 0);
}
#endif
return quat * tangent;
}
void CoreComponentModel::getSensors(
std::vector<boost::shared_ptr<Sensor> >& sensors) {
if (sensor_ != NULL) {
sensor_->getSensors(sensors);
}
}
void CoreComponentModel::updateSensors(boost::shared_ptr<Environment>& env) {
if (sensor_ != NULL) {
dVector3 gravity;
dWorldGetGravity(getPhysicsWorld(), gravity);
sensor_->update(this->getRootPosition(), this->getRootAttitude(),
env->getTimeElapsed(),
osg::Vec3(gravity[0], gravity[1], gravity[2]));
}
}
}
namespace robogen {
const float ActiveHingeModel::MASS_SERVO = inGrams(4 + 6);//inGrams(9);
const float ActiveHingeModel::MASS_SLOT = inGrams(2);//inGrams(7);
const float ActiveHingeModel::MASS_FRAME = inGrams(1);////inGrams(1.2);
const float ActiveHingeModel::SLOT_WIDTH = inMm(34);
const float ActiveHingeModel::SLOT_THICKNESS = inMm(1.5);
const float ActiveHingeModel::FRAME_LENGTH = inMm(20.5);//inMm(18);
const float ActiveHingeModel::FRAME_HEIGHT = inMm(13.0);//inMm(10);
const float ActiveHingeModel::FRAME_WIDTH = inMm(36);
const float ActiveHingeModel::FRAME_ROTATION_OFFSET = inMm(14); // Left to right
const float ActiveHingeModel::SERVO_LENGTH = inMm(27.5);//inMm(32);//inMm(24.5);
const float ActiveHingeModel::SERVO_HEIGHT = inMm(16.25);//inMm(10);
const float ActiveHingeModel::SERVO_WIDTH = inMm(36.5);
const float ActiveHingeModel::SERVO_ROTATION_OFFSET = inMm(21.5);//inMm(20.5); // Right to left
const float ActiveHingeModel::SERVO_POSITION_OFFSET = inMm(0.5);
ActiveHingeModel::ActiveHingeModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id) :
ActuatedComponent(odeWorld, odeSpace, id) {
}
ActiveHingeModel::~ActiveHingeModel() {
}
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;
}
dBodyID ActiveHingeModel::getRoot() {
return hingeRoot_;
}
dBodyID ActiveHingeModel::getSlot(unsigned int i) {
if (i == SLOT_A) {
return hingeRoot_;
} else {
return hingeTail_;
}
}
osg::Vec3 ActiveHingeModel::getSlotPosition(unsigned int i) {
if (i > 2) {
std::cout << "[HingeModel] Invalid slot: " << i << std::endl;
assert(i <= 2);
}
osg::Vec3 slotPos;
if (i == SLOT_A) {
osg::Vec3 curPos = this->getPosition(hingeRoot_);
osg::Vec3 slotAxis = this->getSlotAxis(i);
slotPos = curPos - slotAxis * (SLOT_THICKNESS / 2);
} else {
osg::Vec3 curPos = this->getPosition(hingeTail_);
osg::Vec3 slotAxis = this->getSlotAxis(i);
slotPos = curPos - slotAxis * (SLOT_THICKNESS / 2);
}
return slotPos;
}
osg::Vec3 ActiveHingeModel::getSlotAxis(unsigned int i) {
if (i > 2) {
std::cout << "[HingeModel] Invalid slot: " << i << std::endl;
assert(i <= 2);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->getAttitude(this->hingeRoot_);
axis.set(-1, 0, 0);
} else if (i == SLOT_B) {
quat = this->getAttitude(this->hingeTail_);
axis.set(1, 0, 0);
}
return quat * axis;
}
osg::Vec3 ActiveHingeModel::getSlotOrientation(unsigned int i) {
if (i > 2) {
std::cout << "[HingeModel] Invalid slot: " << i << std::endl;
assert(i <= 2);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->getAttitude(this->hingeRoot_);
axis.set(0, 1, 0);
} else if (i == SLOT_B) {
quat = this->getAttitude(this->hingeTail_);
axis.set(0, 1, 0);
}
return quat * axis;
}
void ActiveHingeModel::getMotors(
std::vector<boost::shared_ptr<Motor> >& motors) {
motors.resize(1);
motors[0] = this->motor_;
}
}
namespace robogen {
const float ActiveWheelModel::MASS_SLOT = inGrams(4);
const float ActiveWheelModel::MASS_SERVO = inGrams(9);
const float ActiveWheelModel::MASS_WHEEL = inGrams(5);
const float ActiveWheelModel::SLOT_WIDTH = inMm(34);
const float ActiveWheelModel::SLOT_THICKNESS = inMm(1.5);
const float ActiveWheelModel::SERVO_Z_OFFSET = inMm(0); // zCenter shift respect to slot z-center
const float ActiveWheelModel::SERVO_WIDTH = inMm(14);
//take off wheel thickness, because in reality overlap
const float ActiveWheelModel::SERVO_LENGTH = inMm(36.8) -
ActiveWheelModel::WHEEL_THICKNESS;
const float ActiveWheelModel::SERVO_HEIGHT = inMm(14);
const float ActiveWheelModel::WHEEL_THICKNESS = inMm(3);
const float ActiveWheelModel::SEPARATION = inMm(1.0);
const float ActiveWheelModel::X_SERVO = -ActiveWheelModel::SLOT_THICKNESS +
ActiveWheelModel::SEPARATION + ActiveWheelModel::SERVO_LENGTH / 2;
const float ActiveWheelModel::X_WHEEL = ActiveWheelModel::X_SERVO +
ActiveWheelModel::SERVO_LENGTH / 2;
ActiveWheelModel::ActiveWheelModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id, float radius) :
ActuatedComponent(odeWorld, odeSpace, id), radius_(radius) {
}
ActiveWheelModel::~ActiveWheelModel() {
}
float ActiveWheelModel::getRadius() const {
return radius_;
}
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;
}
dBodyID ActiveWheelModel::getRoot() {
return wheelRoot_;
}
dBodyID ActiveWheelModel::getSlot(unsigned int i) {
if (i > 1) {
std::cout << "[ActiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
return wheelRoot_;
}
osg::Vec3 ActiveWheelModel::getSlotPosition(unsigned int i) {
if (i > 1) {
std::cout << "[ActiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Vec3 curPos = this->getPosition(wheelRoot_);
osg::Vec3 slotAxis = this->getSlotAxis(i);
return curPos + slotAxis * (SLOT_THICKNESS / 2);
}
osg::Vec3 ActiveWheelModel::getSlotAxis(unsigned int i) {
if (i > 1) {
std::cout << "[ActiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat = this->getAttitude(this->wheelRoot_);
osg::Vec3 axis(-1, 0, 0);
return quat * axis;
}
osg::Vec3 ActiveWheelModel::getSlotOrientation(unsigned int i) {
if (i > 1) {
std::cout << "[ActiveWheelModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat = this->getAttitude(this->wheelRoot_);
osg::Vec3 axis(0, 1, 0);
return quat * axis;
}
void ActiveWheelModel::getMotors(
std::vector<boost::shared_ptr<Motor> >& motors) {
motors.resize(1);
motors[0] = this->motor_;
}
}
namespace robogen {
const float LightSensorModel::MASS = inGrams(2);
const float LightSensorModel::SENSOR_BASE_WIDTH = inMm(34);
const float LightSensorModel::SENSOR_BASE_THICKNESS = inMm(1.5);
const float LightSensorModel::SENSOR_PLATFORM_THICKNESS = inMm(2);
const float LightSensorModel::SENSOR_PLATFORM_WIDTH = inMm(16);
const float LightSensorModel::SENSOR_CYLINDER_HEIGHT = inMm(6);
const float LightSensorModel::SENSOR_CYLINDER_RADIUS = inMm(4);
const float LightSensorModel::SENSOR_DISPLACEMENT = inMm(6);
LightSensorModel::LightSensorModel(dWorldID odeWorld, dSpaceID odeSpace,
std::string id, bool internalSensor) :
PerceptiveComponent(odeWorld, odeSpace, id),
internalSensor_(internalSensor) {
}
LightSensorModel::~LightSensorModel() {
}
bool LightSensorModel::initModel() {
//just dividing the mass by 3 for each component, is there a better way to do this?
sensorRoot_ = this->addBox(MASS/3.0, osg::Vec3(0, 0, 0),
SENSOR_BASE_THICKNESS, SENSOR_BASE_WIDTH, SENSOR_BASE_WIDTH,
B_SENSOR_BASE_ID);
boost::shared_ptr<SimpleBody> platform = this->addBox(MASS/3.0,
osg::Vec3(SENSOR_BASE_THICKNESS/2.0 +
SENSOR_PLATFORM_THICKNESS/2.0, 0, 0),
SENSOR_PLATFORM_THICKNESS, SENSOR_PLATFORM_WIDTH,
SENSOR_PLATFORM_WIDTH, B_SENSOR_PLATFORM_ID);
this->fixBodies(sensorRoot_, platform);
boost::shared_ptr<SimpleBody> cylinder = this->addCylinder(MASS/3.0,
osg::Vec3(SENSOR_BASE_THICKNESS/2.0 + SENSOR_PLATFORM_THICKNESS +
SENSOR_CYLINDER_HEIGHT/2.0, 0, 0), 1,
SENSOR_CYLINDER_RADIUS, SENSOR_CYLINDER_HEIGHT,
B_SENSOR_CYLINDER_ID);
this->fixBodies(platform, cylinder);
this->sensor_.reset(new LightSensor(this->getCollisionSpace(),
this->getBodies(), this->getId()));
return true;
}
bool LightSensorModel::isInternal() {
return internalSensor_;
}
boost::shared_ptr<SimpleBody> LightSensorModel::getRoot() {
return sensorRoot_;
}
boost::shared_ptr<SimpleBody> LightSensorModel::getSlot(unsigned int /*i*/) {
return sensorRoot_;
}
osg::Vec3 LightSensorModel::getSlotPosition(unsigned int i) {
if (i > 1) {
std::cout << "[LightSensorModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Vec3 slotPos;
if (i == SLOT_A) {
osg::Vec3 curPos = this->sensorRoot_->getPosition();
osg::Vec3 slotAxis = this->getSlotAxis(i);
slotPos = curPos + slotAxis * (SENSOR_BASE_THICKNESS / 2);
}
return slotPos;
}
osg::Vec3 LightSensorModel::getSlotAxis(unsigned int i) {
if (i > 1) {
std::cout << "[LightSensorModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->sensorRoot_->getAttitude();
axis.set(-1, 0, 0);
}
return quat * axis;
}
osg::Vec3 LightSensorModel::getSlotOrientation(unsigned int i) {
if (i > 1) {
std::cout << "[LightSensorModel] Invalid slot: " << i << std::endl;
assert(i <= 1);
}
osg::Quat quat;
osg::Vec3 axis;
if (i == SLOT_A) {
quat = this->sensorRoot_->getAttitude();
axis.set(0, 1, 0);
}
return quat * axis;
}
void LightSensorModel::getSensors(
std::vector<boost::shared_ptr<Sensor> >& sensors) {
sensors.resize(1);
sensors[0] = sensor_;
}
void LightSensorModel::updateSensors(boost::shared_ptr<Environment>& env) {
// Axis
osg::Quat quat = this->sensorRoot_->getAttitude();
osg::Vec3 curPos = this->sensorRoot_->getPosition();
osg::Vec3 axis(1, 0, 0);
osg::Vec3 sensorPos = curPos + quat * axis * SENSOR_DISPLACEMENT;
this->sensor_->update(sensorPos, this->sensorRoot_->getAttitude(), env);
}
}
