bool Jr3Sensor::GetCartesianForcesTorques(float * MeasuredCartesianForcesTorques, int filter) {
Eigen::Vector3f forces(3), torques(3);
forces.setZero(3);
torques.setZero(3);
if (jr3DataRead(&jr3Data) != sizeof(jr3Data)) {
std::cout << "Jr3Sensor: Could not read from device!\n";
for (int i=0; i<6; i++) MeasuredCartesianForcesTorques[i] = 0.0;
return false;
}
else {
// 500Hz
if (filter == 1) {
forces(0) = float(jr3Data.f1.fx) * float(jr3Data.fullScale.fx) / 16384.0;
forces(1) = float(jr3Data.f1.fy) * float(jr3Data.fullScale.fy) / 16384.0;
forces(2) = float(jr3Data.f1.fz) * float(jr3Data.fullScale.fz) / 16384.0;
torques(0) = float(jr3Data.f1.mx) * float(jr3Data.fullScale.mx) / 10.0 / 16384.0;
torques(1) = float(jr3Data.f1.my) * float(jr3Data.fullScale.my) / 10.0 / 16384.0;
torques(2) = float(jr3Data.f1.mz) * float(jr3Data.fullScale.mz) / 10.0 / 16384.0;
}
// 125Hz
if (filter == 2) {
forces(0) = float(jr3Data.f2.fx) * float(jr3Data.fullScale.fx) / 16384.0;
forces(1) = float(jr3Data.f2.fy) * float(jr3Data.fullScale.fy) / 16384.0;
forces(2) = float(jr3Data.f2.fz) * float(jr3Data.fullScale.fz) / 16384.0;
torques(0) = float(jr3Data.f2.mx) * float(jr3Data.fullScale.mx) / 10.0 / 16384.0;
torques(1) = float(jr3Data.f2.my) * float(jr3Data.fullScale.my) / 10.0 / 16384.0;
torques(2) = float(jr3Data.f2.mz) * float(jr3Data.fullScale.mz) / 10.0 / 16384.0;
}
if (filter != 1 && filter != 2) return false;
// std::cout << "Fz [N]: " << float(jr3Data.f1.fz) * float(jr3Data.fullScale.fz) / 16384.0 << "\n";
// std::cout << "Units: " << jr3Data.units << "\n";
// std::cout << "jr3Data.fullScale.fx: " << jr3Data.fullScale.fx << "\n";
// std::cout << "jr3Data.fullScale.mx: " << jr3Data.fullScale.mx << "\n";
// std::cout << "jr3Data.fullScale.fy: " << jr3Data.fullScale.fy << "\n";
// std::cout << "jr3Data.fullScale.my: " << jr3Data.fullScale.my << "\n";
// std::cout << "jr3Data.fullScale.fz: " << jr3Data.fullScale.fz << "\n";
// std::cout << "jr3Data.fullScale.mz: " << jr3Data.fullScale.mz << "\n";
// std::cout << "jr3Data.f1.fx: " << jr3Data.f1.fx << "\n";
// std::cout << "jr3Data.f1.mx: " << jr3Data.f1.mx << "\n";
for (int i=0; i<3; i++) {
// Forces in N
MeasuredCartesianForcesTorques[i] = forces(i);
}
for (int i=0; i<3; i++) {
// Torques in Nm
MeasuredCartesianForcesTorques[i+3] = torques(i);
}
return true;
}
}
// Compute angular acceleration in body frame
// Parameters:
// I: inertia matrix
void angular_acceleration(float32_t *omegad , float32_t *inputs, float32_t* omega, float32_t* I, float32_t L, float32_t b, float32_t k)
{
float32_t tau[3];
float32_t tmp_3x1[3], cross[3];
float32_t II[9];
torques(tau, inputs, L, b, k);
Matrix_Inv_3x3(II, I);
Matrix_3x3_Multiply_Vector_3x1(tmp_3x1, I, omega);
Vector_Cross(cross, omega, tmp_3x1);
Vector_Subtract(tmp_3x1, tau, cross);
Matrix_3x3_Multiply_Vector_3x1(omegad, II, tmp_3x1);
}
/* Calculates a score using the current state of the board
*/
int eval_fn(int exhausted, int phase)
{
int score, t[2];
/* calculate score */
torques(t);
score = abs(t[0]*t[1]);
if (exhausted)
{
player = -1 * player;
return score;
// return player * inf;
}
else
player = -1 * player;
return score;
}
double* angleaccel(double omega[3], struct copter quad)
{/*Computes angular acceleration vector of quad*/
double tau[3];
double Iomega[3];
int i;
for (i=1;i<4;i++){
Iomega[i] = quad.I[i]*omega[i];
}
C = crossprod(omega,Iomega);
/*Compute Torques*/
torques(tau,quad);
ddtomega[1] = 1/quad.I[1]*(tau[1]);
return ddtomega;
}
iDynTree::Wrench simulateFTSensorFromKinematicState(const KDL::CoDyCo::UndirectedTree & icub_undirected_tree,
const KDL::JntArray & q,
const KDL::JntArray & dq,
const KDL::JntArray & ddq,
const KDL::Twist & base_velocity,
const KDL::Twist & base_acceleration,
const std::string ft_sensor_name,
const iDynTree::SensorsList & sensors_tree
)
{
// We can try to simulate the same sensor with the usual inverse dynamics
KDL::CoDyCo::Traversal traversal;
icub_undirected_tree.compute_traversal(traversal);
std::vector<KDL::Twist> v(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Twist> a(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> f(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> f_gi(icub_undirected_tree.getNrOfLinks());
std::vector<KDL::Wrench> f_ext(icub_undirected_tree.getNrOfLinks(),KDL::Wrench::Zero());
KDL::JntArray torques(icub_undirected_tree.getNrOfDOFs());
KDL::Wrench base_force;
KDL::CoDyCo::rneaKinematicLoop(icub_undirected_tree,
q,dq,ddq,traversal,base_velocity,base_acceleration,
v,a,f_gi);
KDL::CoDyCo::rneaDynamicLoop(icub_undirected_tree,q,traversal,f_gi,f_ext,f,torques,base_force);
unsigned int sensor_index;
sensors_tree.getSensorIndex(iDynTree::SIX_AXIS_FORCE_TORQUE,ft_sensor_name,sensor_index);
std::cout << ft_sensor_name << " has ft index " << sensor_index << std::endl;
iDynTree::SixAxisForceTorqueSensor * p_sens
= (iDynTree::SixAxisForceTorqueSensor *) sensors_tree.getSensor(iDynTree::SIX_AXIS_FORCE_TORQUE,sensor_index);
iDynTree::Wrench simulate_measurement;
KDL::CoDyCo::Regressors::simulateMeasurement_sixAxisFTSensor(traversal,f,p_sens,simulate_measurement);
return simulate_measurement;
}
/**
* In this example we will perform a classical inverse dynamics on tree structured robot.
* We will load the kinematic and dynamic parameters of the robot from an URDF file.
*/
int main(int argc, char** argv)
{
if(argc != 2)
{
std::cerr << "tree_inverse_dynamics example usage: ./tree_inverse_dynamics robot.urdf" << std::endl;
return EXIT_FAILURE;
}
std::string urdf_file_name = argv[1];
//We are using the kdl_format_io library for loading the URDF file to a KDL::Tree object, but if
//you use ROS you can use the kdl_parser from the robot_model package (but please note that
//there are some open issues with the robot_model kdl_parser : https://github.com/ros/robot_model/pull/66 )
KDL::Tree my_tree;
bool urdf_loaded_correctly = iDynTree::treeFromUrdfFile(urdf_file_name.c_str(),my_tree);
if( !urdf_loaded_correctly )
{
std::cerr << "Could not generate robot model and extract kdl tree" << std::endl;
return EXIT_FAILURE;
}
//We will now create a tree inverse dynamics solver
//This solver performs the classical inverse dynamics
//so there is a link called floating base that is the one for
//which the velocity and the acceleration is specified, and
//where unknown external wrench is assumed applied.
//The floating base is by default the base link of the URDF, but
//can be changed with the changeBase method
KDL::CoDyCo::TreeIdSolver_RNE rne_solver(my_tree);
//The input variables are :
// - Joint positions, velocities, accelerations.
int n_dof = rne_solver.getUndirectedTree().getNrOfDOFs();
KDL::JntArray q_j(n_dof), dq_j(n_dof), ddq_j(n_dof);
// - Floating base velocity and (proper) acceleration.
// The proper acceleration is the sum of the classical and gravitational acceleration,
// i.e. the acceleration that you get reading the output of linear accelerometer.
KDL::Twist v_base, a_base;
// - External wrenches applied to the link. This wrenches are expressed in the local reference frame of the link.
int n_links = rne_solver.getUndirectedTree().getNrOfLinks();
std::vector<KDL::Wrench> f_ext(n_links,KDL::Wrench::Zero());
//The output variables are :
// - Joint torques.
KDL::JntArray torques(n_dof);
// - The base residual wrench (mismatch between external wrenches in input and the model).
KDL::Wrench f_base;
//We populate the input variables with random data
srand(0);
for(int dof=0; dof < n_dof; dof++)
{
q_j(dof) = fRand(-10,10);
dq_j(dof) = fRand(-10,10);
ddq_j(dof) = fRand(-10,10);
}
//We fill also the linear part of the base velocity
//but please note that the dynamics is indipendent from it (Galilean relativity)
for(int i=0; i < 6; i++ )
{
v_base[i] = fRand(-10,10);
a_base[i] = fRand(-10,10);
}
//For setting the input wrenches, we first get the index for the link for which we want to set the external wrench.
//In this example we assume that we have measures for the input wrenches at the four end effectors (two hands, two legs)
//We use the names defined in REP 120 ( http://www.ros.org/reps/rep-0120.html ) for end effector frames
//but please change the names if you want to use a different set of links
int r_gripper_id = rne_solver.getUndirectedTree().getLink("r_gripper")->getLinkIndex();
int l_gripper_id = rne_solver.getUndirectedTree().getLink("l_gripper")->getLinkIndex();
int r_sole_id = rne_solver.getUndirectedTree().getLink("r_sole")->getLinkIndex();
int l_sole_id = rne_solver.getUndirectedTree().getLink("l_sole")->getLinkIndex();
for(int i=0; i < 6; i++ )
{
f_ext[r_gripper_id][i] = fRand(-10,10);
f_ext[l_gripper_id][i] = fRand(-10,10);
f_ext[r_sole_id][i] = fRand(-10,10);
f_ext[l_sole_id][i] = fRand(-10,10);
}
//Now that we have the input, we can compute the inverse dynamics
int inverse_dynamics_status = rne_solver.CartToJnt(q_j,dq_j,ddq_j,v_base,a_base,f_ext,torques,f_base);
if( inverse_dynamics_status != 0 )
{
std::cerr << "There was an error in the inverse dynamics computations" << std::endl;
return EXIT_FAILURE;
}
std::cout << "Torque computed successfully. " << std::endl;
std::cout << "Computed torques : " << std::endl;
for(int dof=0; dof < n_dof; dof++ )
{
std::cout << torques(dof) << " ";
}
std::cout << std::endl;
return EXIT_SUCCESS;
}
/* Recrusively realizes feasible sequences of nontipping moves and calls
* the evaulation function
*/
void alpha_better(int phase)
{
int best_v = -2 * inf, v = inf;
int i, j;
int t[2];
int best_move[2];
int *pw;
int tmp;
int p1wn = 0;
if (phase == 1)
{
if (player > 0)
pw = p1w;
else
pw = p2w;
for (j = 0; j < 12; j++)
{
if(pw[j])
pw[j] = 0;
else
continue;
for (i = 0; i < 31; i++)
{
if (board[i])
continue;
board[i] = player * (j + 1);
torques(t);
if(!tipped(t))
{
v = value((-1 * inf), inf, 1, 0, phase);
if (v > best_v)
{
best_v = v;
best_move[0] = i;
best_move[1] = (player > 0) ? (j + 1) : (-1 * (j + 1));
}
}
board[i] = 0;
}
pw[j] = 1;
}
}
if (phase == 2)
{
for (i = 0; i < 31; i++)
{
if (board[i] > 0)
p1wn += 1;
}
for (i = 0; i < 31; i++)
{
if (p1wn > 0 && board[i] < 0)
continue;
if (!board[i])
continue;
tmp = board[i];
board[i] = 0;
torques(t);
if (!tipped(t))
{
v = value((-1 * inf), inf, 1, 0, phase);
if (v > best_v)
{
best_v = v;
best_move[0] = i;
best_move[1] = tmp;
}
}
board[i] = tmp;
}
}
printf("%d %d %d\n", best_move[0], abs(best_move[1]), best_v);
}
int value(int alpha, int beta, int depth, int max, int phase)
{
int v = -inf, i, next = 0, j, *pw;
int t[2], wleft = 0, tmp, p1wn = 0;
for (i = 0; i < 12; i++)
{
wleft += p1w[i] + p2w[i];
}
if (!wleft)
{
phase += 1;
p1w[0] = 2;
}
player = -1 * player;
if (depth > d){
return eval_fn(0, phase);
}
if (phase == 1)
{
if (player > 0)
pw = p1w;
else
pw = p2w;
for (j = 0; j < 12; j++)
{
if (pw[j])
pw[j] = 0;
else
continue;
for (i = 0; i < 31; i++)
{
if (board[i])
continue;
board[i] = player * (j + 1);
torques(t);
if(!tipped(t))
{
next = 1;
if (max)
{
v = max(v, value(alpha, beta, depth + 1, 0, phase));
if (v >= beta)
{
player = -1 * player;
board[i] = 0;
return v;
}
alpha = max(alpha, v);
}
else
{
v = min(v, value(alpha, beta, depth + 1, 1, phase));
if (v <= alpha)
{
player = -1 * player;
board[i] = 0;
return v;
}
beta = min(beta, v);
}
}
board[i] = 0;
}
pw[j] = 1;
}
}
if (phase == 2)
{
for (i = 0; i < 31; i++)
{
if (board[i] > 0)
p1wn += 1;
}
for (i = 0; i < 31; i++)
{
if (p1wn > 0 && board[i] < 0)
continue;
if (!board[i])
continue;
tmp = board[i];
board[i] = 0;
torques(t);
if(!tipped(t))
{
next = 1;
if (max)
{
v = max(v, value(alpha, beta, depth + 1, 0, phase));
if (v >= beta)
{
player = -1 * player;
board[i] = tmp;
return v;
}
alpha = max(alpha, v);
}
else
{
v = min(v, value(alpha, beta, depth + 1, 1, phase));
if (v <= alpha)
{
player = -1 * player;
board[i] = tmp;
return v;
}
beta = min(beta, v);
}
}
board[i] = tmp;
}
}
if (!next)
return eval_fn(1, phase);
player = -1 * player;
return v;
}
/* Calculates a score using the current state of the board
*/
int eval_fn(int exhausted, int phase)
{
int score, t[2];
int i, j, stab1 = 0, stab2 = 0, unstab1 = 0, unstab2 = 0;
int p1l = 0, p1r = 0;
int num1 = 0, num2 = 0, count = 0;
int nearmid = 0;
int count_1 = 0, count_2 = 0;
int p1t[2];
int all_left = 0;
int all_right = 0;
int l_count = 0, r_count = 0;
FILE *file;
// file = fopen("scores.txt", "a");
// assert(file);
torques(t);
p1_torques(p1t);
// if(pplayer == -1)
// score = w1 * stab2 - w2 * count;
// if(pplayer == 1)
// score = w3 * abs(t[0] * t[1]) - w4 * abs(p1l - p1r) - w5 * abs(t[0] - t[1]) + w6 * nearmid;
// if(t[0] == 0 || t[1] == 0)
// return inf;
// else
// return 500 - min(abs(t[0]),abs(t[1]));
// if(t[0] == 0 || t[1] == 0)
// return inf;
// else
// return (1.0 / (float)(min(abs(t[0]),abs(t[1]))));
//return 500 - min(abs(t[0]), abs(t[1]));
// if(abs(t[0]) <= 4 || abs(t[1]) <= 4)
// {
// player = -1 * player;
// return player * inf;
// }
if (pplayer == -1)
{
if (exhausted)
{
player = -1 * player;
// fprintf(file, "%d\n", player * inf);
// fclose(file);
return player * inf;
}
else
{
l_count = 0;
r_count = 0;
for(i = 0; i < 32; i++)
{
if((i < 12) && (board[i] > 0))
{
l_count++;
}
else if((i > 15) && (board[i] > 0))
{
r_count++;
}
}
all_left = 0;
all_right = 0;
if((l_count == 0) && (r_count > 0))
{
all_right = 1;
}
else if((l_count > 0) && (r_count == 0))
{
all_left = 1;
}
if(all_left == 1 || all_right == 1)
{
score = 500 - abs(abs(last_t[0] - abs(t[0]))) - abs(abs(last_t[1] - abs(t[1])));
}
else
{
score = p1t[1] * p1t[0];
}
return score;
}
//fclose(file);
}
return 1000 * (wasd + 1) + 10 * abs(t[0]) + 10 * abs(t[1]);
}
