void InvertedPendulum::createCart() {
CollisionManager *cm = Physics::getCollisionManager();
PhysicsManager *pm = Physics::getPhysicsManager();
SimObject *box = pm->getPrototype(SimulationConstants::PROTOTYPE_BOX_BODY);
SimObject *cylinder = pm->getPrototype(SimulationConstants::PROTOTYPE_CYLINDER_BODY);
SimObject *servo = pm->getPrototype(SimulationConstants::PROTOTYPE_SERVO_MOTOR);
SimObject *fix = pm->getPrototype(SimulationConstants::PROTOTYPE_FIXED_JOINT);
SimObject *hinge = pm->getPrototype(SimulationConstants::PROTOTYPE_HINGE_JOINT);
SimObject *other;
INIT_PARAMS;
SimBody *body = dynamic_cast<SimBody*>(box->createCopy());
mAgent->addObject(body);
MAKE_CURRENT(body);
SETP("Name", mNamePrefix + "Cart");
SETP("Mass", QString::number(0.05));
SETP("Position", Vector3DValue(0.0, mHalfBodyWidth, 0.0).getValueAsString());
SETP("CenterOfMass", "(0,0,0)");
SETP("Orientation", "(0,0,0)");
SETP("Dynamic", "true");
SETP("Width", DoubleValue(mBodyLength).getValueAsString());
SETP("Height", DoubleValue(mHalfBodyWidth * 2.0).getValueAsString());
SETP("Depth", DoubleValue(mHalfBodyWidth * 2.0).getValueAsString());
SETP("Color", "red");
SETP("Material", "ABS");
other = cylinder->createCopy();
mAgent->addObject(other);
MAKE_CURRENT(other);
SETP("Name", mNamePrefix + "RightWheel");
SETP("Color", "(96, 96, 96)");
SETP("Position", Vector3DValue(mBodyLength / 2.0, -0.01, 0.0).getValueAsString());
SETP("Radius", "0.01");
SETP("Length", DoubleValue(mHalfBodyWidth * 2.0).getValueAsString());
SETP("Orientation", "(0, 0, 0)");
SETP("Mass", "0.01");
SETP("CenterOfMass", "(0, 0, 0)");
SETP("Dynamic", "true");
cm->disableCollisions(
dynamic_cast<SimBody*>(body)->getCollisionObjects(),
dynamic_cast<SimBody*>(other)->getCollisionObjects(),
true);
other = cylinder->createCopy();
mAgent->addObject(other);
MAKE_CURRENT(other);
SETP("Name", mNamePrefix + "LeftWheel");
SETP("Color", "(96, 96, 96)");
SETP("Position", Vector3DValue(-mBodyLength / 2.0, -0.01, 0.0).getValueAsString());
SETP("Radius", "0.01");
SETP("Length", DoubleValue(mHalfBodyWidth * 2.0).getValueAsString());
SETP("Orientation", "(0, 0, 0)");
SETP("Mass", "0.01");
SETP("CenterOfMass", "(0, 0, 0)");
SETP("Dynamic", "true");
other = servo->createCopy();
mAgent->addObject(other);
MAKE_CURRENT(other);
SETP("MaxForce", "10.0");
SETP("MaxTorque", "10.0");
PARAM(StringValue, other, "Name")->set("/RightMotor");
PARAM(Vector3DValue, other, "AxisPoint1")->set(mBodyLength / 2.0, -0.01, -1);
PARAM(Vector3DValue, other, "AxisPoint2")->set(mBodyLength / 2.0, -0.01, 1);
PARAM(BoolValue, other, "IsAngularMotor")->set(false);
PARAM(StringValue, other, "FirstBody")->set(mGroupPrefix + "/Cart");
PARAM(StringValue, other, "SecondBody")->set(mGroupPrefix + "/RightWheel");
other = servo->createCopy();
mAgent->addObject(other);
MAKE_CURRENT(other);
SETP("MaxForce", "10.0");
SETP("MaxTorque", "10.0");
PARAM(StringValue, other, "Name")->set("/LeftMotor");
PARAM(Vector3DValue, other, "AxisPoint1")->set(-mBodyLength / 2.0, -0.01, -1);
PARAM(Vector3DValue, other, "AxisPoint2")->set(-mBodyLength / 2.0, -0.01, 1);
PARAM(BoolValue, other, "IsAngularMotor")->set(false);
PARAM(StringValue, other, "FirstBody")->set(mGroupPrefix + "/Cart");
PARAM(StringValue, other, "SecondBody")->set(mGroupPrefix + "/LeftWheel");
SimObject *stick = box->createCopy();
mAgent->addObject(stick);
MAKE_CURRENT(stick);
SETP("Name", mNamePrefix + "Stick");
SETP("Mass", QString::number(0.00002));
SETP("Position", Vector3DValue(0.0, mHalfBodyWidth * 2.0 + mStickLength / 2.0, 0.0).getValueAsString());
SETP("CenterOfMass", "(0,0,0)");
SETP("Orientation", "(0,0,0)");
SETP("Dynamic", "true");
SETP("Width", DoubleValue(0.005).getValueAsString());
SETP("Height", DoubleValue(mStickLength).getValueAsString());
SETP("Depth", DoubleValue(mHalfBodyWidth * 2.0).getValueAsString());
SETP("Color", "blue");
other = hinge->createCopy();
mAgent->addObject(other);
PARAM(StringValue, other, "Name")->set("/StickToBody");
PARAM(Vector3DValue, other, "AxisPoint1")->set(0, mHalfBodyWidth * 2.0, -1);
PARAM(Vector3DValue, other, "AxisPoint2")->set(0, mHalfBodyWidth * 2.0, 1);
PARAM(StringValue, other, "FirstBody")->set(mGroupPrefix + "/Cart");
PARAM(StringValue, other, "SecondBody")->set(mGroupPrefix + "/Stick");
SimObject *bob = cylinder->createCopy();
mAgent->addObject(bob);
MAKE_CURRENT(bob);
SETP("Name", mNamePrefix + "Bob");
SETP("Position", Vector3DValue(0, mHalfBodyWidth * 2.0 + mStickLength, 0.0).getValueAsString());
SETP("Radius", "0.01");
SETP("Length", DoubleValue(mHalfBodyWidth * 2.0).getValueAsString());
SETP("Orientation", "(0, 0, 0)");
SETP("Mass", "0.0005");
SETP("CenterOfMass", "(0, 0, 0)");
SETP("Dynamic", "true");
SETP("Color", "blue");
other = fix->createCopy();
mAgent->addObject(other);
PARAM(StringValue, other, "Name")->set("/BobFix");
PARAM(StringValue, other, "FirstBody")->set(mGroupPrefix + "/Stick");
PARAM(StringValue, other, "SecondBody")->set(mGroupPrefix + "/Bob");
cm->disableCollisions(
dynamic_cast<SimBody*>(body)->getCollisionObjects(),
dynamic_cast<SimBody*>(stick)->getCollisionObjects(),
true);
cm->disableCollisions(
dynamic_cast<SimBody*>(stick)->getCollisionObjects(),
dynamic_cast<SimBody*>(bob)->getCollisionObjects(),
true);
}
void MSeriesMotorMultipleTest::createModel() {
mGroupName = getName();
mNamePrefix = "/";
PhysicsManager *pm = Physics::getPhysicsManager();
// SimObject *boxInertiaBody = pm->getPrototype(
// SimulationConstants::PROTOTYPE_BOX_INERTIA_BODY);
SimObject *testMotor = pm->getPrototype(
SimulationConstants::PROTOTYPE_M_SERIES_TORQUE_DYNAMIXEL_MOTOR_MULTIPLE_TEST);
SimObject *boxBody = pm->getPrototype(
SimulationConstants::PROTOTYPE_BOX_BODY);
// SimObject *cylinderBody = pm->getPrototype(
// SimulationConstants::PROTOTYPE_CYLINDER_BODY);
//Agent SimObject group.
SimObjectGroup *agent = mAgent;
// First box
double mFirstBoxSize = 0.056;
mFirstBox = dynamic_cast<BoxBody*>(boxBody->createCopy());
PARAM(StringValue, mFirstBox, "Name")->set(mNamePrefix + "FirstBox");
PARAM(BoolValue, mFirstBox, "Dynamic")->set(false);
PARAM(Vector3DValue, mFirstBox, "Position")->set(0.0, mFirstBoxSize / 2.0, 0.0);
PARAM(Vector3DValue, mFirstBox, "CenterOfMass")->set(0.0, 0.0, 0.0);
PARAM(DoubleValue, mFirstBox, "Width")->set(mFirstBoxSize);
PARAM(DoubleValue, mFirstBox, "Height")->set(mFirstBoxSize);
PARAM(DoubleValue, mFirstBox, "Depth")->set(mFirstBoxSize);
PARAM(DoubleValue, mFirstBox, "Mass")->set(0.1);
PARAM(ColorValue, mFirstBox, "Color")->set(Color(255, 255, 255, 230));
PARAM(StringValue, mFirstBox, "Material")->set("ABS");
agent->addObject(mFirstBox);
// Long Arm
double mLongArmHeight = 0.50;
double mLongArmDepth = 0.002;
double mLongArmWidth = 0.025;
double mLongArmWeight = 0.065;
mLongArm = dynamic_cast<BoxBody*>(boxBody->createCopy());
PARAM(StringValue, mLongArm, "Name")->set(mNamePrefix + "LongArm");
PARAM(BoolValue, mLongArm, "Dynamic")->set(true);
PARAM(Vector3DValue, mLongArm, "Position")->set(0.0, mFirstBoxSize + mLongArmHeight / 2.0 - 0.017 / 2.0, 0.0);
PARAM(Vector3DValue, mLongArm, "CenterOfMass")->set(0.0, 0.0, 0.0);
PARAM(DoubleValue, mLongArm, "Width")->set(mLongArmWidth);
PARAM(DoubleValue, mLongArm, "Height")->set(mLongArmHeight);
PARAM(DoubleValue, mLongArm, "Depth")->set(mLongArmDepth);
PARAM(DoubleValue, mLongArm, "Mass")->set(mLongArmWeight);
PARAM(ColorValue, mLongArm, "Color")->set(Color(255, 255, 0, 230));
PARAM(StringValue, mLongArm, "Material")->set("ABS");
agent->addObject(mLongArm);
// Add motor
QString groupPrefix = "/";
groupPrefix.append(mGroupName).append("/");
mMotor = dynamic_cast<TorqueDynamixelMotorAdapter*>(testMotor->createCopy());
PARAM(StringValue, mMotor, "Name")->set(mNamePrefix + "TestMotor");
PARAM(StringValue, mMotor, "FirstBody")->set(groupPrefix + "FirstBox");
PARAM(StringValue, mMotor, "SecondBody")->set(groupPrefix + "LongArm");
PARAM(Vector3DValue, mMotor, "AxisPoint1")->set(0.0, mFirstBoxSize, 0.1);
PARAM(Vector3DValue, mMotor, "AxisPoint2")->set(0.0, mFirstBoxSize, 0.0);
PARAM(DoubleValue, mMotor, "MinAngle")->set(-150);
PARAM(DoubleValue, mMotor, "MaxAngle")->set(150);
PARAM(StringValue, mMotor, "ActiveMotorModel")->set("ODE_H_MSeriesTorqueSpringTestMotor");
agent->addObject(mMotor);
// Disable collision between objects which are allowed to interpenetrate
CollisionManager *collisionManager = Physics::getCollisionManager();
collisionManager->disableCollisions(mFirstBox->getCollisionObjects(),
mLongArm->getCollisionObjects(), true);
mSimObjects = agent->getObjects();
//add agent SimObjectGroup at PhysicsManager to provide the interface to the robot.
pm->addSimObjectGroup(agent);
}
