void Motor::playFullHaptic( uint8_t library, uint8_t effect )
{
if( isPlaying() || isPlayingVib() )
{
Serial.println( F( "Already playing effect" ) );
return;
}
stopPlaying = false;
playing = true;
digitalWrite( SW_LRA_M, LOW ); // Select the grip
digitalWrite( SW_MOS_DRV, LOW ); // Select DRV+ and DRV-
digitalWrite( DRV_2605_EN, HIGH ); // Enable the DRV2605
uint8_t compensation, backEMF, feedback;
if( !getMotorCal( selectedMotor, &compensation, &backEMF, &feedback ) )
{
Serial.println( F("Not playing effect. Motor is not calibrated. Need to call autoCalibrate for this motor.") );
playing = false;
return;
}
if( currentMotor.LRA )
{
library = 6;
}
else
{
if( library < 1 )
library = 1;
else if( library > 5 )
library = 5;
}
if( effect < 1 )
effect = 1;
else if( effect > 124 )
effect = 124;
Serial.print( F("Playing library ") );
Serial.print( library, DEC );
Serial.print( F(" effect ") );
Serial.print( effect, DEC );
Serial.print( F(" on ") );
Serial.println( getMotorName() );
drv2605.playFullHaptic( library, effect, currentMotor.rated_duty, currentMotor.max_duty, compensation, backEMF, feedback );
digitalWrite( DRV_2605_EN, LOW ); // Disable the DRV2605 (low power mode)
digitalWrite( SW_MOS_DRV, HIGH ); // Select +3.3V and MOS-
digitalWrite( SW_LRA_M, LOW ); // Select M+ and M-
playing = false;
}
/** 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;
}
