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_;
}

}
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;

}
Exemplo n.º 3
0
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;

}

}
Exemplo n.º 4
0
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_));
}

}
Exemplo n.º 5
0
bool ActiveCardanModel::initModel() {

	// Create the 6 components of the model
	// now created directly with calls to this->add___

	float separation = inMm(0.1);

	cardanRoot_ = this->addBox(MASS_SLOT, osg::Vec3(0, 0, 0),
			SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, B_SLOT_A_ID);

	dReal xPartA = SLOT_THICKNESS / 2 + separation
			+ CONNNECTION_PART_LENGTH / 2;
	boost::shared_ptr<SimpleBody> connectionPartA = this->addBox(MASS_SERVO,
			osg::Vec3(xPartA, 0, 0),
			CONNNECTION_PART_LENGTH, CONNNECTION_PART_WIDTH,
			CONNECTION_PART_HEIGHT, B_CONNECTION_A_ID);

	dReal xCrossPartA = xPartA + CONNECTION_PART_OFFSET;
	boost::shared_ptr<SimpleBody> crossPartA = this->addBox(MASS_CROSS / 3,
			osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_WIDTH,
			CROSS_HEIGHT, B_CROSS_PART_A_ID);
	boost::shared_ptr<SimpleBody> crossPartB = this->addBox(MASS_CROSS / 3,
			osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_HEIGHT,
			CROSS_WIDTH, B_CROSS_PART_B_ID);

	dReal xPartB = xCrossPartA + CONNECTION_PART_OFFSET;
	boost::shared_ptr<SimpleBody> connectionPartB = this->addBox(MASS_SERVO,
			osg::Vec3(xPartB, 0, 0),
			CONNNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT,
			CONNNECTION_PART_WIDTH, B_CONNECTION_B_ID);

	dReal xTail = xPartB + CONNNECTION_PART_LENGTH / 2 + separation
			+ SLOT_THICKNESS / 2;
	cardanTail_ = this->addBox(MASS_SLOT, osg::Vec3(xTail, 0, 0),
			SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH, B_SLOT_B_ID);

	// Cross Geometries

	boost::shared_ptr<SimpleBody> crossPartAedge1 = this->addBox(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, B_CROSS_PART_A_EDGE_1_ID);

	boost::shared_ptr<SimpleBody> crossPartAedge2 = this->addBox(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, B_CROSS_PART_A_EDGE_2_ID);

	boost::shared_ptr<SimpleBody> crossPartBedge1 = this->addBox(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, B_CROSS_PART_B_EDGE_1_ID);

	boost::shared_ptr<SimpleBody> crossPartBedge2 = this->addBox(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, B_CROSS_PART_B_EDGE_2_ID);

	// Create joints to hold pieces in position

	// root <-> connectionPartA
	this->fixBodies(cardanRoot_, connectionPartA);

	// connectionPartA <(hinge)> crossPartA
	boost::shared_ptr<Joint> joint = this->attachWithHinge(connectionPartA,
			connectionPartB, osg::Vec3(0, 0, 1),
			osg::Vec3(xPartA
					+ ((CONNNECTION_PART_LENGTH / 2)
					- (CONNNECTION_PART_LENGTH
					- CONNNECTION_PART_ROTATION_OFFSET)), 0, 0));


	// crossPartA <-> crossPartB
	this->fixBodies(crossPartA, crossPartB);

	// crossPartB <(hinge)> connectionPartB
	boost::shared_ptr<Joint> joint2 = this->attachWithHinge(crossPartB,
			connectionPartB, osg::Vec3(0, 1, 0),
			osg::Vec3(xPartB
					- ((CONNNECTION_PART_LENGTH / 2)
					- (CONNNECTION_PART_LENGTH
					- CONNNECTION_PART_ROTATION_OFFSET)), 0, 0));

	// connectionPartB <-> tail
	this->fixBodies(connectionPartB, cardanTail_);

	// Fix cross Parts
	this->fixBodies(crossPartA, crossPartAedge1);
	this->fixBodies(crossPartA, crossPartAedge2);
	this->fixBodies(crossPartB, crossPartBedge1);
	this->fixBodies(crossPartB, crossPartBedge2);

	// Create motors
	motor1_.reset(
			new ServoMotor(ioPair(this->getId(),0), joint,
							ServoMotor::DEFAULT_MAX_FORCE_SERVO,
							ServoMotor::DEFAULT_GAIN));
	motor2_.reset(
			new ServoMotor(ioPair(this->getId(),1), joint2,
							ServoMotor::DEFAULT_MAX_FORCE_SERVO,
							ServoMotor::DEFAULT_GAIN));
	return true;

}
bool ActiveCardanRenderModel::initRenderModel() {

	bool meshLoadingA = this->partA_->loadMesh(
			"../models/ActiveCardanHinge_Servo_Holder.stl");

	if (!meshLoadingA) {
		std::cerr << "[ActiveCardanRenderModel] Error loading model"
				<< std::endl;
		return false;
	}

	bool meshLoadingB = this->partB_->loadMesh(
			"../models/ActiveCardanHinge_Servo_Holder.stl");

	if (!meshLoadingB) {
		std::cerr << "[ActiveCardanRenderModel] Error loading model"
				<< std::endl;
		return false;
	}

	bool meshLoadingC = this->patCross_->loadMesh(
			"../models/ActiveCardan_CrossShaft.stl");

	if (!meshLoadingC) {
		std::cerr << "[ActiveCardanRenderModel] Error loading model"
				<< std::endl;
		return false;
	}

	if (isDebugActive()) {
		this->showDebugView();
	}

	float meshCorrection = inMm(0.5);

	// PART A
	osg::ref_ptr<osg::PositionAttitudeTransform> partA =
			this->partA_->getMesh();

	partA_->setColor(osg::Vec4(1, 0, 0, 1));
	partB_->setColor(osg::Vec4(0, 1, 0, 1));

	partA->setPosition(
			osg::Vec3(
					fromOde(meshCorrection +
							ActiveCardanModel::SLOT_THICKNESS
									+ ActiveCardanModel::CONNNECTION_PART_LENGTH
											/ 2), 0, 0));

	partA->setAttitude(osg::Quat(osg::inDegrees(90.0), osg::Vec3(0, 0, 1)) *
			osg::Quat(osg::inDegrees(90.0), osg::Vec3(1, 0, 0)));

	osg::ref_ptr<osg::PositionAttitudeTransform> patPartA(
			new osg::PositionAttitudeTransform());
	patPartA->addChild(partA);

	this->getRootNode()->addChild(patPartA.get());
	patPartA->setUpdateCallback(
			new BodyCallback(this->getModel(), ActiveCardanModel::B_SLOT_A_ID));

	//attachAxis(patPartA);

	// Cross
	osg::ref_ptr<osg::PositionAttitudeTransform> cross =
			this->patCross_->getMesh();

	osg::ref_ptr<osg::PositionAttitudeTransform> patCross(
			new osg::PositionAttitudeTransform());
	patCross->addChild(cross.get());

	this->getRootNode()->addChild(patCross.get());
	patCross->setUpdateCallback(
			new BodyCallback(this->getModel(),
					ActiveCardanModel::B_CROSS_PART_A_ID));
	//attachAxis(patCross);

	// PART B
	osg::ref_ptr<osg::PositionAttitudeTransform> partB =
			this->partB_->getMesh();
	partB->setPosition(
			fromOde(
					osg::Vec3(
							-(meshCorrection + ActiveCardanModel::SLOT_THICKNESS
									+ ActiveCardanModel::CONNNECTION_PART_LENGTH
											/ 2), 0, 0)));

	osg::Quat rotateY(osg::inDegrees(90.0), osg::Vec3(0, 1, 0));
	osg::Quat rotateServoX(osg::inDegrees(90.0), osg::Vec3(1, 0, 0));
	partB->setAttitude(rotateServoX * rotateY *
			osg::Quat(osg::inDegrees(90.0), osg::Vec3(1, 0, 0)));

	osg::ref_ptr<osg::PositionAttitudeTransform> patPartB(
			new osg::PositionAttitudeTransform());
	patPartB->addChild(partB.get());

	this->getRootNode()->addChild(patPartB.get());
	patPartB->setUpdateCallback(
			new BodyCallback(this->getModel(), ActiveCardanModel::B_SLOT_B_ID));

	//attachAxis(patPartB);

	return true;

}
Exemplo n.º 7
0
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]));
	}
}

}
Exemplo n.º 8
0
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_;
}

}
Exemplo n.º 9
0
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_;
}

}
Exemplo n.º 10
0
bool ActiveHingeRenderModel::initRenderModel() {

	bool meshLoadingA = this->partA_->loadMesh("../models/ActiveHinge_Frame.stl");

	if (!meshLoadingA) {
		std::cerr << "[ActiveHingeRenderModel] Error loading model"
				<< std::endl;
		return false;
	}

	bool meshLoadingB = this->partB_->loadMesh(
			"../models/ActiveCardanHinge_Servo_Holder.stl");

	if (!meshLoadingB) {
		std::cerr << "[ActiveHingeRenderModel] Error loading model"
				<< std::endl;
		return false;
	}

	if (isDebugActive()) {
		this->showDebugView();
		return true;
	}

	// PART A
	osg::ref_ptr<osg::PositionAttitudeTransform> frame =
			this->partA_->getMesh();

	partA_->setColor(osg::Vec4(1, 0, 0, 1));
	partB_->setColor(osg::Vec4(0, 1, 0, 1));

	frame->setPosition(
			fromOde(
					osg::Vec3(
							ActiveHingeModel::FRAME_LENGTH / 2 -
							ActiveHingeModel::SLOT_THICKNESS, 0,
							0)));
	frame->setAttitude(osg::Quat(osg::inDegrees(90.0), osg::Vec3(1, 0, 0)));
	osg::ref_ptr<osg::PositionAttitudeTransform> patFrame(
			new osg::PositionAttitudeTransform());
	patFrame->addChild(frame);

	this->getRootNode()->addChild(patFrame.get());
	patFrame->setUpdateCallback(
			new BodyCallback(this->getModel(), ActiveHingeModel::B_SLOT_A_ID));

	// PART B
	float servoMeshCorrection = inMm(2.2);

	osg::ref_ptr<osg::PositionAttitudeTransform> servo =
			this->partB_->getMesh();
	servo->setPosition(
			fromOde(
					osg::Vec3(
							-(ActiveHingeModel::SERVO_LENGTH / 2) - servoMeshCorrection, 0,
							0)));

	servo->setAttitude(osg::Quat(osg::inDegrees(90.0), osg::Vec3(0, 0, 1)) *
			osg::Quat(osg::inDegrees(180.0), osg::Vec3(0, 1, 0)));

	osg::ref_ptr<osg::PositionAttitudeTransform> patServo(
			new osg::PositionAttitudeTransform());
	patServo->addChild(servo.get());

	this->getRootNode()->addChild(patServo.get());
	patServo->setUpdateCallback(
			new BodyCallback(this->getModel(), ActiveHingeModel::B_SLOT_B_ID));

	return true;

}
Exemplo n.º 11
0
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);
}

}