KinematicNodeDummy *generatePenNode(boost::property_tree::ptree::value_type &ptree, kinematics::NodeID nodeID, uint intID) const {
MotorID id = MotorID(0);
std::string name = "";
Millimeter translationX = 0 * millimeters;
Millimeter translationY = 0 * millimeters;
Millimeter translationZ = 0 * millimeters;
Degree alphaX = 0 * degrees;
Degree alphaY = 0 * degrees;
Degree alphaZ = 0 * degrees;
double defaultValue = 0.;
double minValue = 0.;
double maxValue = 0.;
double maxForce = 0.;
double maxSpeed = 0.;
assembleAttrs(ptree, id, name, translationX, translationY, translationZ, alphaX, alphaY, alphaZ, defaultValue, minValue, maxValue, maxForce, maxSpeed);
KinematicNodePen *node = new KinematicNodePen(nullptr, name, translationX, translationY, translationZ, alphaX, alphaY, alphaZ);
node->setID(nodeID);
node->setMaxValues({{id, maxValue}});
node->setMinValues({{id, minValue}});
node->setPreferedValues({{id, defaultValue}});
node->setValues({{id, defaultValue}});
node->setMotor2IntMap({{id, intID}});
return node;
}
osaODEServoMotor::osaODEServoMotor(dWorldID world,
dBodyID body1,
dBodyID body2,
const vctFixedSizeVector<double,3>& axis,
double vwmax,
double ftmax,
dJointType motortype ) :
vwmax( fabs( vwmax ) ),
ftmax( fabs( ftmax ) ) {
if( motortype == dJointTypeHinge ){
motorid = dJointCreateAMotor( world, 0 ); // create the motor
dJointAttach( MotorID(), body1, body2 ); // attach the joint
dJointSetAMotorMode( MotorID(), dAMotorUser ); // motor is in user mode
dJointSetAMotorNumAxes( MotorID(), 1 ); // only 1 axis
dJointSetAMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );
SetVelocity( 0.0 ); // idle the motor
}
if( motortype == dJointTypeSlider ){
motorid = dJointCreateLMotor( world, 0 ); // create the motor
dJointAttach( MotorID(), body1, body2 ); // attach the joint
dJointSetLMotorNumAxes( MotorID(), 1 ); // 1 axis
dJointSetLMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );
SetVelocity( 0.0 ); // idle the motor
}
}
KinematicNodeWheel *generatePropellerNode(boost::property_tree::ptree::value_type &ptree, kinematics::NodeID nodeID, uint intID) const {
MotorID id = MotorID(0);
std::string name = "";
Millimeter translationX = 0 * millimeters;
Millimeter translationY = 0 * millimeters;
Millimeter translationZ = 0 * millimeters;
Degree alphaX = 0 * degrees;
Degree alphaY = 0 * degrees;
Degree alphaZ = 0 * degrees;
double defaultValue = 0.;
double minValue = 0.;
double maxValue = 0.;
double maxForce = 0.;
double maxSpeed = 0.;
assembleAttrs(ptree, id, name, translationX, translationY, translationZ, alphaX, alphaY, alphaZ, defaultValue, minValue, maxValue, maxForce, maxSpeed);
double speedToForceFactor = 0.;
boost::optional<double> speedToForceFactorProp = ptree.second.get_optional<double>("<xmlattr>.speedtoforcefactor");
if (speedToForceFactorProp.is_initialized()) {
speedToForceFactor = speedToForceFactorProp.get();
}
KinematicNodePropeller *node = new KinematicNodePropeller(nullptr,
name,
maxForce,
maxSpeed * rounds_per_minute,
translationX,
translationY,
translationZ,
alphaX,
alphaY,
alphaZ,
speedToForceFactor);
node->setID(nodeID);
// following values must be in rad
node->setMaxValues({{id, Radian(maxValue * degrees).value()}});
node->setMinValues({{id, Radian(minValue * degrees).value()}});
node->setPreferedValues({{id, Radian(defaultValue * degrees).value()}});
node->setValues({{id, Radian(defaultValue * degrees).value()}});
node->setMotor2IntMap({{id, intID}});
return node;
}
void osaODEServoMotor::SetVelocity( double qd ){
if( dJointGetType( MotorID() ) == dJointTypeAMotor ){
dJointSetAMotorParam( MotorID(), dParamVel, qd );
dJointSetAMotorParam( MotorID(), dParamFMax, ftmax );
}
if( dJointGetType( MotorID() ) == dJointTypeLMotor ){
dJointSetLMotorParam( MotorID(), dParamVel, qd/1000.0 );
dJointSetLMotorParam( MotorID(), dParamFMax, ftmax );
}
}
KinematicNodeFixed *generateFixedNode(boost::property_tree::ptree::value_type &ptree, kinematics::NodeID nodeID) const {
MotorID id = MotorID(0);
std::string name = "";
Millimeter translationX = 0 * millimeters;
Millimeter translationY = 0 * millimeters;
Millimeter translationZ = 0 * millimeters;
Degree alphaX = 0 * degrees;
Degree alphaY = 0 * degrees;
Degree alphaZ = 0 * degrees;
double defaultValue = 0.;
double minValue = 0.;
double maxValue = 0.;
double maxForce = 0.;
double maxSpeed = 0.;
assembleAttrs(ptree, id, name, translationX, translationY, translationZ, alphaX, alphaY, alphaZ, defaultValue, minValue, maxValue, maxForce, maxSpeed);
KinematicNodeFixed *node = new KinematicNodeFixed(nullptr, name, translationX, translationY, translationZ, alphaX, alphaY, alphaZ);
node->setID(nodeID);
return node;
}
KinematicNodeWheel *generateWheelNode(boost::property_tree::ptree::value_type &ptree, kinematics::NodeID nodeID, uint intID) const {
MotorID id = MotorID(0);
std::string name = "";
Millimeter translationX = 0 * millimeters;
Millimeter translationY = 0 * millimeters;
Millimeter translationZ = 0 * millimeters;
Degree alphaX = 0 * degrees;
Degree alphaY = 0 * degrees;
Degree alphaZ = 0 * degrees;
double defaultValue = 0.;
double minValue = 0.;
double maxValue = 0.;
double maxForce = 0.;
double maxSpeed = 0.;
assembleAttrs(ptree, id, name, translationX, translationY, translationZ, alphaX, alphaY, alphaZ, defaultValue, minValue, maxValue, maxForce, maxSpeed);
KinematicNodeWheel* node = new KinematicNodeWheel(nullptr,
name,
maxForce,
maxSpeed * rounds_per_minute,
translationX,
translationY,
translationZ,
alphaX,
alphaY,
alphaZ);
node->setID(nodeID);
// following values must be in rad
node->setMaxValues({{id, Radian(maxValue * degrees).value()}});
node->setMinValues({{id, Radian(minValue * degrees).value()}});
node->setPreferedValues({{id, Radian(defaultValue * degrees).value()}});
node->setValues({{id, Radian(defaultValue * degrees).value()}});
node->setMotor2IntMap({{id, intID}});
return node;
}
