bool TorqueSensor::sense(const GlobalData& globaldata) {
int num = getSensorNumber();
if((signed)values.size()<num){
values.resize(num,0);
}
Pos t1;
Pos t2;
joint->getTorqueFeedback(t1,t2);
for(int i=0; i<num; i++){
const Pos& a = joint->getAxis(i);
// scalar product of axis and force gives the resulting torque
double p1 = t1 * a;
double p2 = t2 * a;
if(tau<1.0)
values[i] = values[i]*(1-tau) + (p1+p2)*(-tau/maxtorque);
else
values[i] = (p1+p2)/(-maxtorque);
}
// debugging:
// std::cout << "T1:"; t1.print();
// std::cout << "T2:"; t2.print();
// std::cout << "\t\tT1+T2:"; Pos(t1+t2).print();
// Pos f1;
// Pos f2;
// joint->getForceFeedback(f1,f2);
// std::cout << "F1:"; f1.print();
// std::cout << "F2:"; f2.print();
// std::cout << "\t\tF1+F2:"; Pos(f1+f2).print();
return true;
}
// read data from the sensor.
errlHndl_t SensorBase::readSensorData(uint8_t *& o_data)
{
// get sensor reading command only requires one byte of extra data,
// which will be the sensor number, the command will return between
// 3 and 5 bytes of data.
size_t len = 1;
// need to allocate some me to hold the sensor number this will be
// deleted by the IPMI transport layer
o_data = new uint8_t[len];
o_data[0] = getSensorNumber();
IPMI::completion_code cc = IPMI::CC_UNKBAD;
// o_data will hold the response when this returns
errlHndl_t l_err = sendrecv(IPMI::get_sensor_reading(), cc, len,
(uint8_t *&)o_data);
// if we didn't get an error back from the BT interface, but see a
// bad completion code from the BMC, process the CC to see if we
// need to create a PEL
if( (l_err == NULL ) && (cc != IPMI::CC_OK) )
{
l_err = processCompletionCode( cc );
}
return l_err;
};
std::list<sensor> CameraSensor::CameraSensor::get() const {
int num = getSensorNumber();
sensor* s = new sensor[num];
get(s,num);
std::list<sensor> result(s,s+num);
delete[] s;
return result;
}
int TorqueSensor::get(sensor* sensors, int length) const {
// we assume sense was called before.
int num = getSensorNumber();
assert(length >= num);
for(int i=0; i<num; i++){
sensors[i]= values[i];
}
return num;
}
//
// Helper function to send the data to the BMC using the correct interface
// protocol
//
errlHndl_t SensorBase::writeSensorData()
{
errlHndl_t l_err = NULL;
iv_msg->iv_sensor_number = getSensorNumber();
if( iv_msg->iv_sensor_number != INVALID_SENSOR )
{
IPMI::completion_code l_rc = IPMI::CC_UNKBAD;
// iv_msg is deleted by the destructor
l_err = sendSetSensorReading( iv_msg, l_rc);
if( l_err )
{
TRACFCOMP(g_trac_ipmi,"error returned from "
"sendSetSensorReading() for sensor number 0x%x",
getSensorNumber());
// return error log to caller
}
else
{
// check the completion code to see if we need to generate a
// PEL.
l_err = processCompletionCode( l_rc );
}
}
else
{
TRACFCOMP(g_trac_ipmi,"We were not able to find a sensor number in"
" the IPMI_SENSORS attribute for sensor_name=0x%x"
"for target with huid=0x%x, skipping call to "
"sendSetSensorReading()",
iv_name, TARGETING::get_huid( iv_target ));
assert(false);
}
return l_err;
};
void initNode (void) {
int i = 0;
uint8_t * ID_Sensor_List;
this.ID = SENSOR_NODE_ID;
this.num_sensors = getSensorNumber(); // dobbiamo modificare con la getSensorNumbers
this.authentication_key = SENSOR_NODE_KEY;
this.sensor = malloc(this.num_sensors*sizeof(Sensor));
ID_Sensor_List = malloc(this.num_sensors*sizeof(uint8_t));
getIDSensorList(ID_Sensor_List);
for (i = 0; i < this.num_sensors; i++) {
this.sensor[i].ID = ID_Sensor_List[i];
initSensor(&(this.sensor[i]));
}
}
int Hexabot::getSensorsIntern(sensor* sensors, int sensornumber){
assert(created);
assert(sensornumber == getSensorNumber());
// angle sensors
//
sensors[T1_as] = servos[T1_m] ? servos[T1_m]->get() : 0;
sensors[T2_as] = servos[T2_m] ? servos[T2_m]->get() : 0;
sensors[T3_as] = servos[T3_m] ? servos[T3_m]->get() : 0;
sensors[T4_as] = servos[T4_m] ? servos[T4_m]->get() : 0;
sensors[T5_as] = servos[T5_m] ? servos[T5_m]->get() : 0;
sensors[T6_as] = servos[T6_m] ? servos[T6_m]->get() : 0;
sensors[C1_as] = servos[C1_m] ? servos[C1_m]->get() : 0;
sensors[C2_as] = servos[C2_m] ? servos[C2_m]->get() : 0;
sensors[C3_as] = servos[C3_m] ? servos[C3_m]->get() : 0;
sensors[C4_as] = servos[C4_m] ? servos[C4_m]->get() : 0;
sensors[C5_as] = servos[C5_m] ? servos[C5_m]->get() : 0;
sensors[C6_as] = servos[C6_m] ? servos[C6_m]->get() : 0;
sensors[F1_as] = servos[F1_m] ? servos[F1_m]->get() : 0;
sensors[F2_as] = servos[F2_m] ? servos[F2_m]->get() : 0;
sensors[F3_as] = servos[F3_m] ? servos[F3_m]->get() : 0;
sensors[F4_as] = servos[F4_m] ? servos[F4_m]->get() : 0;
sensors[F5_as] = servos[F5_m] ? servos[F5_m]->get() : 0;
sensors[F6_as] = servos[F6_m] ? servos[F6_m]->get() : 0;
// Contact sensors
sensors[L1_fs] = legContactSensors[L1] ? legContactSensors[L1]->get() : 0;
sensors[L2_fs] = legContactSensors[L2] ? legContactSensors[L2]->get() : 0;
sensors[L3_fs] = legContactSensors[L3] ? legContactSensors[L3]->get() : 0;
sensors[L4_fs] = legContactSensors[L4] ? legContactSensors[L4]->get() : 0;
sensors[L5_fs] = legContactSensors[L5] ? legContactSensors[L5]->get() : 0;
sensors[L6_fs] = legContactSensors[L6] ? legContactSensors[L6]->get() : 0;
// Torque sensors
//motorTorqSensors[T0_m]->update();
sensors[T1_ts] = getTorqueData(motorTorqSensors[T1_m]);
sensors[T2_ts] = getTorqueData(motorTorqSensors[T2_m]);
sensors[T3_ts] = getTorqueData(motorTorqSensors[T3_m]);
sensors[T4_ts] = getTorqueData(motorTorqSensors[T4_m]);
sensors[T5_ts] = getTorqueData(motorTorqSensors[T5_m]);
sensors[T6_ts] = getTorqueData(motorTorqSensors[T6_m]);
sensors[C1_ts] = getTorqueData(motorTorqSensors[C1_m]);
sensors[C2_ts] = getTorqueData(motorTorqSensors[C2_m]);
sensors[C3_ts] = getTorqueData(motorTorqSensors[C3_m]);
sensors[C4_ts] = getTorqueData(motorTorqSensors[C4_m]);
sensors[C5_ts] = getTorqueData(motorTorqSensors[C5_m]);
sensors[C6_ts] = getTorqueData(motorTorqSensors[C6_m]);
sensors[F1_ts] = getTorqueData(motorTorqSensors[F1_m]);
sensors[F2_ts] = getTorqueData(motorTorqSensors[F2_m]);
sensors[F3_ts] = getTorqueData(motorTorqSensors[F3_m]);
sensors[F4_ts] = getTorqueData(motorTorqSensors[F4_m]);
sensors[F5_ts] = getTorqueData(motorTorqSensors[F5_m]);
sensors[F6_ts] = getTorqueData(motorTorqSensors[F6_m]);
//Pose sensor
osg::Vec3d a = this->convert_Quat_to_RollPitchYaw(this->getMainPrimitive()->getPose().getRotate());
sensors[POSE_r] = a[0]; // rad angle
sensors[POSE_p] = a[1];
sensors[POSE_y] = a[2];
//angular velocity of center of the robot
a = this->getMainPrimitive()->getAngularVel();
sensors[W_x] = a[0]; // angular vel vector
sensors[W_y] = a[1];
sensors[W_z] = a[2];
// grobal position of robot center
// (lower heagonal plate center position + upper hexagonal plate center position) / 2. is OK
a = (this->getMainPrimitive()->getPosition() + trunk.tUpTrans->getChildPose().getTrans()) * 1./2.;
sensors[GPOS_Rx] = a[0]; // grobal position of the robot
sensors[GPOS_Ry] = a[1];
sensors[GPOS_Rz] = a[2];
// grobal speed of robot center
//a = this->legs[L0].shoulder->getPose().getTrans();
a = this->getMainPrimitive()->getVel();
sensors[GSPD_Rx] = a[0]; // grobal spd of the robot
sensors[GSPD_Ry] = a[1];
sensors[GSPD_Rz] = a[2];
// grobal position of the COG
a = this->calc_COGPosition();
sensors[GPOS_COGx] = a[0]; // grobal cog of the robot
sensors[GPOS_COGy] = a[1];
sensors[GPOS_COGz] = a[2];
// grobal position of the leg toe
a = this->legs[L1].foot->getChildPose().getTrans();
sensors[GPOS_L1x] = a[0];
sensors[GPOS_L1y] = a[1];
sensors[GPOS_L1z] = a[2];
a = this->legs[L2].foot->getChildPose().getTrans();
sensors[GPOS_L2x] = a[0];
sensors[GPOS_L2y] = a[1];
sensors[GPOS_L2z] = a[2];
a = this->legs[L3].foot->getChildPose().getTrans();
sensors[GPOS_L3x] = a[0];
sensors[GPOS_L3y] = a[1];
sensors[GPOS_L3z] = a[2];
a = this->legs[L4].foot->getChildPose().getTrans();
sensors[GPOS_L4x] = a[0];
sensors[GPOS_L4y] = a[1];
sensors[GPOS_L4z] = a[2];
a = this->legs[L5].foot->getChildPose().getTrans();
sensors[GPOS_L5x] = a[0];
sensors[GPOS_L5y] = a[1];
sensors[GPOS_L5z] = a[2];
a = this->legs[L6].foot->getChildPose().getTrans();
sensors[GPOS_L6x] = a[0];
sensors[GPOS_L6y] = a[1];
sensors[GPOS_L6z] = a[2];
return HEXABOT_SENSOR_MAX;
}
/**
* @function: main(int argc, char **argv)
* @brief: Main function that loops reading the sensors and commanding
* Hubo's arm joints based on the poses of the foots
*/
int main(int argc, char **argv)
{
// check if no arguments given, if not report usage
if (argc < 2)
{
usage(std::cerr);
return 1;
}
// command line argument variables
bool left = false; // whether to set left arm angles
bool right = false; // whether to set right arm angles
bool print = false; // whether to print output or not
bool send = true; // whether to send commands or not
int leftSensorNumberDefault = 3; // default left foot sensor number
int rightSensorNumberDefault = 4; // default right foot sensor number
int leftSensorNumber = leftSensorNumberDefault; // left foot sensor number
int rightSensorNumber = rightSensorNumberDefault; // right foot sensor number
const char *teleopDeviceName = "liberty"; // name of teleop device
// local variables
LegVector lActualAngles, lLegAnglesNext, lLegAnglesCurrent;
LegVector rActualAngles, rLegAnglesNext, rLegAnglesCurrent;
Vector3d lFootOrigin, lSensorChange, lSensorOrigin, lSensorPos;
Vector3d rFootOrigin, rSensorChange, rSensorOrigin, rSensorPos;
Eigen::Matrix3d lRotInitial, rRotInitial, lSensorRot, rSensorRot;
Eigen::Isometry3d lFootInitialPose, lFootPoseCurrent, lFootPoseDesired;
Eigen::Isometry3d rFootInitialPose, rFootPoseCurrent, rFootPoseDesired;
LegVector speeds; speeds << 0.75, 0.75, 0.75, 0.75, 0.75, 0.75;
LegVector accels; accels << 0.40, 0.40, 0.40, 0.40, 0.40, 0.40;
double initialFootHeight = 0.1;
double dt, ptime;
int counter=0, counterMax=50;
bool updateRight;
// command line long options
const struct option long_options[] =
{
{ "left", optional_argument, 0, 'l' },
{ "right", optional_argument, 0, 'r' },
{ "nosend", no_argument, 0, 'n' },
{ "device", optional_argument, 0, 'd' },
{ "verbose", no_argument, 0, 'V' },
{ "help", no_argument, 0, 'H' },
{ 0, 0, 0, 0 },
};
// command line short options
const char* short_options = "l::r::nd::VH";
// command line option and option index number
int opt, option_index;
// loop through command line options and set values accordingly
while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 )
{
switch (opt)
{
case 'l': left = true; if(NULL != optarg) leftSensorNumber = getSensorNumber(optarg); break;
case 'r': right = true; if(NULL != optarg) rightSensorNumber = getSensorNumber(optarg); break;
case 'n': send = false; break;
case 'd': if(NULL != optarg) teleopDeviceName = getDeviceName(optarg); break;
case 'V': print = true; break;
case 'H': usage(std::cout); exit(0); break;
default: usage(std::cerr); exit(1); break;
}
}
// check to see if there are any invalid arguments on command line
if (optind < argc)
{
std::cerr << "Error: extra arguments on command line.\n\n";
usage(std::cerr);
exit(1);
}
// make sure the sensor numbers are not the same for both feet
if(leftSensorNumber == rightSensorNumber)
{
if(left == true && right == true)
{
std::cerr << "Error!\nSensor #'s are the same.\n"
<< "Default sensor #'s are \n\tLEFT: " << leftSensorNumberDefault
<< "\n\tRIGHT: " << rightSensorNumberDefault
<< ".\nPlease choose different sensor numbers.\n\n";
usage(std::cerr);
exit(1);
}
}
Hubo_Control hubo; // Create Hubo_Control object
// Hubo_Control hubo("teleop-arms"); // Create Hubo_Control object and daemonize program
Collision_Checker collisionChecker; // Create Collision_Checker object
// Create Teleop object
Teleop teleop(teleopDeviceName); // Create Teleop object
if (left == true) // if using the left arm
{
teleop.getPose( lSensorOrigin, lRotInitial, leftSensorNumber, true ); // get initial sensor pose
hubo.setLeftLegNomSpeeds( speeds ); // Set left arm nominal joint speeds
hubo.setLeftLegNomAcc( accels ); // Set left arm nominal joint accelerations
}
if (right == true) // if using the right arm
{
if(left == true) updateRight = false; else updateRight = true;
teleop.getPose( rSensorOrigin, rRotInitial, rightSensorNumber, updateRight ); // get initial sensor pose
hubo.setRightLegNomSpeeds( speeds ); // Set right arm nominal joint speeds
hubo.setRightLegNomAcc( accels ); // Set right arm nomimal joint accelerations
}
if(send == true) // if user wants to send commands
hubo.sendControls(); // send commands to the control daemon
if(left == true)
{
hubo.getLeftLegAngles(lLegAnglesNext);
hubo.huboLegFK(lFootInitialPose, lLegAnglesNext, LEFT); // Get left foot pose
lFootOrigin = lFootInitialPose.translation(); // Set relative zero for foot location
}
if(right == true)
{
hubo.getRightLegAngles(rLegAnglesNext);
hubo.huboLegFK(rFootInitialPose, rLegAnglesNext, RIGHT); // Get right foot pose
rFootOrigin = rFootInitialPose.translation(); // Set relative zero for foot location
}
// while the daemon is running
while(!daemon_sig_quit)
{
hubo.update(); // Get latest state info from Hubo
dt = hubo.getTime() - ptime; // compute change in time
ptime = hubo.getTime(); // get current time
if(dt>0 || (send == false && print == true)); // if new data was received over ach
{
if(left == true) // if using left arm
{
hubo.getLeftLegAngles(lLegAnglesCurrent); // get left arm joint angles
hubo.huboLegFK(lFootPoseCurrent, lLegAnglesCurrent, LEFT); // get left foot pose
teleop.getPose(lSensorPos, lSensorRot, leftSensorNumber, true); // get teleop data
lSensorChange = lSensorPos - lSensorOrigin; // compute teleop relative translation
lFootPoseDesired = Eigen::Matrix4d::Identity(); // create 4d identity matrix
lFootPoseDesired.translate(lSensorChange + lFootOrigin); // pretranslate relative translation
// make sure feet don't cross sagittal plane
if(lFootPoseDesired(1,3) - FOOT_WIDTH/2 < 0)
lFootPoseDesired(1,3) = FOOT_WIDTH/2;
lFootPoseDesired.rotate(lSensorRot); // add rotation to top-left of TF matrix
hubo.huboLegIK( lLegAnglesNext, lFootPoseDesired, lLegAnglesCurrent, LEFT ); // get joint angles for desired TF
hubo.setLeftLegAngles( lLegAnglesNext, false ); // set joint angles
hubo.getLeftLegAngles( lActualAngles ); // get current joint angles
}
if( right==true ) // if using right arm
{
if(left == true) updateRight = false; else updateRight = true;
hubo.getRightLegAngles(rLegAnglesCurrent); // get right arm joint angles
hubo.huboLegFK(rFootPoseCurrent, rLegAnglesCurrent, RIGHT); // get right foot pose
teleop.getPose(rSensorPos, rSensorRot, rightSensorNumber, updateRight); // get teleop data
rSensorChange = rSensorPos - rSensorOrigin; // compute teleop relative translation
rFootPoseDesired = Eigen::Matrix4d::Identity(); // create 4d identity matrix
rFootPoseDesired.translate(rSensorChange + rFootOrigin); // pretranslation by relative translation
// make sure feet don't cross sagittal plane
if(rFootPoseDesired(1,3) + FOOT_WIDTH/2 > 0)
rFootPoseDesired(1,3) = -FOOT_WIDTH/2;
rFootPoseDesired.rotate(rSensorRot); // add rotation to top-left corner of TF matrix
hubo.huboLegIK( rLegAnglesNext, rFootPoseDesired, rLegAnglesCurrent, RIGHT ); // get joint angles for desired TF
hubo.setRightLegAngles( rLegAnglesNext, false ); // set joint angles
hubo.getRightLegAngles( rActualAngles ); // get current joint angles
}
if( send == true ) // if user wants to send commands the control boards
hubo.sendControls(); // send reference commands set above
if( counter>=counterMax && print==true ) // if user wants output, print output every imax cycles
{
std::cout
<< "Teleop Position Lt(m): " << lSensorChange.transpose()
<< "\nTeleop Rotation Lt: \n" << lSensorRot
<< "\nTeleop Position Rt(m): " << rSensorChange.transpose()
<< "\nTeleop Rotation Rt: \n" << rSensorRot
<< "\nLeft Leg Actual Angles (rad): " << lActualAngles.transpose()
<< "\nLeft Leg Desired Angles(rad): " << lLegAnglesNext.transpose()
<< "\nRight Leg Actual Angles (rad): " << rActualAngles.transpose()
<< "\nRight Leg Desired Angles(rad): " << rLegAnglesNext.transpose()
<< "\nRight Foot Desired Pose: \n" << rFootPoseDesired.matrix()
<< "\nLeft Foot Desired Pose: \n" << lFootPoseDesired.matrix()
<< "\nRight foot torques(N-m)(Mx,My): " << hubo.getRightFootMx() << ", " << hubo.getRightFootMy()
<< "\nLeft foot torques(N-m)(Mx,My): " << hubo.getLeftFootMx() << ", " << hubo.getLeftFootMy()
<< std::endl;
}
if(counter>=counterMax) counter=0; counter++; // reset counter if it reaches counterMax
}
}
}
//
// Helper function to process completion codes returned from the BMC.
// If the completion code warrants a PEL the function will build and
// return an error log with the correct data captured.
//
errlHndl_t SensorBase::processCompletionCode( IPMI::completion_code i_rc )
{
errlHndl_t l_err = NULL;
IPMI::IPMIReasonCode l_reasonCode;
if( i_rc != IPMI::CC_OK )
{
// bad rc from the BMC
TRACFCOMP(g_trac_ipmi,"completion code 0x%x returned from the BMC"
" , creating error log", i_rc);
switch(i_rc)
{
case SENSOR::CC_SENSOR_READING_NOT_SETTABLE:
{
/* @errorlog tag
* @errortype ERRL_SEV_UNRECOVERABLE
* @moduleid IPMI::MOD_IPMISENSOR
* @reasoncode IPMI::RC_SENSOR_NOT_SETTABLE
* @userdata1 BMC IPMI Completion code.
* @userdata2 bytes [0-3]sensor number
* bytes [4-7]HUID of target.
* @devdesc Set sensor reading command failed.
*/
l_reasonCode = IPMI::RC_SENSOR_NOT_SETTABLE;
TRACFCOMP(g_trac_ipmi,"Attempt to change sensor reading or"
"set/clear status bits that are not settable"
" via this command");
break;
}
case SENSOR::CC_EVENT_DATA_BYTES_NOT_SETTABLE:
{
/* @errorlog tag
* @errortype ERRL_SEV_UNRECOVERABLE
* @moduleid IPMI::MOD_IPMISENSOR
* @reasoncode IPMI::RC_EVENT_DATA_NOT_SETTABLE
* @userdata1 BMC IPMI Completion code.
* @userdata2 bytes[0-3]sensor number
* bytes[4-7]HUID of target.
* @devdesc Set sensor reading command failed.
*/
l_reasonCode = IPMI::RC_EVENT_DATA_NOT_SETTABLE;
TRACFCOMP(g_trac_ipmi,"Attempted to set event data bytes but"
"setting event data bytes is not supported for"
" this sensor");
break;
}
case IPMI::CC_CMDSENSOR:
{
/* @errorlog tag
* @errortype ERRL_SEV_UNRECOVERABLE
* @moduleid IPMI::MOD_IPMISENSOR
* @reasoncode IPMI::RC_INVALID_SENSOR_CMD
* @userdata1 BMC IPMI Completion code.
* @userdata2 bytes [0-3]sensor number
* bytes [4-7]HUID of target.
* @devdesc Command not valid for this sensor.
*/
l_reasonCode = IPMI::RC_INVALID_SENSOR_CMD;
TRACFCOMP(g_trac_ipmi,"Command not valid for this sensor");
break;
}
case IPMI::CC_BADSENSOR:
{
/* @errorlog tag
* @errortype ERRL_SEV_UNRECOVERABLE
* @moduleid IPMI::MOD_IPMISENSOR
* @reasoncode IPMI::RC_SENSOR_NOT_PRESENT
* @userdata1 BMC IPMI Completion code.
* @userdata2 bytes [0-3]sensor number
* bytes [4-7]HUID of target.
* @devdesc Requested sensor is not present.
*/
l_reasonCode = IPMI::RC_SENSOR_NOT_PRESENT;
TRACFCOMP(g_trac_ipmi,"Requested sensor not present");
break;
}
default:
{
// lump everything else into a general failure for
// now.
/* @errorlog tag
* @errortype ERRL_SEV_UNRECOVERABLE
* @moduleid IPMI::MOD_IPMISENSOR
* @reasoncode IPMI::RC_SET_SENSOR_FAILURE
* @userdata1 BMC IPMI Completion code.
* @userdata2 bytes [0-3]sensor number
* bytes [4-7]HUID of target.
* @devdesc Set sensor reading command failed.
*/
TRACFCOMP(g_trac_ipmi,"Set sensor reading command failed");
l_reasonCode = IPMI::RC_SET_SENSOR_FAILURE;
break;
}
}
// shift the sensor number into to bytes 0-3 and then
// or in the HUID to bytes 4-7
uint64_t userdata2 = getSensorNumber();
userdata2 = (userdata2 << 32) | TARGETING::get_huid(iv_target);
l_err = new ERRORLOG::ErrlEntry(
ERRORLOG::ERRL_SEV_UNRECOVERABLE,
IPMI::MOD_IPMISENSOR,
l_reasonCode,
i_rc, userdata2, true);
l_err->collectTrace(IPMI_COMP_NAME);
}
return l_err;
}
