void GazeboRosTest::UpdateChild()
{
// 状態アップデート
// Copies the commands from the mechanism joints into the gazebo joints.
ROS_ERROR("num=%d", this->joints_.size());
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
if (!this->joints_[i])
continue;
// double damping_coef = 100.0;
switch (this->joints_[i]->GetType())
{
case Joint::HINGE: {
// PD control
HingeJoint *hj = (HingeJoint*)this->joints_[i];
double current_velocity = hj->GetAngleRate();
double current_angle = hj->GetAngle();
double damping_force = dgain_ * (0 - current_velocity);
double diff_force = pgain_ * (cmd_[i] - current_angle);
double effort_command = diff_force + damping_force;
ROS_ERROR("c=%f,cmd=%f, f=%f, v=%f", current_angle, cmd_[i], effort_command, current_velocity);
// hj->SetTorque(effort_command);
hj->SetParam(dParamVel, effort_command);
break;
}
default:
abort();
}
}
}
//------------------------------------------------------------------------------
//!
int hingeJointVM( VMState* vm )
{
WorldContext* context = getContext(vm);
RigidEntity* entityA = 0;
RigidEntity* entityB = 0;
if( VM::geti( vm, -1, 1 ) )
{
entityA = (RigidEntity*)VM::toPtr( vm, -1 );
VM::pop( vm, 1 );
}
if( VM::geti( vm, -1, 2 ) )
{
entityB = (RigidEntity*)VM::toPtr( vm, -1 );
VM::pop( vm, 1 );
}
HingeJoint* joint = context->_world->createHingeJoint( entityA, entityB );
Vec3f anchor(0.0f);
VM::get( vm, -1, "anchor", anchor );
Vec3f axis(0.0f, 1.0f, 0.0f );
VM::get( vm, -1, "axis", axis );
joint->anchor( anchor );
joint->axis( axis );
return 0;
}
bool SimulationModel::addHingeJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
{
HingeJoint *hj = new HingeJoint();
const bool res = hj->initConstraint(*this, rbIndex1, rbIndex2, pos, axis);
if (res)
{
m_constraints.push_back(hj);
m_groupsInitialized = false;
}
return res;
}
/** 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;
};
XERCES_CPP_NAMESPACE_USE
#define ARRAY_SIZE(ARY) ( (int)(sizeof(ARY)/sizeof(ARY[0])) )
#ifdef CONF_DUMP
#define ENABLE_DUMP
#define ENABLE_DUMP1
#endif
#include "ParserDump.h"
void JointParser::parse(DOMNode &target, Eval &eval)
{
char *name = XMLUtils::getAttribute(target, "name");
DUMP1(("name %s\n", name));
m_transform.push();
DOMNode *p = target.getFirstChild();
while (p) {
char *s = XMLString::transcode(p->getNodeName());
// printf("%s\n", s);
if (strcmp(s, "type") == 0) {
char *type = XMLUtils::parseJointType(*p);
DUMP1(("type=\"%s\"\n", type));
Joint *j = 0;
if (strcmp(type, "fixed") == 0) {
j = new FixedJoint(name);
} else if (strcmp(type, "ball") == 0) {
j = new BallJoint(name);
} else if (strcmp(type, "hinge") == 0) {
j = new HingeJoint(name, Vector3d(0, 1, 0));
}
if (j) {
m_joint = j;
}
XMLString::release(&type);
/*
} else if (strcmp(s, "limitOrientation") == 0) {
double *v = XMLUtils::parseLimitOrientation(*p);
if (m_joint) {
delete m_joint;
}
m_joint = new HingeJoint(name, Vector3d(v[0], v[1], v[2]));
*/
} else if (strcmp(s, "translation") == 0) {
double * values = XMLUtils::parseTranslation(*p, eval);
DUMP1(("translation (%f, %f, %f)\n", values[0], values[1], values[2]));
m_transform.curr().push(Vector3d(values[0], values[1], values[2]));
} else if (strcmp(s, "rotation") == 0) {
double *values = XMLUtils::parseRotation(*p);
DUMP1(("rotation (%f, %f, %f, %f)\n", values[0], values[1], values[2], values[3]));
Rotation rot;
rot.setAxisAndAngle(values[0], values[1], values[2], values[3]);
m_transform.curr().push(rot);
} else if (strcmp(s, "axis") == 0) {
double *values = XMLUtils::parseAxis(*p);
DUMP1(("axis (%f, %f, %f)\n", values[0], values[1], values[2]));
if (m_joint) {
if (m_joint->type() == Joint::TYPE_HINGE) {
HingeJoint *hinge = static_cast<HingeJoint*>(m_joint);
hinge->setAxis(values[0], values[1], values[2]);
} else {
DUMP1(("not hinge type joint. axis ingored\n"));
}
} else {
DUMP1(("no joint type"));
}
} else if (strcmp(s, "children") == 0) {
DUMP1(("JOINT TRANSFORM TEST \n"));
TRANSFORM_TEST(m_transform.curr());
parseChildren(*p, eval);
}
p = p->getNextSibling();
XMLString::release(&s);
}
{
Transform &t = m_transform.curr();
const Vector3d &v = t.translation();
if (m_joint) {
m_f.addJoint(m_joint);
DUMP(("Joint : (%f, %f, %f)\n", v.x(), v.y(), v.z()));
m_joint->setAnchor(v.x(), v.y(), v.z());
}
}
m_transform.pop();
char *strs[] = { name,};
for (int i=0; i<ARRAY_SIZE(strs); i++) {
char *p = strs[i];
XMLString::release(&p);
}
}
/**
* This method updates the position and orientation of the grabbed
* object and all connected objects. If the grabbed object has
* no connections to other objects (via joints) the method simply
* applies the passed position and orientation to the grabbed object.
* Otherwise the calculation of the new transformation takes place in
* two steps:
* First the orientational difference between the grabbed object and
* and the passed orientation is calculated and applied to the object
* as torque. Then a simulation-step takes place, where the timestep
* is set to 0.5, what should leed to the half orientation correction.
* After the simulation step the objects velocities are set to zero.
* In the second step the difference of the objects position and the
* passed one are calculated and applied to the object as force. Like
* in step one a simulation-step with dt=0.5 takes place and the
* velocities are set to zero again.
* After the computation of the new transformations, the method iterates
* over all objects, which are grabbed or are linked with the grabbed
* object and copies their transformation from the ODE-representation
* to the Entity-members.
* At last the method calls the checkConstraints-method of all currently
* used joints, so that the joints can be activated or deactivated.
* @param position: position of the manipulating object
* @param orientation: orientation of the manipulating object
*/
TransformationData JointInteraction::update(TransformationData trans)
{
if (!isInitialized)
init();
gmtl::AxisAnglef AxAng;
gmtl::Vec3f diff;
dQuaternion quat;
userTrans.position = trans.position;
userTrans.orientation = trans.orientation;
if (!isGrabbing)
return identityTransformation();
int numJoints = dBodyGetNumJoints(grabbedObject->body);
if (!numJoints)
{ // Simple code for objects without Joints
convert(quat, userTrans.orientation);
dBodySetPosition(grabbedObject->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
dBodySetQuaternion(grabbedObject->body, quat);
}
else
{ // Code for object with Joints
TransformationData objectTrans;
gmtl::Quatf rotation;
gmtl::Vec3f force, torque;
const dReal* pos = dBodyGetPosition(grabbedObject->body);
const dReal* rot = dBodyGetQuaternion(grabbedObject->body);
convert(objectTrans.position, pos);
convert(objectTrans.orientation, rot);
// TODO: move this code to manipulationTerminationModel
// ungrab object if distance is too high
// diff = userTrans.position - objectTrans.position;
// if (gmtl::length(diff) > maxDist)
// {
// printf("JointInteraction::update(): distance %f too high!\n", gmtl::length(diff));
// ungrab();
// return identityTransformation();
// } // if
// STEP 1: apply Torque
// calculate Torque = angular offset from object orientation to current orientation
rotation = userTrans.orientation;
rotation *= invert(objectTrans.orientation);
gmtl::set(AxAng, rotation);
torque[0] = AxAng[0]*AxAng[1]; // rotAngle*rotX
torque[1] = AxAng[0]*AxAng[2]; // rotAngle*rotY
torque[2] = AxAng[0]*AxAng[3]; // rotAngle*rotZ
// TODO: move this code to manipulationTerminationModel
// ungrab object if angular distance is too high
// if (gmtl::length(torque) > maxRotDist)
// {
// printf("JointInteraction::update(): rotation %f too high!\n", gmtl::length(torque));
// ungrab();
// return identityTransformation();
// } // if
// create a Ball Joint to adjust orientation without changing the position
dJointID universalJoint = dJointCreateBall(world, 0);
dJointAttach(universalJoint, grabbedObject->body, 0);
dJointSetBallAnchor(universalJoint, pos[0], pos[1], pos[2]);
// printf("Torque = %f %f %f\n", torque[0], torque[1], torque[2]);
/***
* old way --> is very instable due to high timestep!!!
***
* dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
* // Half way to destination
* dWorldStep(world, 0.5);
**/
// take some simulation-steps for adjusting object's orientation
for (int i=0; i < stepsPerFrame; i++)
{
dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
dWorldStep(world, 1.0f/stepsPerFrame);
} // for
dJointDestroy(universalJoint);
// Reset speed and angular velocity to 0
dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
// STEP 2: apply force into wanted direction
// calculate Force = vector from object position to current position
force = (userTrans.position - objectTrans.position);
// printf("Force = %f %f %f\n", force[0], force[1], force[2]);
/***
* old way --> is very instable due to high timestep!!!
***
* dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
* // Half way to destination
* dWorldStep(world, 0.5);
**/
// take some simulation-steps for adjusting object's position
for (int i=0; i < stepsPerFrame; i++)
{
dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
dWorldStep(world, 1.0f/stepsPerFrame);
} // for
// Reset speed and angular velocity to 0
dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
}
// Get position and Orientation from simulated object
TransformationData newTransform = identityTransformation();
const dReal* pos = dBodyGetPosition(grabbedObject->body);
const dReal* rot = dBodyGetQuaternion(grabbedObject->body);
convert(newTransform.orientation, rot);
convert(newTransform.position, pos);
// Get position and Orientation from simulated objects
std::map<int, ODEObject*>::iterator it = linkedObjMap.begin();
ODEObject* object;
TransformationData linkedObjectTrans = identityTransformation();
while (it != linkedObjMap.end())
{
object = linkedObjMap[(*it).first];
if (object != grabbedObject)
{
linkedObjectTrans = object->entity->getWorldTransformation();
const dReal* pos1 = dBodyGetPosition(object->body);
const dReal* rot1 = dBodyGetQuaternion(object->body);
convert (linkedObjectTrans.orientation, rot1);
convert (linkedObjectTrans.position, pos1);
linkedObjPipes[it->first]->push_back(linkedObjectTrans);
// gmtl::set(AxAng, newRot);
/* object->entity->setTranslation(pos1[0], pos1[1], pos1[2]);
object->entity->setRotation(AxAng[1], AxAng[2], AxAng[3], AxAng[0]);*/
// assert(false);
// object->entity->setEnvironmentTransformation(linkedObjectTrans);
// object->entity->update();
} // if
++it;
} // while
// Update angles in HingeJoints and check joint-constraints
HingeJoint* hinge;
for (int i=0; i < (int)attachedJoints.size(); i++)
{
hinge = dynamic_cast<HingeJoint*>(attachedJoints[i]);
if (hinge)
hinge->checkAngles();
assert(attachedJoints[i]);
attachedJoints[i]->checkConstraints();
} // for
TransformationData result;
multiply(result, newTransform, deltaTrans);
TransformationPipe* pipe = linkedObjPipes[grabbedObject->entity->getEnvironmentBasedId()];
if (pipe)
pipe->push_back(result);
else
printd(WARNING, "JointInteraction::update(): Could not find Pipe for manipulated Object!\n");
return newTransform;
} // update
Joint* JointInteraction::xmlParseJoint(IrrXMLReader* xml, std::string& jointType)
{
bool finished = false;
Constraint* constraint = NULL;
Joint* joint = JointFactory::create(xml->getAttributeValue("type"));
jointType = xml->getAttributeValue("type");
joint->setWorld(world);
joint->setId(atoi(xml->getAttributeValue("id")));
joint->setJointInteractionClass(this);
if (xml->getAttributeValue("active"))
joint->setActive(atoi(xml->getAttributeValue("active")));
while (!finished && xml && xml->read())
{
switch (xml->getNodeType())
{
case EXN_ELEMENT:
if (!strcmp("entities", xml->getNodeName()))
{
xmlParseEntities(xml, joint);
} // else if
else if (!strcmp("anchor", xml->getNodeName()))
{
if (!jointType.compare("ball") || !jointType.compare("Ball") || !jointType.compare("BALL"))
{
BallJoint* ball = dynamic_cast<BallJoint*>(joint);
ball->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // if
else if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE"))
{
HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint);
hinge->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // else if
else if (!jointType.compare("universal") || !jointType.compare("Universal") || !jointType.compare("UNIVERSAL"))
{
UniversalJoint* universal = dynamic_cast<UniversalJoint*>(joint);
universal->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // else if
else if (!jointType.compare("hinge2") || !jointType.compare("Hinge2") || !jointType.compare("HINGE2"))
{
Hinge2Joint* hinge2 = dynamic_cast<Hinge2Joint*>(joint);
hinge2->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // else if
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s NEEDS NO anchor ELEMENT!!!\n", jointType.c_str());
} // else if
else if (!strcmp("axis", xml->getNodeName()))
{
if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE"))
{
HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint);
hinge->setAxis(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
} // if
else if (!jointType.compare("slider") || !jointType.compare("Slider") || !jointType.compare("SLIDER"))
{
SliderJoint* slider = dynamic_cast<SliderJoint*>(joint);
slider->setAxis(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
} // else if
else if (!jointType.compare("universal") || !jointType.compare("Universal") || !jointType.compare("UNIVERSAL")) {
UniversalJoint* universal = dynamic_cast<UniversalJoint*>(joint);
if (atoi(xml->getAttributeValue("index")) == 1)
universal->setAxis1(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else if (atoi(xml->getAttributeValue("index")) == 2)
universal->setAxis2(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: WRONG ATTRIBUTE FOUND IN axis ELEMENT of UniversalJoint!!!\n");
} // else if
else if (!jointType.compare("hinge2") || !jointType.compare("Hinge2") || !jointType.compare("HINGE2")) {
Hinge2Joint* hinge2 = dynamic_cast<Hinge2Joint*>(joint);
if (atoi(xml->getAttributeValue("index")) == 1)
hinge2->setAxis1(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else if (atoi(xml->getAttributeValue("index")) == 2)
hinge2->setAxis2(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: WRONG ATTRIBUTE FOUND IN axis ELEMENT of Hinge2Joint!!!\n");
} // else if
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s NEEDS NO axis ELEMENT!!!\n", jointType.c_str());
} // else if
else if (!strcmp("angles", xml->getNodeName()))
{
if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE"))
{
HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint);
hinge->setAngles(atof(xml->getAttributeValue("min")), atof(xml->getAttributeValue("max")));
} // if
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s HAS NO angles ELEMENT!!!\n", jointType.c_str());
} // else if
else if (!strcmp("positions", xml->getNodeName()))
{
SliderJoint* slider = dynamic_cast<SliderJoint*>(joint);
slider->setPositions(atof(xml->getAttributeValue("min")), atof(xml->getAttributeValue("max")));
} // else if
else if (!strcmp("activateIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::ACTIVATEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
else if (!strcmp("deactivateIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::DEACTIVATEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
else if (!strcmp("activeIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::ACTIVEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
else if (!strcmp("inactiveIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::INACTIVEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
break;
case EXN_ELEMENT_END:
if (!strcmp("joint", xml->getNodeName()))
finished = true;
break;
default:
break;
} // switch
} // while
assert(xml);
return joint;
} // xmlParseJoint
void GazeboMechanismControl::UpdateChild()
{
assert(joints_.size() == fake_state_->joint_states_.size());
assert(joints_.size() == joints_damping_.size());
//--------------------------------------------------
// Pushes out simulation state
//--------------------------------------------------
// Copies the state from the gazebo joints into the mechanism joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
if (!joints_[i])
continue;
fake_state_->joint_states_[i].applied_effort_ = fake_state_->joint_states_[i].commanded_effort_;
switch(joints_[i]->GetType())
{
case Joint::HINGE: {
HingeJoint *hj = (HingeJoint*)joints_[i];
fake_state_->joint_states_[i].position_ = hj->GetAngle();
fake_state_->joint_states_[i].velocity_ = hj->GetAngleRate();
break;
}
case Joint::SLIDER: {
SliderJoint *sj = (SliderJoint*)joints_[i];
fake_state_->joint_states_[i].position_ = sj->GetPosition();
fake_state_->joint_states_[i].velocity_ = sj->GetPositionRate();
break;
}
default:
abort();
}
}
// Reverses the transmissions to propagate the joint position into the actuators.
fake_state_->propagateStateBackwards();
//--------------------------------------------------
// Runs Mechanism Control
//--------------------------------------------------
hw_.current_time_ = Simulator::Instance()->GetSimTime();
try
{
mcn_.update();
}
catch (const char* c)
{
if (strcmp(c,"dividebyzero")==0)
std::cout << "WARNING:pid controller reports divide by zero error" << std::endl;
else
std::cout << "unknown const char* exception: " << c << std::endl;
}
//--------------------------------------------------
// Takes in actuation commands
//--------------------------------------------------
// Reverses the transmissions to propagate the actuator commands into the joints.
fake_state_->propagateEffortBackwards();
// Copies the commands from the mechanism joints into the gazebo joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
if (!joints_[i])
continue;
double damping_force;
double effort = fake_state_->joint_states_[i].commanded_effort_;
switch (joints_[i]->GetType())
{
case Joint::HINGE:
damping_force = joints_damping_[i] * ((HingeJoint*)joints_[i])->GetAngleRate();
((HingeJoint*)joints_[i])->SetTorque(effort - damping_force);
break;
case Joint::SLIDER:
damping_force = joints_damping_[i] * ((SliderJoint*)joints_[i])->GetPositionRate();
((SliderJoint*)joints_[i])->SetSliderForce(effort - damping_force);
break;
default:
abort();
}
}
}