/** creates vehicle at desired position
@param pos struct Position with desired position
*/
void Hexabot::create(const osg::Matrix& pose){
if (created) {
destroy();
}
#ifdef DEBUG_MODE
cout << "hello, This is the debug mode" << endl;
cout << "@@@@ First, We create the robot!! @@@@" << endl;
#endif
/*********************************************************************************:
* The ordinal configuration
* Environment, color, feature etc/
***********************************************************************************/
// we want legs colliding with other legs, so we set internal collision
// flag to "false".
odeHandle.createNewSimpleSpace(parentspace,false);
// Hexabot color ;-)
osgHandle.color = Color(0/255., 30./255., 0./255., 1.0f);
// Joint color ;-)
OsgHandle dynaHandle(osgHandle);
dynaHandle.color = Color(204/255., 51/255., 0./255., 1.0f);
//dynaHandle.color = Color(0/255., 0/255., 0./255., 1.0f);
// Tibia Color (colored by leg)
OsgHandle tibiaHandle[6] = {osgHandle, osgHandle, osgHandle, osgHandle, osgHandle, osgHandle};
tibiaHandle[0].color = Color(0./255., 0./255., 0./255., 1.0f);
tibiaHandle[1].color = Color(153./255., 102./255., 0./255., 1.0f);
tibiaHandle[2].color = Color(153./255., 204./255., 0./255., 1.0f);
tibiaHandle[3].color = Color(153./255., 0./255., 30./255., 1.0f);
tibiaHandle[4].color = Color(153./255., 102./255., 30./255., 1.0f);
tibiaHandle[5].color = Color(153./255., 204./255., 30./255., 1.0f);
// change Material substance
OdeHandle odeHandleBody(odeHandle);
odeHandleBody.substance.toMetal(3.0);
// get a representation of the origin
// By multipling this parameter to pose, we can get only the position data of pose
const Pos nullpos(0, 0, 0);
/*********************************************************************************:
* Create Body Structure
* Trunk, Leg etc.
***********************************************************************************/
/*********************************************
* Main Frame */
// Make the boxes connect each other to make rectangle
/**********************************************/
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Body frame starts!! @@@@" << endl;
#endif
// make the rectangle boxes to make hexagon (they are connected each other and make hexagon)
// make 2 rectangle plate objects
for(int i=0; i< 2;i++){
trunk.tPlate[i] = new Box( conf.body.length, conf.body.width, conf.body.height);
trunk.tPlate[i]->getOSGPrimitive()->setTexture("Images/wood.rgb");
// This function (setMass) seems not to be implemented
//trunk.tPlate[i]->setMass(conf.body.mass / 6.);
}
// First object should be initialized (I choose the lower rectangle one)
trunk.tPlate[0]->init(odeHandle, conf.body.mass / 2., osgHandle);
trunk.tPlate[0]->setPose( pose );
// add it to Object
objects.push_back(trunk.tPlate[0]);
// just for memorlizing transForm
trunk.tUpTrans = new ImpTransform2(trunk.tPlate[0], trunk.tPlate[1],Matrix::rotate(0., Vec3(0, 0, 1)) *Matrix::translate(0, 0, conf.dyna.height));
trunk.tTrans = trunk.tUpTrans;
trunk.tTrans->init(odeHandle, conf.body.mass / 2., osgHandle);
// add it to Object
objects.push_back(trunk.tTrans);
/*********************************************
LEGs **sholder, coxa femur tibia
//
**********************************************/
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Leg frame starts!! @@@@" << endl;
#endif
// Useful Parameter to make robot model
// Transition Matrix from origin of this robot to center of the robot
// notice: the orgin of the robot is on the center of the lower hexagonal plate
osg::Matrix trans_rO_to_rC = Matrix::translate( 0., 0., conf.dyna.height / 2.);
// Trans Matrix from center of the robot to Shoulder Dynamixel center
// The horizontal length between robot center and Shoulder Dynamixel center
double length_rC_to_sC_x = (conf.jLength.length_x_center_to_TCJ - conf.dyna.length_axis_to_center);
// Matrix
osg::Matrix trans_rC_to_sC_x = Matrix::translate( length_rC_to_sC_x, 0, 0);
// The horizontal length between Shoulder Dynamixel center and Shoulder Dynamixel center
// Matrix
osg::Matrix trans_sC_to_sC_y = Matrix::translate(0, conf.jLength.length_y_TCJ_to_TCJ, 0);
// Trans Matrix from center of the robot to Coxa Dynamixel center
// The horizontal length between robot center and Coxa Dynamixel center
double length_rC_to_cC = length_rC_to_sC_x + conf.jLength.length_TCJ_to_CTJ;
// Matrix
osg::Matrix trans_rC_to_cC = Matrix::translate( length_rC_to_cC, 0, 0);
// Trans Matrix from center of the robot to Femur Dynamixel center
// The horizontal length between robot center and Femur Dynamixel center
double length_rC_to_fC_x = conf.jLength.length_x_center_to_TCJ + conf.jLength.length_TCJ_to_CTJ;
double length_rC_to_fC_z = conf.jLength.length_CTJ_to_FTJ - conf.dyna.length_axis_to_center;
//double length_rC_to_fC = length_rC_to_cC + conf.jLength.length_CTJ_to_FTJ;
// Matrix
osg::Matrix trans_rC_to_fC = Matrix::translate( length_rC_to_fC_x, 0, length_rC_to_fC_z);
// Trans Matrix from center of the robot to Foot Plate center
// The horizontal length between robot center and Foot Plate center
double length_rC_to_tC_x = length_rC_to_fC_x + conf.dyna.length_from_axis_to_tip;
double length_rC_to_tC_z = conf.jLength.length_CTJ_to_FTJ + (-conf.foot.length + conf.dyna.width)/2.;
// Matrix
osg::Matrix trans_rC_to_tC = Matrix::translate( length_rC_to_tC_x, 0, length_rC_to_tC_z);
// Trans Matrix from center of the robot to center of the foot sphere
// Matrix
osg::Matrix trans_rC_to_fsC = Matrix::translate( length_rC_to_fC_x, 0, length_rC_to_tC_z - conf.foot.length/2. - conf.foot.footRadius );
// Trans Matrix from center of the tibia to center of the foot sphere
// Matrix
osg::Matrix trans_tC_to_fsC = Matrix::translate( - conf.dyna.length_from_axis_to_tip, 0, - conf.foot.length/2. - conf.foot.footRadius);
// Trans Matrix from center of the robot to TC joint Center
// Matrix
osg::Matrix trans_rC_to_TCj = Matrix::translate( conf.jLength.length_x_center_to_TCJ, 0, 0);
// Trans Matrix from center of the robot to CT joint Center
// Matrix
osg::Matrix trans_rC_to_CTj = Matrix::translate( conf.jLength.length_x_center_to_TCJ + conf.jLength.length_TCJ_to_CTJ, 0, 0);
// Trans Matrix from center of the robot to FT joint Center
// Matrix
osg::Matrix trans_rC_to_FTj = Matrix::translate( conf.jLength.length_x_center_to_TCJ + conf.jLength.length_TCJ_to_CTJ, 0, + conf.jLength.length_CTJ_to_FTJ);
for(int i = 0; i < LEG_POS_MAX; i++){
// Name of the Leg
LegPos leg = LegPos(i);
// Rotation Matrix : change the rotation direction according to leg number
// By using this, the direction of leg can be changed.. It is used to adapt to the location of the leg
Matrix legRotate;
Matrix legTrans;
if(leg == L1){
legRotate = Matrix::rotate(M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, conf.jLength.length_y_TCJ_to_TCJ + conf.y_trans_center, 0);
}
else if( leg == L2){
legRotate = Matrix::rotate(M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, conf.y_trans_center, 0);
}
else if(leg == L3 ){
legRotate = Matrix::rotate(M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, -conf.jLength.length_y_TCJ_to_TCJ +conf.y_trans_center, 0);
}else if(leg == L4){
legRotate = Matrix::rotate(-M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, -conf.jLength.length_y_TCJ_to_TCJ - conf.y_trans_center, 0);
}else if(leg == L5){
legRotate = Matrix::rotate(-M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, -conf.y_trans_center, 0);
}else{
legRotate = Matrix::rotate(-M_PI*1./2., Vec3(0, 0, 1));
legTrans = Matrix::translate(0, conf.jLength.length_y_TCJ_to_TCJ - conf.y_trans_center, 0);
}
// Shoulder ******************************************
// We connect the sholder dynamixel to main Body
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Shoulder starts!! @@@@" << endl;
#endif
// sholder Dynamixel Box
Box* dBox = new Box( conf.dyna.length, conf.dyna.width, conf.dyna.height);
dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
//dBox->setMass(conf.dyna.mass);
// trans (locate the object on the hexagon plane)
// first, I turn the object to leg's direction and translate it to desired position ()
// mention that this pose matrix is multipled vector from right hand <- it is different from usual one!!
ImpTransform2* dTrans = new ImpTransform2(trunk.tPlate[0], dBox,
trans_rC_to_sC_x * trans_rO_to_rC * legTrans * legRotate);
dTrans->init(odeHandle, conf.dyna.mass, dynaHandle);
// save to the leg struct
legs[leg].shoulderBox = dBox;
legs[leg].shoulder = dTrans;
// add it to object
objects.push_back(legs[leg].shoulder);
// Coxa **********************************************
// We make the first joint and link of Hexabot
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Coxa starts!! @@@@" << endl;
#endif
// Make the Dyanmixel for link
// Coxa (1st link) Dynamixel Box
Box* link1dBox = new Box( conf.dyna.length, conf.dyna.width, conf.dyna.height);
link1dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
link1dBox->init(odeHandle, conf.dyna.mass , dynaHandle);
// about the pose or something
// set the pose
// pose:: pose represent the attitude and position of the object by quotanion
// we can use the function rotate and translate as if it is attitude change 4*4 matrix
//
// I should say about mechanism of the pose in my understanding.
// Pose ; this is the 4 by 4 matrix and it represents the rotation and translation
// It can be used as if it is normal 4 by 4 rotation matrix.
// But what you should notice is that this matrix is for Row Vector (1 by 4 vector)
// So, the row of the calculating is inverse to normal one!!
// You should be careful about it
Matrix l1Pose = Matrix::rotate( M_PI / 2., Vec3(1, 0, 0)) * /* this rotation is used to turn the dymamixel to optimal attitude!! (because, we change the axis of joints) */
trans_rC_to_cC * trans_rO_to_rC * legTrans* legRotate * pose; // first, I turn the object to leg's direction and translate it to desired position ()
link1dBox->setPose(l1Pose);
// save it to leg struct
legs[leg].coxa = link1dBox;
// add it to object
objects.push_back(legs[leg].coxa);
// TC joints *******************************:::::::
// create the joint of dynamixel on Body (from 1st link to body)
// calculate the joint pose (attitude and position) by using trans and rotate function
osg::Matrix j1Pose = trans_rC_to_TCj * trans_rO_to_rC * legTrans* legRotate * pose;
// To make a joint, we need the position vector only, so multiple nullpos to the pose to get the vector.
// the attitude is determined by axis (it is z axis)
HingeJoint* TCj = new HingeJoint( trunk.tPlate[0], link1dBox, nullpos * j1Pose, Axis(0,0,1) * legRotate );
// ^ Notice: When we want to rotate the Axis() like Pose, we have to multiple the rotation matrix from RIGHT SIDE
// Not Left side, it is very different from real calculation
TCj->init(odeHandle, osgHandle, true, conf.rate * 0.04);
// add the joint to the joint vector
joints.push_back(TCj);
// create motor (by using servo motor, we will do position control)
//OneAxisServo* servo1 = new OneAxisServoVel(odeHandle, TCj, conf.servoParam.TC_angle_MIN, conf.servoParam.TC_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
//Debug
OneAxisServo* servo1;
#ifdef HEXABOT_MAIN_VER_1_6
servo1 = new OneAxisServoVel(odeHandle, TCj, conf.servoParam.TC_angle_MIN, conf.servoParam.TC_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
#else
servo1 = new OneAxisServoCentered(TCj, conf.servoParam.TC_angle_MIN, conf.servoParam.TC_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.integ, conf.servoParam.maxVel, 1.0);
#endif
// save it to leg struct
legs[leg].tcJoint = TCj;
legs[leg].tcServo = servo1;
// add it to servo Map
servos[getMotorName(leg, TC)] = servo1;
// create torque sensor
// In this time, I do not use scaller just get real value
motorTorqSensors[getMotorName(leg, TC)] = new TorqueSensor(1., 1.);
// initialize (own is Null, it was not used in simulation)
motorTorqSensors[getMotorName(leg, TC)]->init(NULL, TCj);
// Femur **********************************************
// We make the second joint and link of Hexabot (CT joint and femur)
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Femur starts!! @@@@" << endl;
#endif
// Make the Dyanmixel for link
// Femur (2nd link) Dynamixel Box
Box* link2dBox = new Box( conf.dyna.length, conf.dyna.width, conf.dyna.height);
link2dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
link2dBox->init(odeHandle, conf.dyna.mass , dynaHandle);
// set the pose
// pose:: pose represent the attitude and position of the object by quotanion
// we can use the function rotate and translate as if it is attitude change 4*4 matrix
osg::Matrix l2Pose = Matrix::rotate( M_PI / 2., Vec3(0, 0, 1)) * Matrix::rotate( M_PI / 2., Vec3(1, 0, 0)) * // this rotation is used to turn the dymamixel to optimal attitude!! (because, we change the axis of joints)
trans_rC_to_fC * trans_rO_to_rC * legTrans * legRotate * pose; // first, I turn the object to leg's direction and translate it to desired position ()
link2dBox->setPose(l2Pose);
// save it to leg struct
legs[leg].femur = link2dBox;
// add it to object
objects.push_back(legs[leg].femur);
// CT joint :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
// create the joint of dynamixel on Body (from 2nd link to 1st Link)
// calculate the joint pose (attitude and position) by using trans and rotate function
osg::Matrix j2Pose = trans_rC_to_CTj * trans_rO_to_rC * legTrans* legRotate * pose;
// To make a joint, we need the position vector only, so multiple nullpos to the pose to get the vector.
// the attitude is determined by axis (it is z axis)
HingeJoint* CTj = new HingeJoint( legs[leg].coxa, link2dBox, nullpos * j2Pose, Axis(0,1,0) * legRotate);
// ^ Notice: When we want to rotate the Axis() like Pose, we have to multiple the rotation matrix from RIGHT SIDE
// Not Left side, it is very different from real calculation
CTj->init(odeHandle, osgHandle, true, conf.rate * 0.04);
// add the joint to the joint vector
joints.push_back(CTj);
// create motor (by using servo motor, we will do position control)
// OneAxisServo* servo2 = new OneAxisServoVel(odeHandle, CTj, conf.servoParam.CT_angle_MIN, conf.servoParam.CT_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
//Debug
OneAxisServo* servo2;
#ifdef HEXABOT_MAIN_VER_1_6
servo2 = new OneAxisServoVel(odeHandle, CTj, conf.servoParam.CT_angle_MIN, conf.servoParam.CT_angle_MAX, conf.servoParam.power, conf.servoParam.damp,conf.servoParam.maxVel, 1.0);
#else
servo2 = new OneAxisServoCentered(CTj, conf.servoParam.CT_angle_MIN, conf.servoParam.CT_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.integ, conf.servoParam.maxVel, 1.0);
#endif
// save it to leg struct
legs[leg].ctJoint = CTj;
legs[leg].ctServo = servo2;
// add it to servo Map
servos[Hexabot::getMotorName(leg, CT)] = servo2;
// create torque sensor
// In this time, I do not use scaller just get real value
motorTorqSensors[getMotorName(leg, CT)] = new TorqueSensor(1., 1.);
// initialize (own is Null, it was not used in simulation)
motorTorqSensors[getMotorName(leg, CT)]->init(NULL, CTj);
// Tibia **********************************************
// We make the third joint and link of Hexabot (FT joint and femur)
#ifdef DEBUG_MODE
cout << "@@@@ Creation of Tibia starts!! @@@@" << endl;
#endif
// Make the Plate
// Tibia (3rd link) Plate
Box* link3dBox = new Box( conf.foot.height, conf.foot.width, conf.foot.length);
link3dBox->getOSGPrimitive()->setTexture("Images/wood.rgb");
link3dBox->init(odeHandle, conf.foot.mass , tibiaHandle[leg]);
// set the pose
// pose:: pose represent the attitude and position of the object by quotanion
// we can use the function rotate and translate as if it is attitude change 4*4 matrix
osg::Matrix l3Pose = trans_rC_to_tC * trans_rO_to_rC * legTrans* legRotate * pose; // first, I turn the object to leg's direction and translate it to desired position ()
link3dBox->setPose(l3Pose);
// save it to leg struct
legs[leg].tibia = link3dBox;
// add it to object
objects.push_back(legs[leg].tibia);
//Make Foot
// Sphere
Sphere* foot = new Sphere( conf.foot.footRadius );
foot->getOSGPrimitive()->setTexture("Images/wood.rgb");
// Set the substance of foot
OdeHandle rubberHandle(odeHandle);
const Substance FootSubstance(3.0, 0.0, 500.0, 0.1);//500
rubberHandle.substance = FootSubstance;
legs[leg].footSphere = foot;
//translate
legs[leg].foot = new ImpTransform2(legs[leg].tibia, foot, trans_tC_to_fsC);
// initialize
legs[leg].foot->init(rubberHandle, 0.0, osgHandle);
//add it to objects
objects.push_back(legs[leg].foot);
// FT joint :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
// create the joint of dynamixel on Femur (from 3rd link to 2nd Link)
// calculate the joint pose (attitude and position) by using trans and rotate function
osg::Matrix j3Pose = trans_rC_to_FTj * trans_rO_to_rC * legTrans* legRotate * pose;
// To make a joint, we need the position vector only, so multiple nullpos to the pose to get the vector.
// the attitude is determined by axis (it is z axis)
HingeJoint* FTj = new HingeJoint( legs[leg].femur, link3dBox, nullpos * j3Pose, Axis(0,1,0) * legRotate);
// ^ Notice: When we want to rotate the Axis() like Pose, we have to multiple the rotation matrix from RIGHT SIDE
// Not Left side, it is very different from real calculation
FTj->init(odeHandle, osgHandle, true, conf.rate * 0.04);
// add the joint to the joint vector
joints.push_back(FTj);
// create motor (by using servo motor, we will do position control)
// OneAxisServo* servo3 = new OneAxisServoVel(odeHandle, FTj, conf.servoParam.FT_angle_MIN, conf.servoParam.FT_angle_MAX, conf.servoParam.power, conf.servoParam.damp, conf.servoParam.maxVel, 1.0);
//Debug
OneAxisServo* servo3;
#ifdef HEXABOT_MAIN_VER_1_6
servo3 = new OneAxisServoVel(odeHandle, FTj, conf.servoParam.FT_angle_MIN, conf.servoParam.FT_angle_MAX, conf.servoParam.power, conf.servoParam.damp,conf.servoParam.maxVel, 1.0);
#else
servo3 = new OneAxisServoCentered(FTj, conf.servoParam.FT_angle_MIN, conf.servoParam.FT_angle_MAX, conf.servoParam.power, conf.servoParam.damp,conf.servoParam.integ, conf.servoParam.maxVel, 1.0);
#endif
// save it to leg struct
legs[leg].ftJoint = FTj;
legs[leg].ftServo = servo3;
// add it to servo Map
servos[Hexabot::getMotorName(leg, FT)] = servo3;
// create torque sensor
// In this time, I do not use scaller just get real value
motorTorqSensors[getMotorName(leg, FT)] = new TorqueSensor(1., 1.);
// initialize (own is Null, it was not used in simulation)
motorTorqSensors[getMotorName(leg, FT)]->init(NULL, FTj);
// Leg contact Sensors (Foot toe)
legContactSensors[leg] = new ContactSensor(false, 100, conf.foot.footRadius*1.1, false, true, Color(0,5,0));
//make the sphere a little bit larger than real foot to detect touching 1.01
//legContactSensors[leg]->update();
// We set the contact sensor at the point of foot sphere
//legContactSensors[leg]->init(odeHandle, osgHandle, legs[leg].tibia, true, trans_tC_to_fsC);
// this is changed to adopt the new version of lpzrobots
legContactSensors[leg]->setInitData(odeHandle, osgHandle, TRANSM(0, 0, 0));//trans_tC_to_fsC);
legContactSensors[leg]->init(legs[leg].foot);
//legContactSensors[leg]->update();
//odeHandle.addIgnoredPair(legs[leg].foot, legContactSensors[leg]->getTransformObject());
//legContactSensors[LegPos(i)]->setInitData(odeHandle, osgHandle, TRANSM(0, 0, -(0.5) * l4));
//legContactSensors[LegPos(i)]->init(foot);
}
#ifdef DEBUG_MODE
cout << "@@@@ Creation Phase ends!! @@@@" << endl;
#endif
created=true;
}
/** creates vehicle at desired position
@param pos struct Position with desired position
*/
void Hexapod::create( const osg::Matrix& pose ){
if (created) {
destroy();
}
odeHandle.createNewSimpleSpace(parentspace,true);
// color of joint axis and whiskers
OsgHandle osgHandleJ = osgHandle.changeColor(Color(72./255.,16./255.,16./255.));
TwoAxisServoVel* servo;
OneAxisServo* spring;
OneAxisServo* spring2;
FixedJoint* fixedJoint;
// create body
double twidth = conf.size * conf.width ;// 1/1.5;
double theight = conf.size * conf.height; // 1/4;
trunk = new Box(conf.size, twidth, theight);
trunk->setTexture("Images/toy_fur3.jpg");
trunk->init(odeHandle, conf.mass*conf.percentageBodyMass, osgHandle);
osg::Matrix trunkPos = TRANSM(0,0,conf.legLength)*pose;
trunk->setPose(trunkPos);
objects.push_back(trunk);
if(conf.useBigBox){
Primitive* pole;
double poleheight=conf.size*2;
pole = new Box(poleheight, twidth*3.2, theight*1);
bigboxtransform= new Transform(trunk,pole, osg::Matrix::translate(0,0,2*theight));
bigboxtransform->init(odeHandle, 0, osgHandle,Primitive::Geom /*| Primitive::Draw */);
}else{
bigboxtransform=0;
}
osg::Matrix m0 = pose;
if(conf.irSensors == true){
for(int i = -1; i < 2; i+=2){
irbox = new Box(0.1,0.1,0.1);
irbox->setTexture("Images/toy_fur3.jpg");
irbox->init(odeHandle, 0.00001, osgHandle);
irbox->setPose(ROTM(M_PI/4,0,0,1) * TRANSM(i*conf.size/2,0,theight/2)*trunkPos);
objects.push_back(irbox);
fixedJoint = new FixedJoint(trunk,irbox);
fixedJoint->init(odeHandle, osgHandleJ, true, 0.4);
joints.push_back(fixedJoint);
}
for(int i = -1; i < 2; i+=2){
irbox = new Box(0.1,0.1,0.15);
irbox->setTexture("Images/toy_fur3.jpg");
irbox->init(odeHandle, 0.00001, osgHandle);
irbox->setPose(TRANSM(0,i*twidth/2,theight/2 + 0.05)*trunkPos);
objects.push_back(irbox);
fixedJoint = new FixedJoint(trunk,irbox);
fixedJoint->init(odeHandle, osgHandleJ, true, 0.4);
joints.push_back(fixedJoint);
}
irSensorBank.init(odeHandle, osgHandle);
if (conf.irFront){ // add front left and front right infrared sensor to sensorbank if required
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[2],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::translate(1*0.05,0,0),
conf.irRangeFront, RaySensor::drawAll);
IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[2],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,-0.05,0),
conf.irRangeFront, RaySensor::drawAll);
}
if (conf.irBack){ // add front left and front right infrared sensor to sensorbank if required
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[1],
Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) *
Matrix::translate(-1*0.05,0,0),
conf.irRangeBack, RaySensor::drawAll);
IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[1],
Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,0.05,0),
conf.irRangeBack, RaySensor::drawAll);
}
if(conf.irLeft){
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[3],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,-0.05,0.05),
conf.irRangeLeft, RaySensor::drawAll);
/* IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[3],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,-0.05,0),
conf.irRangeLeft, RaySensor::drawAll);
*/
}
if(conf.irRight){
IRSensor* sensor = new IRSensor();
irSensorBank.registerSensor(sensor, objects[4],
Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,0.05,0.05),
conf.irRangeLeft, RaySensor::drawAll);
/* IRSensor* sensor2 = new IRSensor();
irSensorBank.registerSensor(sensor2, objects[4],
Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) *
Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) *
Matrix::translate(0,0.05,0),
conf.irRangeRight, RaySensor::drawAll);
*/
}
}
std::vector<Primitive*> tarsusParts;
// legs (counted from back to front)
double legdist = conf.size*0.9 / (conf.legNumber/2-1);
for ( int n = 0; n < conf.legNumber; n++ ) {
int v = n;
double len1 = conf.legLength*0.5;
double rad1 = conf.legLength/15;
double len2 = conf.legLength*0.5;
double rad2 = conf.legLength/15;
// upper limp
Primitive* coxaThorax;
Pos pos = Pos(-conf.size/(2+0.2) + ((int)n/2) * legdist,
n%2==0 ? - twidth/2 : twidth/2,
conf.legLength - theight/3);
osg::Matrix m = ROTM(M_PI/2,v%2==0 ? -1 : 1,0,0) * TRANSM(pos) * pose;
coxaThorax = new Capsule(rad1, len1);
coxaThorax->setTexture("Images/toy_fur3.jpg");
coxaThorax->init(odeHandle, legmass, osgHandle);
osg::Matrix m1 = TRANSM(0,0,-len1/2)
* ROTM(M_PI,0,0,v%2==0 ? -1 : 1)
* ROTM(2*M_PI,0,v%2==0 ? -1 : 1,0) * m;
coxaThorax->setPose(m1);
thoraxPos.push_back(coxaThorax->getPosition());
thorax.push_back(coxaThorax);
objects.push_back(coxaThorax);
legs.push_back(coxaThorax);
// powered hip joint
Pos nullpos(0,0,0);
UniversalJoint* j
= new UniversalJoint(trunk, coxaThorax, nullpos * m,
ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * Axis(v%2==0 ? -1 : 1,0,0) * m,
ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * Axis(0,1,0) * m);
j->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
joints.push_back(j);
legContactArray[n].joint = j->getJoint();
// values will be set in setParam later
servo = new TwoAxisServoVel(odeHandle, j,-1,1, 1, -1,1,1,0 );
// servo = new UniversalServo(j,-1,1, 1, -1,1,1,0);
hipservos.push_back(servo);
// lower leg
Primitive* tibia;
tibia = new Capsule(rad2, len2);
tibia->setTexture("Images/toy_fur3.jpg");
tibia->init(odeHandle, legmass, osgHandle);
osg::Matrix m2 = TRANSM(0,0,-len2/2) * ROTM(1.5,v%2==0 ? -1 : 1,0,0)
* TRANSM(0,0,-len1/2) * m1;
tibia->setPose(m2);
objects.push_back(tibia);
legs.push_back(tibia);
legContactArray[n].legID = n;
legContactArray[n].geomid = tibia->getGeom();
legContactArray[n].bodyID = tibia->getBody();
if(conf.useTebiaJoints){
// springy knee joint
HingeJoint* k = new HingeJoint(coxaThorax, tibia, Pos(0,0,-len1/2) * m1,
Axis(v%2==0 ? -1 : 1,0,0) * m1);
k->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
// servo used as a spring
spring = new HingeServo(k, -1, 1, 1, 0.01,0); // parameters are set later
tebiasprings.push_back(spring);
joints.push_back(k);
}else{
// fixed knee joint
FixedJoint* k = new FixedJoint(coxaThorax, tibia);
k->init(odeHandle, osgHandleJ, false, rad1 * 2.1);
joints.push_back(k);
}
// lower limp should not collide with body!
odeHandle.addIgnoredPair(trunk,tibia);
if(conf.tarsus){
// New: tarsus
Primitive *tarsus;
double angle = M_PI/12;
double radius = rad2/2;
double length = len2/2;
double mass = legmass/10;
tarsus = new Capsule(radius,length);
tarsus->setTexture("Images/toy_fur3.jpg");
tarsus->init(odeHandle, mass, osgHandle);
osg::Matrix m4;
osg::Matrix m3 =
ROTM(-angle,v%2==0 ? -1 : 1,0,0) *
TRANSM(0,0,-len2/2) *
m2;
if(v < 2){
m4 = ROTM(v%2==0 ? angle : -angle,0,v%2==0 ? -1 : 1,0) * m3;
}else if( v > 3){
m4 = ROTM(v%2==0 ? -angle : angle,0,v%2==0 ? -1 : 1,0) * m3;
}else{
m4 = m3;
}
m4 = TRANSM(0,0,-length/2) * m4;
tarsus->setPose(m4);
tarsusParts.push_back(tarsus);
objects.push_back(tarsus);
if(conf.useTarsusJoints){
// springy joint
HingeJoint* k = new HingeJoint(tibia, tarsus, Pos(0,0,-len2/2) * m2,
Axis(v%2==0 ? -1 : 1,0,0) * m2);
k->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
// servo used as a spring
// spring2 = new HingeServo(k, -1, 1, 1, 0.01,0); // parameters are set later
spring2 = new OneAxisServoVelocityControlled(odeHandle,k, -1, 1, 1, 0.01); // parameters are set later
tarsussprings.push_back(spring2);
joints.push_back(k);
}else{
FixedJoint* k = new FixedJoint(tibia, tarsus);
k->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
joints.push_back(k);
}
Primitive *section = tarsus;
for(int i = 1; i < conf.numTarsusSections; i++){
double lengthS = length/1.5;
double radiusS = radius/1.5;
section = new Capsule(radiusS,lengthS);
section->setTexture("Images/toy_fur3.jpg");
section->init(odeHandle, mass/2, osgHandle);
osg::Matrix m5;
if(v < 2){
m5 = TRANSM(0,0,-lengthS/2) *
ROTM(v%2==0 ? angle : -angle,0,v%2==0 ? -1 : 1,0) *
ROTM(v%2==0 ? angle : -angle,1,0,0) *
TRANSM(0,0,-length/2) *
m4;
}else if(v > 3){
m5 = TRANSM(0,0,-lengthS/2) *
ROTM(v%2==0 ? -angle : angle,0,v%2==0 ? -1 : 1,0) *
ROTM(v%2==0 ? angle : -angle,1,0,0) *
TRANSM(0,0,-length/2) *
m4;
}else{
m5 = TRANSM(0,0,-lengthS/2) *
ROTM(v%2==0 ? angle : -angle,1,0,0) *
TRANSM(0,0,-length/2) *
m4;
}
section->setPose(m5);
objects.push_back(section);
tarsusParts.push_back(section);
FixedJoint* fj = new FixedJoint(tarsusParts[i-1], tarsusParts[i]);
fj->init(odeHandle, osgHandleJ, true, rad1 * 2.1);
joints.push_back(fj);
m4 = m5;
}
std::cout<< "legContactArray[" << n << "].legID = " << n << std::endl;
legContactArray[n].legID = n;
legContactArray[n].geomid = section->getGeom();
legContactArray[n].bodyID = section->getBody();
tarsusParts.clear();
}
}
// New: wiskers
for ( int n = -1; n < 2; n+=2 ) {
double l1 = conf.legLength*0.5;
double t1 = conf.legLength/30;
Primitive* whisker;
Pos pos = Pos(conf.size/(2)+t1,
n*twidth/4,
conf.legLength + theight/5);
osg::Matrix m = ROTM(M_PI/10, n,0,0) * ROTM(M_PI/2+M_PI/10, 0,-1,0) * TRANSM(pos) * pose;
whisker = new Capsule(t1, l1);
whisker->init(odeHandle, legmass/10, osgHandleJ);
osg::Matrix m1 = TRANSM(0,0,-l1/2) * m;
whisker->setPose(m1);
objects.push_back(whisker);
//FixedJoint* k = new FixedJoint(trunk, whisker);
//k->init(odeHandle, osgHandle, false, 0);
HingeJoint* k = new HingeJoint(trunk, whisker, Pos(0,0,0) * m,
Axis(1,0,0) * m);
k->init(odeHandle, osgHandleJ, true, t1 * 2.1);
// servo used as a spring
spring = new HingeServo(k, -M_PI/6, M_PI/6, .1, 0.01,0);
whiskersprings.push_back(spring);
joints.push_back(k);
Primitive* whisker2;
whisker2 = new Capsule(t1/2, l1);
whisker2->init(odeHandle, legmass/10, osgHandleJ);
osg::Matrix m2 = TRANSM(0,0,-l1/2)
* ROTM(M_PI/10, n,0,0)
* ROTM(M_PI/10, 0,1,0) * TRANSM(0,0,-l1/2) * m1;
whisker2->setPose(m2);
objects.push_back(whisker2);
// k = new FixedJoint(whisker, whisker2);
// k->init(odeHandle, osgHandleJ, false, 0);
k = new HingeJoint(whisker, whisker2, Pos(0,0,-l1/2) * m1,
Axis(0,1,0) * m1);
k->init(odeHandle, osgHandleJ, true, t1 * 2.1);
// servo used as a spring
spring = new HingeServo(k, -M_PI/6, M_PI/6, .05, 0.01,0);
whiskersprings.push_back(spring);
joints.push_back(k);
}
notifyOnChange("dummy"); // apply all parameters.
created=true;
};