demo_kinematic_intermed_cyl.cpp
Go to the documentation of this file.
00001 
00002 #include <sys/time.h>
00003 
00004 #include "ros/ros.h"
00005 #include "hrl_haptic_manipulation_in_clutter_msgs/SkinContact.h"
00006 #include "hrl_haptic_manipulation_in_clutter_msgs/BodyDraw.h"
00007 #include "hrl_haptic_manipulation_in_clutter_msgs/TaxelArray.h"
00008 #include "hrl_haptic_manipulation_in_clutter_msgs/MechanicalImpedanceParams.h"
00009 #include "hrl_msgs/FloatArrayBare.h"
00010 #include "std_msgs/String.h"
00011 //#include "roslib/Clock.h"
00012 #include "rosgraph_msgs/Clock.h"
00013 #include <cmath>
00014 #include <vector>
00015 #include <string>
00016 #include <iostream>
00017 #include <set>
00018 #include <algorithm>
00019 #include <functional>
00020 #include <ode/ode.h>
00021 #include <sstream>
00022 //#include <drawstuff/drawstuff.h>
00023 //#include "texturepath.h"
00024 
00025 #include <tf/transform_broadcaster.h>
00026 
00027 #ifdef dDOUBLE
00028 #define MAX_CONTACTS 20          // maximum number of contact points per body
00029 #define MAX_FEEDBACKNUM 100
00030 #define NUM_OBST 1000
00031 #define PI 3.14159265
00032 #endif
00033 
00034 
00035 
00036 struct MyFeedback {
00037     dJointFeedback fb;
00038 };
00039 
00040 static MyFeedback feedbacks[MAX_FEEDBACKNUM];
00041 static MyFeedback frict_feedbacks[NUM_OBST];
00042 static int fbnum=0;
00043 static int force_group=0;
00044 double max_friction = 2;
00045 double max_tor_friction = 0.5;
00046 int num_used_movable = NUM_OBST;
00047 int num_used_fixed = NUM_OBST;
00048 int num_used_compliant = NUM_OBST;
00049 
00050 using namespace std;
00051 std::vector<double> q(3, 0);
00052 std::vector<double> q_dot(3, 0);
00053 std::vector<std::string> names(MAX_FEEDBACKNUM);
00054 std::vector<int> force_grouping;
00055 std::vector<int> force_sign;
00056 std::vector<double> pt_x;
00057 std::vector<double> pt_y;
00058 std::vector<double> pt_z;
00059 std::vector<double> jep(3, 0);
00060 std::vector<double> k_p(3, 0);
00061 std::vector<double> k_d(3, 0);
00062 std::vector<double> torques(3, 0);
00063 
00064 std::vector<double> mobile_base_k_p(3, 0);
00065 std::vector<double> mobile_base_k_d(3, 0);
00066 std::vector<double> mobile_base_ep(3, 0); // x, y, theta
00067 std::vector<double> mobile_base_generalized_forces(3, 0); // Fx, Fy, Tz
00068 
00069 //Temporary variables that should be cleaned up with TF at some point///////
00070 std::vector<double> x_c;
00071 std::vector<double> y_c;
00072 std::vector<double> z_c;
00073 std::vector<double> x_n;
00074 std::vector<double> y_n;
00075 std::vector<double> z_n;
00076 float torso_half_width = 0.196; // ctorsoy
00077 float torso_half_width_side = 0;
00078 float upper_arm_length = 0.334; // ruparmz
00079 float upper_arm_width = 0;
00080 float forearm_length = 0;//0.288; //+ 0.115; // rlowarmz + rhandz
00081 float forearm_width = 0;
00082 
00084 
00085 //std::vector<dBodyID> link_ids(3);
00086 dBodyID link_ids[3];
00087 dBody obstacles[NUM_OBST];
00088 dBody compliant_obstacles[NUM_OBST];
00089 double obst_home[NUM_OBST][3];
00090 double obst_stiffness[NUM_OBST];
00091 double obst_damping[NUM_OBST];
00092 dBodyID fixed_obst_ids[NUM_OBST];
00093 dBody fixed_obstacles[NUM_OBST];
00094 dJointID plane2d_joint_ids[NUM_OBST];
00095 dJointID compliant_plane2d_joint_ids[NUM_OBST];
00096 dJointID fixed_joint_ids[NUM_OBST];
00097 
00098 dJointGroup joints;
00099 dBody *env;
00100 dWorld *world;
00101 dSpace *space;
00102 
00103 
00104 // body, geometry, joint
00105 // (body is for dynamics, geometry is for collision detection) -- This
00106 // is Advait, so the documentation might be incorrect.
00107 
00108 dBody *link1;
00109 dBox *g_link1_box;
00110 dCapsule *g_link1_cap;
00111 dHingeJoint *hinge1;
00112 
00113 dBody *link2;
00114 dBox *g_link2_box;
00115 dCapsule *g_link2_cap;
00116 dHingeJoint *hinge2;
00117 
00118 dBody *link3;
00119 dBox *g_link3_box;
00120 dCapsule *g_link3_cap;
00121 dHingeJoint *hinge3;
00122 
00123 int include_mobile_base;
00124 
00125 dBody *body_mobile_base;
00126 dBox *geom_mobile_base;
00127 dJointID mobile_base_plane2d_jt_id;
00128 
00129 
00130 hrl_haptic_manipulation_in_clutter_msgs::SkinContact skin;
00131 hrl_haptic_manipulation_in_clutter_msgs::BodyDraw draw;
00132 hrl_haptic_manipulation_in_clutter_msgs::TaxelArray taxel;
00133 hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams impedance_params;
00134 
00135 void inner_torque_loop();
00136 
00137 
00138 void base_ep_cb(const hrl_msgs::FloatArrayBare msg)
00139 {
00140     mobile_base_ep = msg.data;
00141 }
00142 
00143 void jep_cb(const hrl_msgs::FloatArrayBare msg)
00144 {
00145     jep = msg.data;
00146     //jep[0] = msg.data[0];     
00147     //jep[1] = msg.data[1];     
00148     //jep[2] = msg.data[2];      
00149 }
00150 
00151 void ROSCallback_impedance(const hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams msg)
00152 {
00153     k_p = msg.k_p.data;
00154     k_d = msg.k_d.data;
00155 }
00156 
00157 
00158 
00159 static void nearCallback (void *data, dGeomID o1, dGeomID o2)
00160 {
00161     int i;
00162     // if (o1->body && o2->body) return;                                                                                 
00163     //    // if contact with ground, then ignore.
00164 
00165     // exit without doing anything if the two bodies are connected by a joint                                            
00166     dBodyID b1 = dGeomGetBody(o1);
00167     dBodyID b2 = dGeomGetBody(o2);
00168 
00169     if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
00170 
00171     bool b1_is_link = false;
00172     bool b2_is_link = false;
00173     for (int i=0; i<3; i++)
00174     {
00175         if (link_ids[i] == b1)
00176             b1_is_link = true;
00177         if (link_ids[i] == b2)
00178             b2_is_link = true;
00179     }
00180 
00181     if (include_mobile_base != 0)
00182     {
00183         if (body_mobile_base->id() == b1 || body_mobile_base->id() == b2)
00184         {
00185             // ignoring all contact with mobile base.
00186             return;
00187         }
00188     }
00189 
00190     if (b1_is_link && b2_is_link)
00191         // ignoring link-link contact.
00192         return;
00193 
00194     bool arm_contact = false;
00195     stringstream ss;
00196     bool is_b1 = false;
00197     for (int i=0; i<3; i++)
00198         if (link_ids[i] == b1 || link_ids[i] == b2)
00199         {
00200             arm_contact = true;
00201             ss <<"link"<<i+1;
00202             if (link_ids[i] == b1)
00203                 is_b1 = true;
00204             else
00205                 is_b1 = false;
00206         }
00207 
00208     dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box                                         
00209     int numc = dCollide (o1, o2, MAX_CONTACTS, &(contact[0].geom), sizeof(dContact));
00210     if (numc > 0)
00211     {
00212         if (arm_contact == false)
00213         {
00214             for (int i=0; i<numc; i++)
00215             {
00216                 contact[i].surface.mode = dContactSoftCFM | dContactApprox1 | dContactSoftERP;
00217                 contact[i].surface.mu = 0.2;
00218                 contact[i].surface.mu2 = 0.2;
00219                 //contact[i].surface.bounce = 0.01;
00220                 contact[i].surface.soft_cfm = 0.04;
00221                 contact[i].surface.soft_erp = 0.96;
00222 
00223                 dJointID c = dJointCreateContact (*world,joints.id(), &contact[i]);
00224                 dJointAttach (c, dGeomGetBody(contact[i].geom.g1),
00225                         dGeomGetBody(contact[i].geom.g2));
00226             }
00227         }
00228         else
00229         {
00230             dVector3 contact_loc = {0, 0, 0};
00231             skin.link_names.push_back(ss.str());
00232 
00233             for (i=0; i<numc; i++) 
00234             {
00235                 contact[i].surface.mode = dContactSoftCFM | dContactApprox1 | dContactSoftERP;
00236                 contact[i].surface.mu = 0.4;
00237                 contact[i].surface.mu2 = 0.4;
00238                 //contact[i].surface.bounce = 0.01;
00239                 contact[i].surface.soft_cfm = 0.04;
00240                 contact[i].surface.soft_erp = 0.96;
00241 
00242                 contact_loc[0] = contact_loc[0]+contact[i].geom.pos[0];
00243                 contact_loc[1] = contact_loc[1]+contact[i].geom.pos[1];
00244                 contact_loc[2] = contact_loc[2]+contact[i].geom.pos[2];
00245 
00246                 pt_x.push_back(contact[i].geom.pos[0]);
00247                 pt_y.push_back(contact[i].geom.pos[1]);
00248                 pt_z.push_back(contact[i].geom.pos[2]);
00249 
00250                 dJointID c = dJointCreateContact (*world,joints.id(),&contact[i]);
00251                 dJointAttach (c,b1,b2);
00252 
00253                 if (fbnum<MAX_FEEDBACKNUM && (ss.str()=="link1"||ss.str()=="link2"||ss.str()=="link3") )
00254                 {
00255                     dJointSetFeedback (c,&feedbacks[fbnum++].fb);
00256                     force_grouping.push_back(force_group);
00257                     if (is_b1 == true)
00258                         force_sign.push_back(1);
00259                     else
00260                         force_sign.push_back(-1);
00261                 }
00262 
00263             }
00264 
00265             contact_loc[0] = contact_loc[0]/numc;
00266             contact_loc[1] = contact_loc[1]/numc;
00267             contact_loc[2] = contact_loc[2]/numc;
00268             geometry_msgs::Point con_pt;
00269             con_pt.x = contact_loc[0];
00270             con_pt.y = contact_loc[1];
00271             con_pt.z = contact_loc[2];
00272 
00273             skin.locations.push_back(con_pt);
00274             hrl_msgs::FloatArrayBare pts_x_ar;
00275             pts_x_ar.data = pt_x;
00276             skin.pts_x.push_back(pts_x_ar);
00277             hrl_msgs::FloatArrayBare pts_y_ar;
00278             pts_y_ar.data = pt_y;
00279             skin.pts_y.push_back(pts_y_ar);
00280             hrl_msgs::FloatArrayBare pts_z_ar;
00281             pts_z_ar.data = pt_z;
00282             skin.pts_z.push_back(pts_z_ar);
00283             pt_x.clear();
00284             pt_y.clear();
00285             pt_z.clear();
00286             force_group += 1;
00287         }
00288     }
00289 }
00290 
00291 
00292 void go_initial_position(ros::NodeHandle n)
00293 {
00294     XmlRpc::XmlRpcValue init_angle;
00295     while (n.getParam("/m3/software_testbed/joints/init_angle", init_angle) == false)
00296         sleep(0.1);
00297 
00298     int num_links;
00299     while (n.getParam("/m3/software_testbed/linkage/num_links", num_links) == false)
00300         sleep(0.1);
00301 
00302     // moving mobile base to 0, 0, 0
00303     mobile_base_ep[0] = 0;
00304     mobile_base_ep[1] = 0;
00305     mobile_base_ep[2] = 0;
00306 
00307     //implicit assumption that number of links == number of jts
00308     for (int ii = 0; ii < num_links ; ii++)
00309     {
00310         jep[ii] = (double)init_angle[ii];
00311     }
00312 
00313     float error = 1.;
00314     float error_thresh = 0.005;
00315     const dReal timestep = 0.0005;
00316     int torque_step(0);
00317 
00318     while (error > error_thresh)
00319     {
00320         world->step(timestep);
00321         torque_step++;
00322         inner_torque_loop();
00323         hinge1->addTorque(torques[0]);
00324         hinge2->addTorque(torques[1]);  
00325         hinge3->addTorque(torques[2]);          
00326         float e1 = q[0]-jep[0];
00327         float e2 = q[1]-jep[1];
00328         float e3 = q[2]-jep[2];
00329         error = e1*e1 + e2*e2 + e3*e3;
00330     }
00331 }
00332 
00333 
00334 void create_robot(ros::NodeHandle n)
00335 {
00336     //setting intial impedance parameters
00337     k_p[0] = 30;
00338     k_p[1] = 20;
00339     k_p[2] = 15;
00340     k_d[0] = 15;
00341     k_d[1] = 10;
00342     k_d[2] = 8;
00343 
00344     mobile_base_k_p[0] = 5000;
00345     mobile_base_k_p[1] = 5000;
00346     mobile_base_k_d[0] = 2500;
00347     mobile_base_k_d[1] = 2500;
00348 
00349     XmlRpc::XmlRpcValue link_dimensions;
00350     while (n.getParam("/m3/software_testbed/linkage/dimensions", link_dimensions) == false)
00351         sleep(0.1);
00352     XmlRpc::XmlRpcValue link_pos;
00353     while (n.getParam("/m3/software_testbed/linkage/positions", link_pos) == false)
00354         sleep(0.1);
00355     XmlRpc::XmlRpcValue link_shapes;
00356     while (n.getParam("/m3/software_testbed/linkage/shapes", link_shapes) == false)
00357         sleep(0.1);
00358     int num_links;
00359     while (n.getParam("/m3/software_testbed/linkage/num_links", num_links) == false)
00360         sleep(0.1);
00361     XmlRpc::XmlRpcValue link_masses;
00362     while (n.getParam("/m3/software_testbed/linkage/mass", link_masses) == false)
00363         sleep(0.1);
00364     XmlRpc::XmlRpcValue jt_min;
00365     while (n.getParam("/m3/software_testbed/joints/min", jt_min) == false)
00366         sleep(0.1);
00367     XmlRpc::XmlRpcValue jt_max;
00368     while (n.getParam("/m3/software_testbed/joints/max", jt_max) == false)
00369         sleep(0.1);
00370     XmlRpc::XmlRpcValue jt_axes;
00371     while (n.getParam("/m3/software_testbed/joints/axes", jt_axes) == false)
00372         sleep(0.1);
00373     XmlRpc::XmlRpcValue jt_anchor;
00374     while (n.getParam("/m3/software_testbed/joints/anchor", jt_anchor) == false)
00375         sleep(0.1);
00376 
00377 
00378     double torso_ulim = (double)jt_max[0];
00379     double torso_llim = (double)jt_min[0];
00380 
00381     double shoulder_ulim = (double)jt_max[1];
00382     double shoulder_llim = (double)jt_min[1];
00383 
00384     double elbow_ulim = (double)jt_max[2];
00385     double elbow_llim = (double)jt_min[2];
00386     //float elbow_llim = 0 * rad_per_deg;
00387 
00388     //double torso_mass = (double)link_masses[0]; // ctorsomass
00389     //double upper_arm_mass = (double)link_masses[1];// ruparmmass
00390     //double forearm_mass = (double)link_masses[2];//+0.49; // rlowarmmass + rhandmass
00391 
00392     dMatrix3 cyl_rotate = {1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0};
00393         
00394     //link1
00395     string link1_shape;
00396     link1_shape = (string)link_shapes[0];
00397     link1 = new dBody(*world);
00398     dMass mass_l1;
00399 
00400     if (link1_shape == "cube")
00401       {
00402         dMassSetBoxTotal(&mass_l1, (double)link_masses[0], (double)link_dimensions[0][0], (double)link_dimensions[0][1], (double)link_dimensions[0][2]);
00403         link1->setMass(mass_l1);
00404         g_link1_box = new dBox(*space, (double)link_dimensions[0][0], (double)link_dimensions[0][1], (double)link_dimensions[0][2]);
00405         g_link1_box->setBody(*link1);
00406       }
00407     else if (link1_shape == "capsule")
00408       {
00409         dMassSetCapsuleTotal(&mass_l1, (double)link_masses[0], 3, (double)link_dimensions[0][0]/2.0, (double)link_dimensions[0][1]);
00410         link1->setMass(mass_l1);
00411         g_link1_cap = new dCapsule(*space, (double)link_dimensions[0][0]/2.0, (double)link_dimensions[0][1]);
00412         g_link1_cap->setBody(*link1);
00413         link1->setRotation(cyl_rotate);
00414       }
00415     else
00416       {
00417         std::cerr<<"The shape type :"<<link1_shape<<"\n of the given robot link is not defined\n";
00418         assert(false);
00419       }
00420 
00421     link1->setPosition((double)link_pos[0][0], (double)link_pos[0][1], (double)link_pos[0][2]); 
00422     link_ids[0] = link1->id();
00423     torso_half_width = (double)link_dimensions[0][1];
00424     torso_half_width_side = (double)link_dimensions[0][0];
00425 
00426 
00427     //link2
00428     string link2_shape;
00429     link2_shape = (string)link_shapes[1];
00430     link2 = new dBody(*world);
00431     dMass mass_l2;
00432     if (link2_shape == "cube")
00433       {
00434         dMassSetBoxTotal(&mass_l2, (double)link_masses[1], (double)link_dimensions[1][0], (double)link_dimensions[1][1], (double)link_dimensions[1][2]);
00435         link2->setMass(mass_l2);
00436         g_link2_box = new dBox(*space, (double)link_dimensions[1][0], (double)link_dimensions[1][1], (double)link_dimensions[1][2]);
00437         g_link2_box->setBody(*link2);
00438       }
00439     else if (link2_shape == "capsule")
00440       {
00441         dMassSetCapsuleTotal(&mass_l2, (double)link_masses[1], 3, (double)link_dimensions[1][0]/2.0, (double)link_dimensions[1][1]);
00442         link2->setMass(mass_l2);
00443         g_link2_cap = new dCapsule(*space, (double)link_dimensions[1][0]/2.0, (double)link_dimensions[1][1]);
00444         g_link2_cap->setBody(*link2);
00445         link2->setRotation(cyl_rotate);
00446       }
00447     else
00448       {
00449         std::cerr<<"The shape type :"<<link2_shape<<"\n of the given robot link is not defined\n";
00450         assert(false);
00451       }
00452 
00453     link2->setPosition((double)link_pos[1][0], (double)link_pos[1][1], (double)link_pos[1][2]); 
00454     link_ids[1] = link2->id();
00455     upper_arm_length = (double)link_dimensions[1][1];
00456     upper_arm_width = (double)link_dimensions[1][0];
00457 
00458     //link3
00459     string link3_shape;
00460     link3_shape = (string)link_shapes[2];
00461     link3 = new dBody(*world);
00462     dMass mass_l3;
00463     if (link3_shape == "cube")
00464       {
00465         dMassSetBoxTotal(&mass_l3, (double)link_masses[2], (double)link_dimensions[2][0], (double)link_dimensions[2][1], (double)link_dimensions[2][2]);
00466         link3->setMass(mass_l3);
00467         g_link3_box = new dBox(*space, (double)link_dimensions[2][0], (double)link_dimensions[2][1], (double)link_dimensions[2][2]);
00468         g_link3_box->setBody(*link3);
00469       }
00470     else if (link3_shape == "capsule")
00471       {
00472         dMassSetCapsuleTotal(&mass_l3, (double)link_masses[2], 3, (double)link_dimensions[2][0]/2.0, (double)link_dimensions[2][1]);
00473         link3->setMass(mass_l3);
00474         g_link3_cap = new dCapsule(*space, (double)link_dimensions[2][0]/2.0, (double)link_dimensions[2][1]);
00475         g_link3_cap->setBody(*link3);
00476         link3->setRotation(cyl_rotate);
00477       }
00478     else
00479       {
00480         std::cerr<<"The shape type :"<<link3_shape<<"\n of the given robot link is not defined\n";
00481         assert(false);
00482       }
00483 
00484     link3->setPosition((double)link_pos[2][0], (double)link_pos[2][1], (double)link_pos[2][2]); 
00485     link_ids[2] = link3->id();
00486     forearm_length = (double)link_dimensions[2][1];//0.288; //+ 0.115; // rlowarmz + rhandz
00487     forearm_width = (double)link_dimensions[2][0];
00488 
00489 
00490     if (include_mobile_base != 0)
00491     {
00492         dMass mass_base;
00493         double base_x = 0.2;
00494         double base_y = 0.2;
00495         double base_z = 0.05;
00496         dMassSetBoxTotal(&mass_base, 10., base_x, base_y, base_z);
00497 
00498         body_mobile_base = new dBody(*world);
00499         body_mobile_base->setMass(mass_l3);
00500 
00501         geom_mobile_base = new dBox(*space, base_x, base_y, base_z);
00502         geom_mobile_base->setBody(*body_mobile_base);
00503     }
00504 
00505     //------- now define the joints ----------
00506     if (include_mobile_base != 0)
00507     {
00508         mobile_base_plane2d_jt_id = dJointCreatePlane2D(world->id(), 0);
00509         // attach to mobile base body and the world.
00510         dJointAttach(mobile_base_plane2d_jt_id, body_mobile_base->id(), 0);
00511 
00512         dJointSetPlane2DXParam(mobile_base_plane2d_jt_id, dParamFMax, 0);
00513         dJointSetPlane2DYParam(mobile_base_plane2d_jt_id, dParamFMax, 0);
00514 
00515         dJointSetPlane2DXParam(mobile_base_plane2d_jt_id, dParamVel, 0.0);
00516         dJointSetPlane2DYParam(mobile_base_plane2d_jt_id, dParamVel, 0.0);
00517 
00518         dJointSetPlane2DAngleParam(mobile_base_plane2d_jt_id, dParamFMax, 100);
00519         dJointSetPlane2DAngleParam(mobile_base_plane2d_jt_id, dParamVel, 0.0);
00520 
00521         hinge1 = new dHingeJoint(*world);
00522         hinge1->attach(*link1, *body_mobile_base);
00523     }
00524     else
00525     {
00526         hinge1 = new dHingeJoint(*world);
00527         hinge1->attach(*link1, 0);
00528     }
00529 
00530     hinge1->setAnchor((double)jt_anchor[0][0], (double)jt_anchor[0][1], (double)jt_anchor[0][2]);
00531     hinge1->setAxis((double)jt_axes[0][0], (double)jt_axes[0][1], (double)jt_axes[0][2]);
00532     dJointSetHingeParam(hinge1->id(),dParamLoStop, torso_llim);
00533     dJointSetHingeParam(hinge1->id(),dParamHiStop, torso_ulim);
00534 
00535     hinge2 = new dHingeJoint(*world);
00536     hinge2->attach(*link2, *link1);
00537     hinge2->setAnchor((double)jt_anchor[1][0], (double)jt_anchor[1][1], (double)jt_anchor[1][2]);
00538     hinge2->setAxis((double)jt_axes[1][0], (double)jt_axes[1][1], (double)jt_axes[1][2]);
00539     dJointSetHingeParam(hinge2->id(),dParamLoStop, shoulder_llim);
00540     dJointSetHingeParam(hinge2->id(),dParamHiStop, shoulder_ulim);
00541 
00542     hinge3 = new dHingeJoint(*world);
00543     hinge3->attach(*link3, *link2);
00544     hinge3->setAnchor((double)jt_anchor[2][0], (double)jt_anchor[2][1], (double)jt_anchor[2][2]);
00545     hinge3->setAxis((double)jt_axes[2][0], (double)jt_axes[2][1], (double)jt_axes[2][2]);
00546     dJointSetHingeParam(hinge3->id(),dParamLoStop, elbow_llim);
00547     dJointSetHingeParam(hinge3->id(),dParamHiStop, elbow_ulim);
00548 
00549 
00550     // this is for contact joints (Advait)
00551     joints.create();
00552 }
00553 
00554 void create_movable_obstacles(ros::NodeHandle n)
00555 {
00556     float obstacle_mass = 1.;
00557 
00558     // this needs to be the first param that is read for stupid
00559     // synchronization with obstacles.py
00560     XmlRpc::XmlRpcValue cylinders_dim;
00561     while (n.getParam("/m3/software_testbed/movable_dimen", cylinders_dim) == false)
00562         sleep(0.1);
00563 
00564     XmlRpc::XmlRpcValue cylinders_pos;
00565     n.getParam("/m3/software_testbed/movable_position", cylinders_pos);
00566 
00567     n.getParam("/m3/software_testbed/num_movable", num_used_movable);
00568 
00569     XmlRpc::XmlRpcValue cylinders_max_force;     
00570     bool got_max_force;
00571     got_max_force = n.getParam("/m3/software_testbed/movable_max_force", cylinders_max_force);
00572 
00573     for (int i = 0; i < num_used_movable; i++)
00574     {
00575         dMass m_obst;
00576         dCapsule *geom_cyl;
00577 
00578         dMassSetCapsuleTotal(&m_obst, obstacle_mass, 3,
00579                 (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
00580         obstacles[i].create(*world);
00581         obstacles[i].setPosition((double)cylinders_pos[i][0], (double)cylinders_pos[i][1], (double)cylinders_pos[i][2]);
00582         obstacles[i].setMass(&m_obst);
00583         plane2d_joint_ids[i] = dJointCreatePlane2D(world->id(), 0);
00584         dJointAttach(plane2d_joint_ids[i], obstacles[i].id(), 0);
00585         if (got_max_force == false)
00586           {
00587             dJointSetPlane2DXParam(plane2d_joint_ids[i], dParamFMax, max_friction*0.707);
00588             dJointSetPlane2DYParam(plane2d_joint_ids[i], dParamFMax, max_friction*0.707);
00589             dJointSetPlane2DAngleParam(plane2d_joint_ids[i], dParamFMax, max_tor_friction);
00590           }
00591         else
00592           {
00593             dJointSetPlane2DXParam(plane2d_joint_ids[i], dParamFMax, (double)cylinders_max_force[i]*0.707);
00594             dJointSetPlane2DYParam(plane2d_joint_ids[i], dParamFMax, (double)cylinders_max_force[i]*0.707);
00595             dJointSetPlane2DAngleParam(plane2d_joint_ids[i], dParamFMax, max_tor_friction);
00596           }
00597 
00598         dJointSetPlane2DXParam(plane2d_joint_ids[i], dParamVel, 0.0);
00599         dJointSetPlane2DYParam(plane2d_joint_ids[i], dParamVel, 0.0);
00600         dJointSetPlane2DAngleParam(plane2d_joint_ids[i], dParamVel, 0.0);
00601 
00602         geom_cyl = new dCapsule(*space, (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
00603         geom_cyl->setBody(obstacles[i]);
00604         dJointSetFeedback(plane2d_joint_ids[i], &frict_feedbacks[i].fb);
00605     }
00606 }
00607 
00608 
00609 void create_compliant_obstacles(ros::NodeHandle n)
00610 {
00611     float obstacle_mass = 1.;
00612 
00613     // this needs to be the first param that is read for stupid
00614     // synchronization with obstacles.py
00615     XmlRpc::XmlRpcValue cylinders_dim;
00616     while (n.getParam("/m3/software_testbed/compliant_dimen", cylinders_dim) == false)
00617         sleep(0.1);
00618 
00619     XmlRpc::XmlRpcValue cylinders_pos;
00620     n.getParam("/m3/software_testbed/compliant_position", cylinders_pos);
00621 
00622     XmlRpc::XmlRpcValue cylinders_stiffness;
00623     bool got_stiffness;
00624     got_stiffness = n.getParam("/m3/software_testbed/compliant_stiffness_value", cylinders_stiffness);
00625 
00626     n.getParam("/m3/software_testbed/num_compliant", num_used_compliant);
00627 
00628     for (int i = 0; i < num_used_compliant; i++)
00629     {
00630         dMass m_obst;
00631         dCapsule *geom_cyl;
00632         
00633         if(got_stiffness == true)
00634           {
00635             obst_stiffness[i]= (double)cylinders_stiffness[i];
00636 
00637             //we calculate required damping using a
00638             //damping ratio of 1 (critically damped)
00639             obst_damping[i] = 2*sqrt(obst_stiffness[i]*obstacle_mass);
00640           }
00641         else
00642           {
00643             obst_stiffness[i]= -1;
00644           }
00645 
00646         dMassSetCapsuleTotal(&m_obst, obstacle_mass, 3,
00647                 (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
00648         compliant_obstacles[i].create(*world);
00649         compliant_obstacles[i].setPosition((double)cylinders_pos[i][0], (double)cylinders_pos[i][1], (double)cylinders_pos[i][2]);
00650         obst_home[i][0] = (double)cylinders_pos[i][0];
00651         obst_home[i][1] = (double)cylinders_pos[i][1];
00652         obst_home[i][2] = (double)cylinders_pos[i][2];
00653         compliant_obstacles[i].setMass(&m_obst);
00654         compliant_plane2d_joint_ids[i] = dJointCreatePlane2D(world->id(), 0);
00655         dJointAttach(compliant_plane2d_joint_ids[i], compliant_obstacles[i].id(), 0);
00656         // if (got_max_force == false)
00657         //   {
00658         //     dJointSetPlane2DXParam(compliant_plane2d_joint_ids[i], dParamFMax, max_friction*0.707);
00659         //     dJointSetPlane2DYParam(compliant_plane2d_joint_ids[i], dParamFMax, max_friction*0.707);
00660         //     dJointSetPlane2DAngleParam(compliant_plane2d_joint_ids[i], dParamFMax, max_tor_friction);
00661         //   }
00662         // else
00663         //   {
00664         //     dJointSetPlane2DXParam(compliant_plane2d_joint_ids[i], dParamFMax, (double)cylinders_max_force[i]*0.707);
00665         //     dJointSetPlane2DYParam(compliant_plane2d_joint_ids[i], dParamFMax, (double)cylinders_max_force[i]*0.707);
00666         //     dJointSetPlane2DAngleParam(compliant_plane2d_joint_ids[i], dParamFMax, max_tor_friction);
00667         //   }
00668 
00669         dJointSetPlane2DXParam(compliant_plane2d_joint_ids[i], dParamVel, 0.0);
00670         dJointSetPlane2DYParam(compliant_plane2d_joint_ids[i], dParamVel, 0.0);
00671         dJointSetPlane2DAngleParam(compliant_plane2d_joint_ids[i], dParamVel, 0.0);
00672 
00673         geom_cyl = new dCapsule(*space, (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
00674         geom_cyl->setBody(compliant_obstacles[i]);
00675         //dJointSetFeedback(compliant_plane2d_joint_ids[i], &compliant_frict_feedbacks[i].fb);
00676     }
00677 }
00678 
00679 
00680 void create_fixed_obstacles(ros::NodeHandle n)
00681 {
00682     float obstacle_mass = 1.;
00683 
00684     while(n.getParam("/m3/software_testbed/num_fixed", num_used_fixed) == false)
00685       sleep(0.1);
00686 
00687     XmlRpc::XmlRpcValue fixed_pos;
00688     n.getParam("/m3/software_testbed/fixed_position", fixed_pos);
00689     XmlRpc::XmlRpcValue fixed_dim;
00690     n.getParam("/m3/software_testbed/fixed_dimen", fixed_dim);
00691 
00692     for (int i = 0; i < num_used_fixed; i++)
00693     {
00694         dMass m_obst;
00695         dCapsule *geom_fixed;
00696         dMassSetCapsuleTotal(&m_obst, obstacle_mass, 3,
00697                               (double)fixed_dim[i][0], (double)fixed_dim[i][2]);
00698 
00699         fixed_obstacles[i].create(*world);
00700         fixed_obstacles[i].setPosition((double)fixed_pos[i][0], (double)fixed_pos[i][1], (double)fixed_pos[i][2]);
00701         fixed_obstacles[i].setMass(&m_obst);
00702         fixed_joint_ids[i] = dJointCreateFixed(world->id(), 0);
00703         dJointAttach(fixed_joint_ids[i], fixed_obstacles[i].id(), 0);
00704         dJointSetFixed(fixed_joint_ids[i]);
00705         geom_fixed = new dCapsule(*space, (double)fixed_dim[i][0], (double)fixed_dim[i][2]);
00706         geom_fixed->setBody(fixed_obstacles[i]);
00707     }
00708 }
00709 
00710 double get_wall_clock_time()
00711 {
00712     timeval tim;
00713     gettimeofday(&tim, NULL);
00714     double t1=tim.tv_sec+(tim.tv_usec/1000000.0);
00715     return t1;
00716 }
00717 
00718 
00719 
00720 void taxel_simulation_code(double resolution)
00721 {
00722     /**********************************************************************************/
00723     /*  THE TAXEL CODE BELOW IS A MESS, IT WORKS ONLY FOR 2D ALONG A LINE AND NEEDS TO BE
00724         CLEANED UP AND CONDENSED INTO A SINGLE FUNCTION CALL FOR EACH LINK *******************/
00725     // float torso_half_width = 0.196; // ctorsoy
00726     /*************************************************************************************/
00727     taxel.header.frame_id = "/torso_lift_link";
00728     taxel.header.stamp = ros::Time::now();
00729 
00730     std::vector < string > taxel_links;
00731 
00734 
00735 
00736     int k = 2;
00737 
00738     assert(torso_half_width>0);
00739     assert(torso_half_width_side>0);
00740 
00741     int num = floor(resolution*torso_half_width);
00742     float step =  1.0/resolution;
00743     float link1_width = torso_half_width_side/2.0;
00744 
00745     dVector3 global_pt0;
00746     dBodyGetRelPointPos(link1->id(), link1_width, 0.0,  0.0, global_pt0);
00747     taxel.centers_x.push_back(global_pt0[0]);
00748     taxel.centers_y.push_back(global_pt0[1]);
00749     taxel.centers_z.push_back(global_pt0[2]);
00750     taxel.link_names.push_back("link1");
00751 
00752     taxel_links.push_back("link1");
00753 
00754     dBodyGetRelPointPos(link1->id(), -link1_width, 0.0,  0.0, global_pt0);
00755     taxel.centers_x.push_back(global_pt0[0]);
00756     taxel.centers_y.push_back(global_pt0[1]);
00757     taxel.centers_z.push_back(global_pt0[2]);
00758     taxel.link_names.push_back("link1");
00759 
00760     taxel_links.push_back("link1");
00761 
00762     dVector3 global_n0;
00763     dBodyVectorToWorld (link1->id(), 1.0, 0.0, 0.0, global_n0);
00764     taxel.normals_x.push_back(global_n0[0]);
00765     taxel.normals_y.push_back(global_n0[1]);
00766     taxel.normals_z.push_back(global_n0[2]);
00767 
00768     dBodyVectorToWorld (link1->id(), -1.0, 0.0, 0.0, global_n0);
00769     taxel.normals_x.push_back(global_n0[0]);
00770     taxel.normals_y.push_back(global_n0[1]);
00771     taxel.normals_z.push_back(global_n0[2]);
00772 
00773     while ( k < num )
00774     {
00775         dVector3 global_pt;
00776         dVector3 global_pt2;
00777 
00778         if (num >1 )
00779         {
00780             dBodyGetRelPointPos(link1->id(), link1_width, (k/2)*step,  0.0, global_pt);
00781             taxel.centers_x.push_back(global_pt[0]);
00782             taxel.centers_y.push_back(global_pt[1]);
00783             taxel.centers_z.push_back(global_pt[2]);
00784             taxel.link_names.push_back("link1");
00785 
00786             taxel_links.push_back("link1");
00787 
00788             dBodyGetRelPointPos(link1->id(), -link1_width, (k/2)*step,  0.0, global_pt2);
00789             taxel.centers_x.push_back(global_pt2[0]);
00790             taxel.centers_y.push_back(global_pt2[1]);
00791             taxel.centers_z.push_back(global_pt2[2]);
00792             taxel.link_names.push_back("link1");
00793 
00794             taxel_links.push_back("link1");
00795 
00796             dBodyGetRelPointPos(link1->id(), link1_width, -(k/2)*step,  0.0, global_pt);
00797             taxel.centers_x.push_back(global_pt[0]);
00798             taxel.centers_y.push_back(global_pt[1]);
00799             taxel.centers_z.push_back(global_pt[2]);
00800             taxel.link_names.push_back("link1");
00801 
00802             taxel_links.push_back("link1");
00803 
00804             dBodyGetRelPointPos(link1->id(), -link1_width, -(k/2)*step,  0.0, global_pt2);
00805             taxel.centers_x.push_back(global_pt2[0]);
00806             taxel.centers_y.push_back(global_pt2[1]);
00807             taxel.centers_z.push_back(global_pt2[2]);
00808             taxel.link_names.push_back("link1");
00809 
00810             taxel_links.push_back("link1");
00811 
00812             dVector3 global_n;
00813             dVector3 global_n2;
00814             dBodyVectorToWorld (link1->id(), 1.0, 0.0, 0.0, global_n);
00815             taxel.normals_x.push_back(global_n[0]);
00816             taxel.normals_y.push_back(global_n[1]);
00817             taxel.normals_z.push_back(global_n[2]);
00818 
00819             dBodyVectorToWorld (link1->id(), -1.0, 0.0, 0.0, global_n2);
00820             taxel.normals_x.push_back(global_n2[0]);
00821             taxel.normals_y.push_back(global_n2[1]);
00822             taxel.normals_z.push_back(global_n2[2]);
00823 
00824             dBodyVectorToWorld (link1->id(), 1.0, 0.0, 0.0, global_n);
00825             taxel.normals_x.push_back(global_n[0]);
00826             taxel.normals_y.push_back(global_n[1]);
00827             taxel.normals_z.push_back(global_n[2]);
00828 
00829             dBodyVectorToWorld (link1->id(), -1.0, 0.0, 0.0, global_n2);
00830             taxel.normals_x.push_back(global_n2[0]);
00831             taxel.normals_y.push_back(global_n2[1]);
00832             taxel.normals_z.push_back(global_n2[2]);
00833         }
00834         k = k+2;
00835     }
00836 
00837     k = 2;
00838     num = floor(resolution*(torso_half_width_side+0.0001));
00839 
00840     dBodyGetRelPointPos(link1->id(), 0.0, -torso_half_width/2.0,  0.0, global_pt0);
00841     taxel.centers_x.push_back(global_pt0[0]);
00842     taxel.centers_y.push_back(global_pt0[1]);
00843     taxel.centers_z.push_back(global_pt0[2]);
00844     taxel.link_names.push_back("link1");
00845 
00846     taxel_links.push_back("link1");
00847 
00848     dBodyVectorToWorld (link1->id(), 0.0, -1.0, 0.0, global_n0);
00849     taxel.normals_x.push_back(global_n0[0]);
00850     taxel.normals_y.push_back(global_n0[1]);
00851     taxel.normals_z.push_back(global_n0[2]);
00852 
00854     dBodyGetRelPointPos(link1->id(), 0.0, torso_half_width/2.0,  0.0, global_pt0);
00855     taxel.centers_x.push_back(global_pt0[0]);
00856     taxel.centers_y.push_back(global_pt0[1]);
00857     taxel.centers_z.push_back(global_pt0[2]);
00858     taxel.link_names.push_back("link1");
00859 
00860     taxel_links.push_back("link1");
00861 
00862     dBodyVectorToWorld (link1->id(), 0.0, 1.0, 0.0, global_n0);
00863     taxel.normals_x.push_back(global_n0[0]);
00864     taxel.normals_y.push_back(global_n0[1]);
00865     taxel.normals_z.push_back(global_n0[2]);
00867 
00868 
00869     while ( k < num )
00870     {
00871         dVector3 global_pt;
00872         dVector3 global_pt2;
00873         dVector3 global_n;
00874         dVector3 global_n2;
00875 
00876         if (num >  1.0)
00877         {
00878             dBodyGetRelPointPos(link1->id(), (k/2)*step, -torso_half_width/2.0,  0.0, global_pt);               
00879             taxel.centers_x.push_back(global_pt[0]);
00880             taxel.centers_y.push_back(global_pt[1]);
00881             taxel.centers_z.push_back(global_pt[2]);
00882             taxel.link_names.push_back("link1");
00883 
00884             taxel_links.push_back("link1");
00885 
00886             dBodyGetRelPointPos(link1->id(), -(k/2)*step, -torso_half_width/2.0,  0.0, global_pt2);             
00887             taxel.centers_x.push_back(global_pt2[0]);
00888             taxel.centers_y.push_back(global_pt2[1]);
00889             taxel.centers_z.push_back(global_pt2[2]);
00890             taxel.link_names.push_back("link1");
00891 
00892             taxel_links.push_back("link1");
00893 
00894             dBodyVectorToWorld (link1->id(), 0.0, -1.0, 0.0, global_n);
00895             taxel.normals_x.push_back(global_n[0]);
00896             taxel.normals_y.push_back(global_n[1]);
00897             taxel.normals_z.push_back(global_n[2]);
00898 
00899             dBodyVectorToWorld (link1->id(), 0.0, -1.0, 0.0, global_n2);
00900             taxel.normals_x.push_back(global_n2[0]);
00901             taxel.normals_y.push_back(global_n2[1]);
00902             taxel.normals_z.push_back(global_n2[2]);
00903 
00904             dBodyGetRelPointPos(link1->id(), (k/2)*step, torso_half_width/2.0,  0.0, global_pt);                
00905             taxel.centers_x.push_back(global_pt[0]);
00906             taxel.centers_y.push_back(global_pt[1]);
00907             taxel.centers_z.push_back(global_pt[2]);
00908             taxel.link_names.push_back("link1");
00909 
00910             taxel_links.push_back("link1");
00911 
00912             dBodyGetRelPointPos(link1->id(), -(k/2)*step, torso_half_width/2.0,  0.0, global_pt2);              
00913             taxel.centers_x.push_back(global_pt2[0]);
00914             taxel.centers_y.push_back(global_pt2[1]);
00915             taxel.centers_z.push_back(global_pt2[2]);
00916             taxel.link_names.push_back("link1");
00917 
00918             taxel_links.push_back("link1");
00919 
00920             dBodyVectorToWorld (link1->id(), 0.0, 1.0, 0.0, global_n);
00921             taxel.normals_x.push_back(global_n[0]);
00922             taxel.normals_y.push_back(global_n[1]);
00923             taxel.normals_z.push_back(global_n[2]);
00924 
00925             dBodyVectorToWorld (link1->id(), 0.0, 1.0, 0.0, global_n2);
00926             taxel.normals_x.push_back(global_n2[0]);
00927             taxel.normals_y.push_back(global_n2[1]);
00928             taxel.normals_z.push_back(global_n2[2]);
00929 
00930         }
00931 
00932         k = k+2;
00933     }
00934 
00937 
00938 
00939 
00942 
00943 
00944     // float upper_arm_length = 0.334; // ruparmz
00945     assert(upper_arm_length>0);
00946     assert(upper_arm_width>0);
00947     float link2_width = upper_arm_width/2.0;
00948     k = 2;
00949     num = floor(resolution*upper_arm_length);
00950     dBodyGetRelPointPos(link2->id(), link2_width, 0.0,  0.0, global_pt0);
00951     taxel.centers_x.push_back(global_pt0[0]);
00952     taxel.centers_y.push_back(global_pt0[1]);
00953     taxel.centers_z.push_back(global_pt0[2]);
00954     taxel.link_names.push_back("link2");
00955 
00956     taxel_links.push_back("link2");
00957 
00958     dBodyGetRelPointPos(link2->id(), -link2_width, 0.0,  0.0, global_pt0);
00959     taxel.centers_x.push_back(global_pt0[0]);
00960     taxel.centers_y.push_back(global_pt0[1]);
00961     taxel.centers_z.push_back(global_pt0[2]);
00962     taxel.link_names.push_back("link2");
00963 
00964     taxel_links.push_back("link2");
00965 
00966     dBodyVectorToWorld (link2->id(), 1.0, 0.0, 0.0, global_n0);
00967     taxel.normals_x.push_back(global_n0[0]);
00968     taxel.normals_y.push_back(global_n0[1]);
00969     taxel.normals_z.push_back(global_n0[2]);
00970 
00971     dBodyVectorToWorld (link2->id(), -1.0, 0.0, 0.0, global_n0);
00972     taxel.normals_x.push_back(global_n0[0]);
00973     taxel.normals_y.push_back(global_n0[1]);
00974     taxel.normals_z.push_back(global_n0[2]);
00975 
00976     while ( k < num )
00977     {
00978         dVector3 global_pt;
00979         dVector3 global_pt2;
00980 
00981         if ( num > 1)
00982         {
00983             dBodyGetRelPointPos(link2->id(), link2_width, (k/2)*step,  0.0, global_pt);
00984             taxel.centers_x.push_back(global_pt[0]);
00985             taxel.centers_y.push_back(global_pt[1]);
00986             taxel.centers_z.push_back(global_pt[2]);
00987             taxel.link_names.push_back("link2");
00988 
00989             taxel_links.push_back("link2");
00990 
00991             dBodyGetRelPointPos(link2->id(), -link2_width, (k/2)*step,  0.0, global_pt2);
00992             taxel.centers_x.push_back(global_pt2[0]);
00993             taxel.centers_y.push_back(global_pt2[1]);
00994             taxel.centers_z.push_back(global_pt2[2]);
00995             taxel.link_names.push_back("link2");
00996 
00997             taxel_links.push_back("link2");
00998 
00999             dBodyGetRelPointPos(link2->id(), link2_width, -(k/2)*step,  0.0, global_pt);
01000             taxel.centers_x.push_back(global_pt[0]);
01001             taxel.centers_y.push_back(global_pt[1]);
01002             taxel.centers_z.push_back(global_pt[2]);
01003             taxel.link_names.push_back("link2");
01004 
01005             taxel_links.push_back("link2");
01006 
01007             dBodyGetRelPointPos(link2->id(), -link2_width, -(k/2)*step,  0.0, global_pt2);
01008             taxel.centers_x.push_back(global_pt2[0]);
01009             taxel.centers_y.push_back(global_pt2[1]);
01010             taxel.centers_z.push_back(global_pt2[2]);
01011             taxel.link_names.push_back("link2");
01012 
01013             taxel_links.push_back("link2");
01014 
01015             dVector3 global_n;
01016             dVector3 global_n2;
01017             dBodyVectorToWorld (link2->id(), 1.0, 0.0, 0.0, global_n);
01018             taxel.normals_x.push_back(global_n[0]);
01019             taxel.normals_y.push_back(global_n[1]);
01020             taxel.normals_z.push_back(global_n[2]);
01021 
01022             dBodyVectorToWorld (link2->id(), -1.0, 0.0, 0.0, global_n2);
01023             taxel.normals_x.push_back(global_n2[0]);
01024             taxel.normals_y.push_back(global_n2[1]);
01025             taxel.normals_z.push_back(global_n2[2]);
01026 
01027             dBodyVectorToWorld (link2->id(), 1.0, 0.0, 0.0, global_n);
01028             taxel.normals_x.push_back(global_n[0]);
01029             taxel.normals_y.push_back(global_n[1]);
01030             taxel.normals_z.push_back(global_n[2]);
01031 
01032             dBodyVectorToWorld (link2->id(), -1.0, 0.0, 0.0, global_n2);
01033             taxel.normals_x.push_back(global_n2[0]);
01034             taxel.normals_y.push_back(global_n2[1]);
01035             taxel.normals_z.push_back(global_n2[2]);
01036         }
01037         k = k+2;
01038     }
01039 
01041 
01042     k = 2;
01043     num = floor(resolution*(upper_arm_width+0.0001));
01044 
01045     dBodyGetRelPointPos(link2->id(), 0.0, -upper_arm_length/2.0,  0.0, global_pt0);
01046     taxel.centers_x.push_back(global_pt0[0]);
01047     taxel.centers_y.push_back(global_pt0[1]);
01048     taxel.centers_z.push_back(global_pt0[2]);
01049     taxel.link_names.push_back("link2");
01050 
01051     taxel_links.push_back("link2");
01052 
01053     dBodyVectorToWorld (link2->id(), 0.0, -1.0, 0.0, global_n0);
01054     taxel.normals_x.push_back(global_n0[0]);
01055     taxel.normals_y.push_back(global_n0[1]);
01056     taxel.normals_z.push_back(global_n0[2]);
01057 
01059     dBodyGetRelPointPos(link2->id(), 0.0, upper_arm_length/2.0,  0.0, global_pt0);
01060     taxel.centers_x.push_back(global_pt0[0]);
01061     taxel.centers_y.push_back(global_pt0[1]);
01062     taxel.centers_z.push_back(global_pt0[2]);
01063     taxel.link_names.push_back("link2");
01064 
01065     taxel_links.push_back("link2");
01066 
01067     dBodyVectorToWorld (link2->id(), 0.0, 1.0, 0.0, global_n0);
01068     taxel.normals_x.push_back(global_n0[0]);
01069     taxel.normals_y.push_back(global_n0[1]);
01070     taxel.normals_z.push_back(global_n0[2]);
01072 
01073 
01074     while ( k < num )
01075     {
01076         dVector3 global_pt;
01077         dVector3 global_pt2;
01078         dVector3 global_n;
01079         dVector3 global_n2;
01080 
01081         if (num >  1.0)
01082         {
01083             dBodyGetRelPointPos(link2->id(), (k/2)*step, -upper_arm_length/2.0,  0.0, global_pt);               
01084             taxel.centers_x.push_back(global_pt[0]);
01085             taxel.centers_y.push_back(global_pt[1]);
01086             taxel.centers_z.push_back(global_pt[2]);
01087             taxel.link_names.push_back("link2");
01088 
01089             taxel_links.push_back("link2");
01090 
01091             dBodyGetRelPointPos(link2->id(), -(k/2)*step, -upper_arm_length/2.0,  0.0, global_pt2);             
01092             taxel.centers_x.push_back(global_pt2[0]);
01093             taxel.centers_y.push_back(global_pt2[1]);
01094             taxel.centers_z.push_back(global_pt2[2]);
01095             taxel.link_names.push_back("link2");
01096 
01097             taxel_links.push_back("link2");
01098 
01099             dBodyVectorToWorld (link2->id(), 0.0, -1.0, 0.0, global_n);
01100             taxel.normals_x.push_back(global_n[0]);
01101             taxel.normals_y.push_back(global_n[1]);
01102             taxel.normals_z.push_back(global_n[2]);
01103 
01104             dBodyVectorToWorld (link2->id(), 0.0, -1.0, 0.0, global_n2);
01105             taxel.normals_x.push_back(global_n2[0]);
01106             taxel.normals_y.push_back(global_n2[1]);
01107             taxel.normals_z.push_back(global_n2[2]);
01108 
01109             dBodyGetRelPointPos(link2->id(), (k/2)*step, upper_arm_length/2.0,  0.0, global_pt);                
01110             taxel.centers_x.push_back(global_pt[0]);
01111             taxel.centers_y.push_back(global_pt[1]);
01112             taxel.centers_z.push_back(global_pt[2]);
01113             taxel.link_names.push_back("link2");
01114 
01115             taxel_links.push_back("link2");
01116 
01117             dBodyGetRelPointPos(link2->id(), -(k/2)*step, upper_arm_length/2.0,  0.0, global_pt2);              
01118             taxel.centers_x.push_back(global_pt2[0]);
01119             taxel.centers_y.push_back(global_pt2[1]);
01120             taxel.centers_z.push_back(global_pt2[2]);
01121             taxel.link_names.push_back("link2");
01122 
01123             taxel_links.push_back("link2");
01124 
01125             dBodyVectorToWorld (link2->id(), 0.0, 1.0, 0.0, global_n);
01126             taxel.normals_x.push_back(global_n[0]);
01127             taxel.normals_y.push_back(global_n[1]);
01128             taxel.normals_z.push_back(global_n[2]);
01129 
01130             dBodyVectorToWorld (link2->id(), 0.0, 1.0, 0.0, global_n2);
01131             taxel.normals_x.push_back(global_n2[0]);
01132             taxel.normals_y.push_back(global_n2[1]);
01133             taxel.normals_z.push_back(global_n2[2]);
01134 
01135         }
01136 
01137         k = k+2;
01138     }
01139 
01140 
01143 
01144 
01145 
01146 
01149 
01150 
01151     // float forearm_length = 0.288; //+ 0.115; // rlowarmz + rhandz
01152     k = 2;
01153     assert(forearm_length>0);
01154     assert(forearm_width>0);
01155     float link3_width = forearm_width/2.0;
01156     num = floor(resolution*forearm_length);
01157 
01158     dBodyGetRelPointPos(link3->id(), link3_width, 0.0,  0.0, global_pt0);
01159     taxel.centers_x.push_back(global_pt0[0]);
01160     taxel.centers_y.push_back(global_pt0[1]);
01161     taxel.centers_z.push_back(global_pt0[2]);
01162     taxel.link_names.push_back("link3");
01163 
01164     taxel_links.push_back("link3");
01165 
01166     dBodyGetRelPointPos(link3->id(), -link3_width, 0.0,  0.0, global_pt0);
01167     taxel.centers_x.push_back(global_pt0[0]);
01168     taxel.centers_y.push_back(global_pt0[1]);
01169     taxel.centers_z.push_back(global_pt0[2]);
01170     taxel.link_names.push_back("link3");
01171 
01172     taxel_links.push_back("link3");
01173 
01174     dBodyVectorToWorld (link3->id(), 1.0, 0.0, 0.0, global_n0);
01175     taxel.normals_x.push_back(global_n0[0]);
01176     taxel.normals_y.push_back(global_n0[1]);
01177     taxel.normals_z.push_back(global_n0[2]);
01178 
01179     dBodyVectorToWorld (link3->id(), -1.0, 0.0, 0.0, global_n0);
01180     taxel.normals_x.push_back(global_n0[0]);
01181     taxel.normals_y.push_back(global_n0[1]);
01182     taxel.normals_z.push_back(global_n0[2]);
01183 
01184     // while ( k < num )
01185     // {
01186     //     dVector3 global_pt;
01187     //     dVector3 global_pt2;
01188 
01189     //     if (num > 1)
01190     //     {
01191     //         dBodyGetRelPointPos(link3->id(), link3_width, (k/2)*step,  0.0, global_pt);
01192     //         taxel.centers_x.push_back(global_pt[0]);
01193     //         taxel.centers_y.push_back(global_pt[1]);
01194     //         taxel.centers_z.push_back(global_pt[2]);
01195     //         taxel.link_names.push_back("link3");
01196 
01197     //         taxel_links.push_back("link3");
01198 
01199     //         dBodyGetRelPointPos(link3->id(), -link3_width, (k/2)*step,  0.0, global_pt2);
01200     //         taxel.centers_x.push_back(global_pt2[0]);
01201     //         taxel.centers_y.push_back(global_pt2[1]);
01202     //         taxel.centers_z.push_back(global_pt2[2]);
01203     //         taxel.link_names.push_back("link3");
01204 
01205     //         taxel_links.push_back("link3");
01206 
01207     //         dBodyGetRelPointPos(link3->id(), link3_width, -(k/2)*step,  0.0, global_pt);
01208     //         taxel.centers_x.push_back(global_pt[0]);
01209     //         taxel.centers_y.push_back(global_pt[1]);
01210     //         taxel.centers_z.push_back(global_pt[2]);
01211     //         taxel.link_names.push_back("link3");
01212 
01213     //         taxel_links.push_back("link3");
01214 
01215     //         dBodyGetRelPointPos(link3->id(), -link3_width, -(k/2)*step,  0.0, global_pt2);
01216     //         taxel.centers_x.push_back(global_pt2[0]);
01217     //         taxel.centers_y.push_back(global_pt2[1]);
01218     //         taxel.centers_z.push_back(global_pt2[2]);
01219     //         taxel.link_names.push_back("link3");
01220 
01221     //         taxel_links.push_back("link3");
01222 
01223     //         dVector3 global_n;
01224     //         dVector3 global_n2;
01225     //         dBodyVectorToWorld (link3->id(), 1.0, 0.0, 0.0, global_n);
01226     //         taxel.normals_x.push_back(global_n[0]);
01227     //         taxel.normals_y.push_back(global_n[1]);
01228     //         taxel.normals_z.push_back(global_n[2]);
01229 
01230     //         dBodyVectorToWorld (link3->id(), -1.0, 0.0, 0.0, global_n2);
01231     //         taxel.normals_x.push_back(global_n2[0]);
01232     //         taxel.normals_y.push_back(global_n2[1]);
01233     //         taxel.normals_z.push_back(global_n2[2]);
01234 
01235     //         dBodyVectorToWorld (link3->id(), 1.0, 0.0, 0.0, global_n);
01236     //         taxel.normals_x.push_back(global_n[0]);
01237     //         taxel.normals_y.push_back(global_n[1]);
01238     //         taxel.normals_z.push_back(global_n[2]);
01239 
01240     //         dBodyVectorToWorld (link3->id(), -1.0, 0.0, 0.0, global_n2);
01241     //         taxel.normals_x.push_back(global_n2[0]);
01242     //         taxel.normals_y.push_back(global_n2[1]);
01243     //         taxel.normals_z.push_back(global_n2[2]);
01244     //     }
01245     //     k = k+2;
01246     // }
01247 
01248     // // float forearm_length = 0.288; //+ 0.115; // rlowarmz + rhandz
01249     // k = 2;
01250     // num = floor(resolution*(forearm_width+0.0001));
01251 
01252     // dBodyGetRelPointPos(link3->id(), 0.0, -forearm_length/2.0,  0.0, global_pt0);
01253     // taxel.centers_x.push_back(global_pt0[0]);
01254     // taxel.centers_y.push_back(global_pt0[1]);
01255     // taxel.centers_z.push_back(global_pt0[2]);
01256     // taxel.link_names.push_back("link3");
01257 
01258     // taxel_links.push_back("link3");
01259 
01260     // dBodyVectorToWorld (link3->id(), 0.0, -1.0, 0.0, global_n0);
01261     // taxel.normals_x.push_back(global_n0[0]);
01262     // taxel.normals_y.push_back(global_n0[1]);
01263     // taxel.normals_z.push_back(global_n0[2]);
01264 
01265     // ///////////////////////////////////////////////////test for taxel////////////
01266     // dBodyGetRelPointPos(link3->id(), 0.0, forearm_length/2.0,  0.0, global_pt0);
01267     // taxel.centers_x.push_back(global_pt0[0]);
01268     // taxel.centers_y.push_back(global_pt0[1]);
01269     // taxel.centers_z.push_back(global_pt0[2]);
01270     // taxel.link_names.push_back("link3");
01271 
01272     // taxel_links.push_back("link3");
01273 
01274     // dBodyVectorToWorld (link3->id(), 0.0, 1.0, 0.0, global_n0);
01275     // taxel.normals_x.push_back(global_n0[0]);
01276     // taxel.normals_y.push_back(global_n0[1]);
01277     // taxel.normals_z.push_back(global_n0[2]);
01278     // ///////////////////////////////////////////////////////////////////////////////
01279 
01280 
01281     // while ( k < num )
01282     // {
01283     //     dVector3 global_pt;
01284     //     dVector3 global_pt2;
01285     //     dVector3 global_n;
01286     //     dVector3 global_n2;
01287 
01288     //     if (num >  1.0)
01289     //     {
01290     //         dBodyGetRelPointPos(link3->id(), (k/2)*step, -forearm_length/2.0,  0.0, global_pt);              
01291     //         taxel.centers_x.push_back(global_pt[0]);
01292     //         taxel.centers_y.push_back(global_pt[1]);
01293     //         taxel.centers_z.push_back(global_pt[2]);
01294     //         taxel.link_names.push_back("link3");
01295 
01296     //         taxel_links.push_back("link3");
01297 
01298     //         dBodyGetRelPointPos(link3->id(), -(k/2)*step, -forearm_length/2.0,  0.0, global_pt2);            
01299     //         taxel.centers_x.push_back(global_pt2[0]);
01300     //         taxel.centers_y.push_back(global_pt2[1]);
01301     //         taxel.centers_z.push_back(global_pt2[2]);
01302     //         taxel.link_names.push_back("link3");
01303 
01304     //         taxel_links.push_back("link3");
01305 
01306     //         dBodyVectorToWorld (link3->id(), 0.0, -1.0, 0.0, global_n);
01307     //         taxel.normals_x.push_back(global_n[0]);
01308     //         taxel.normals_y.push_back(global_n[1]);
01309     //         taxel.normals_z.push_back(global_n[2]);
01310 
01311     //         dBodyVectorToWorld (link3->id(), 0.0, -1.0, 0.0, global_n2);
01312     //         taxel.normals_x.push_back(global_n2[0]);
01313     //         taxel.normals_y.push_back(global_n2[1]);
01314     //         taxel.normals_z.push_back(global_n2[2]);
01315 
01316     //         dBodyGetRelPointPos(link3->id(), (k/2)*step, forearm_length/2.0,  0.0, global_pt);               
01317     //         taxel.centers_x.push_back(global_pt[0]);
01318     //         taxel.centers_y.push_back(global_pt[1]);
01319     //         taxel.centers_z.push_back(global_pt[2]);
01320     //         taxel.link_names.push_back("link3");
01321 
01322     //         taxel_links.push_back("link3");
01323 
01324     //         dBodyGetRelPointPos(link3->id(), -(k/2)*step, forearm_length/2.0,  0.0, global_pt2);             
01325     //         taxel.centers_x.push_back(global_pt2[0]);
01326     //         taxel.centers_y.push_back(global_pt2[1]);
01327     //         taxel.centers_z.push_back(global_pt2[2]);
01328     //         taxel.link_names.push_back("link3");
01329 
01330     //         taxel_links.push_back("link3");
01331 
01332     //         dBodyVectorToWorld (link3->id(), 0.0, 1.0, 0.0, global_n);
01333     //         taxel.normals_x.push_back(global_n[0]);
01334     //         taxel.normals_y.push_back(global_n[1]);
01335     //         taxel.normals_z.push_back(global_n[2]);
01336 
01337     //         dBodyVectorToWorld (link3->id(), 0.0, 1.0, 0.0, global_n2);
01338     //         taxel.normals_x.push_back(global_n2[0]);
01339     //         taxel.normals_y.push_back(global_n2[1]);
01340     //         taxel.normals_z.push_back(global_n2[2]);
01341 
01342     //     }
01343 
01344     //     k = k+2;
01345     // }
01346 
01347     while ( k < num )
01348     {
01349         dVector3 global_pt;
01350         dVector3 global_pt2;
01351 
01352         if (num > 1)
01353         {
01354           dBodyGetRelPointPos(link3->id(), link3_width,  0.0, (k/2)*step, global_pt);
01355             taxel.centers_x.push_back(global_pt[0]);
01356             taxel.centers_y.push_back(global_pt[1]);
01357             taxel.centers_z.push_back(global_pt[2]);
01358             taxel.link_names.push_back("link3");
01359 
01360             taxel_links.push_back("link3");
01361 
01362             dBodyGetRelPointPos(link3->id(), -link3_width, 0.0, (k/2)*step, global_pt2);
01363             taxel.centers_x.push_back(global_pt2[0]);
01364             taxel.centers_y.push_back(global_pt2[1]);
01365             taxel.centers_z.push_back(global_pt2[2]);
01366             taxel.link_names.push_back("link3");
01367 
01368             taxel_links.push_back("link3");
01369 
01370             dBodyGetRelPointPos(link3->id(), link3_width, 0.0,  -(k/2)*step, global_pt);
01371             taxel.centers_x.push_back(global_pt[0]);
01372             taxel.centers_y.push_back(global_pt[1]);
01373             taxel.centers_z.push_back(global_pt[2]);
01374             taxel.link_names.push_back("link3");
01375 
01376             taxel_links.push_back("link3");
01377 
01378             dBodyGetRelPointPos(link3->id(), -link3_width, 0.0,  -(k/2)*step, global_pt2);
01379             taxel.centers_x.push_back(global_pt2[0]);
01380             taxel.centers_y.push_back(global_pt2[1]);
01381             taxel.centers_z.push_back(global_pt2[2]);
01382             taxel.link_names.push_back("link3");
01383 
01384             taxel_links.push_back("link3");
01385 
01386             dVector3 global_n;
01387             dVector3 global_n2;
01388             dBodyVectorToWorld (link3->id(), 1.0, 0.0, 0.0, global_n);
01389             taxel.normals_x.push_back(global_n[0]);
01390             taxel.normals_y.push_back(global_n[1]);
01391             taxel.normals_z.push_back(global_n[2]);
01392 
01393             dBodyVectorToWorld (link3->id(), -1.0, 0.0, 0.0, global_n2);
01394             taxel.normals_x.push_back(global_n2[0]);
01395             taxel.normals_y.push_back(global_n2[1]);
01396             taxel.normals_z.push_back(global_n2[2]);
01397 
01398             dBodyVectorToWorld (link3->id(), 1.0, 0.0, 0.0, global_n);
01399             taxel.normals_x.push_back(global_n[0]);
01400             taxel.normals_y.push_back(global_n[1]);
01401             taxel.normals_z.push_back(global_n[2]);
01402 
01403             dBodyVectorToWorld (link3->id(), -1.0, 0.0, 0.0, global_n2);
01404             taxel.normals_x.push_back(global_n2[0]);
01405             taxel.normals_y.push_back(global_n2[1]);
01406             taxel.normals_z.push_back(global_n2[2]);
01407         }
01408         k = k+2;
01409     }
01410 
01411     // float forearm_length = 0.288; //+ 0.115; // rlowarmz + rhandz
01412     k = 2;
01413     num = floor(resolution*(forearm_width+0.0001));
01414 
01415     dBodyGetRelPointPos(link3->id(), 0.0, 0.0,  -forearm_length/2.0, global_pt0);
01416     taxel.centers_x.push_back(global_pt0[0]);
01417     taxel.centers_y.push_back(global_pt0[1]);
01418     taxel.centers_z.push_back(global_pt0[2]);
01419     taxel.link_names.push_back("link3");
01420 
01421     taxel_links.push_back("link3");
01422 
01423     dBodyVectorToWorld (link3->id(), 0.0, 0.0,  -1.0, global_n0);
01424     taxel.normals_x.push_back(global_n0[0]);
01425     taxel.normals_y.push_back(global_n0[1]);
01426     taxel.normals_z.push_back(global_n0[2]);
01427 
01429     dBodyGetRelPointPos(link3->id(), 0.0, 0.0,  forearm_length/2.0, global_pt0);
01430     taxel.centers_x.push_back(global_pt0[0]);
01431     taxel.centers_y.push_back(global_pt0[1]);
01432     taxel.centers_z.push_back(global_pt0[2]);
01433     taxel.link_names.push_back("link3");
01434 
01435     taxel_links.push_back("link3");
01436 
01437     dBodyVectorToWorld (link3->id(), 0.0, 0.0,  1.0, global_n0);
01438     taxel.normals_x.push_back(global_n0[0]);
01439     taxel.normals_y.push_back(global_n0[1]);
01440     taxel.normals_z.push_back(global_n0[2]);
01442 
01443 
01444     while ( k < num )
01445     {
01446         dVector3 global_pt;
01447         dVector3 global_pt2;
01448         dVector3 global_n;
01449         dVector3 global_n2;
01450 
01451         if (num >  1.0)
01452         {
01453             dBodyGetRelPointPos(link3->id(), (k/2)*step, 0.0,  -forearm_length/2.0, global_pt);         
01454             taxel.centers_x.push_back(global_pt[0]);
01455             taxel.centers_y.push_back(global_pt[1]);
01456             taxel.centers_z.push_back(global_pt[2]);
01457             taxel.link_names.push_back("link3");
01458 
01459             taxel_links.push_back("link3");
01460 
01461             dBodyGetRelPointPos(link3->id(), -(k/2)*step, 0.0, -forearm_length/2.0, global_pt2);                
01462             taxel.centers_x.push_back(global_pt2[0]);
01463             taxel.centers_y.push_back(global_pt2[1]);
01464             taxel.centers_z.push_back(global_pt2[2]);
01465             taxel.link_names.push_back("link3");
01466 
01467             taxel_links.push_back("link3");
01468 
01469             dBodyVectorToWorld (link3->id(), 0.0, 0.0,  -1.0,global_n);
01470             taxel.normals_x.push_back(global_n[0]);
01471             taxel.normals_y.push_back(global_n[1]);
01472             taxel.normals_z.push_back(global_n[2]);
01473 
01474             dBodyVectorToWorld (link3->id(), 0.0, 0.0,  -1.0, global_n2);
01475             taxel.normals_x.push_back(global_n2[0]);
01476             taxel.normals_y.push_back(global_n2[1]);
01477             taxel.normals_z.push_back(global_n2[2]);
01478 
01479             dBodyGetRelPointPos(link3->id(), (k/2)*step, 0.0, forearm_length/2.0, global_pt);           
01480             taxel.centers_x.push_back(global_pt[0]);
01481             taxel.centers_y.push_back(global_pt[1]);
01482             taxel.centers_z.push_back(global_pt[2]);
01483             taxel.link_names.push_back("link3");
01484 
01485             taxel_links.push_back("link3");
01486 
01487             dBodyGetRelPointPos(link3->id(), -(k/2)*step,  0.0, forearm_length/2.0, global_pt2);                
01488             taxel.centers_x.push_back(global_pt2[0]);
01489             taxel.centers_y.push_back(global_pt2[1]);
01490             taxel.centers_z.push_back(global_pt2[2]);
01491             taxel.link_names.push_back("link3");
01492 
01493             taxel_links.push_back("link3");
01494 
01495             dBodyVectorToWorld (link3->id(), 0.0, 0.0, 1.0, global_n);
01496             taxel.normals_x.push_back(global_n[0]);
01497             taxel.normals_y.push_back(global_n[1]);
01498             taxel.normals_z.push_back(global_n[2]);
01499 
01500             dBodyVectorToWorld (link3->id(), 0.0, 0.0, 1.0, global_n2);
01501             taxel.normals_x.push_back(global_n2[0]);
01502             taxel.normals_y.push_back(global_n2[1]);
01503             taxel.normals_z.push_back(global_n2[2]);
01504 
01505         }
01506 
01507         k = k+2;
01508     }
01509 
01512 
01513 
01514     //doing nearest neighbor to assign forces to discrete taxels
01515     std::vector < int > f_ind;
01516     for (unsigned int j = 0; j < skin.forces.size(); j++)
01517     {
01518         float min_distance = 10000;
01519         // sometimes the magnitude of the contact force is zero. This
01520         // causes ind_buf to be erroneously set to 0. In the skin
01521         // client (python code -- epc_skin.py), Advait is ignoring
01522         // forces less than 0.1N.
01523         // Previously, Advait ignored low forces within
01524         // demo_kinematic, initialized ind_buf to a -ve number and had
01525         // an assertion to detect strange things.
01526         // Unfortunately, only skin.forces and normals is getting
01527         // populated here. The rest happens in the nearCallback and so
01528         // there is no good way that Advait can think of of keeping
01529         // the link_names, locations and forces consistent. Removing
01530         // low forces in the python skin client appears to be an
01531         // okayish solution.
01532         int ind_buf = -134241;
01533 
01534         for (unsigned int k = 0; k <  taxel.centers_x.size(); k++)
01535         {
01536             float distance = sqrt(pow((taxel.centers_x[k]-skin.locations[j].x),2)+pow((taxel.centers_y[k]-skin.locations[j].y),2)+pow((taxel.centers_z[k]-skin.locations[j].z),2));
01537             if (distance < min_distance && taxel_links[k] == skin.link_names[j])
01538             {
01539                 dVector3 body_pt;
01540                 dVector3 body_taxel_pt;
01541                 float length(0);
01542                 float width(0);
01543 
01544                 if (skin.link_names[j] == "link1")
01545                 {
01546                     dBodyGetPosRelPoint (link1->id(), skin.locations[j].x, skin.locations[j].y, skin.locations[j].z, body_pt);
01547                     dBodyGetPosRelPoint (link1->id(), taxel.centers_x[k], taxel.centers_y[k], taxel.centers_z[k], body_taxel_pt);
01548                     length = torso_half_width/2.0;
01549                     width = torso_half_width_side/2.0;
01550                 }
01551                 else if (skin.link_names[j] == "link2")
01552                 {
01553                     dBodyGetPosRelPoint (link2->id(), skin.locations[j].x, skin.locations[j].y, skin.locations[j].z, body_pt);
01554                     dBodyGetPosRelPoint (link2->id(), taxel.centers_x[k], taxel.centers_y[k], taxel.centers_z[k], body_taxel_pt);
01555                     length = upper_arm_length/2.0;
01556                     width = upper_arm_width/2.0;
01557                 }
01558                 else if (skin.link_names[j] == "link3")
01559                 {
01560                     dBodyGetPosRelPoint (link3->id(), skin.locations[j].x, skin.locations[j].y, skin.locations[j].z, body_pt);
01561                     dBodyGetPosRelPoint (link3->id(), taxel.centers_x[k], taxel.centers_y[k], taxel.centers_z[k], body_taxel_pt);
01562                     length = forearm_length/2.0;
01563                     width = forearm_width/2.0;
01564                 }
01565 
01566                 double mag_norm = sqrt(taxel.normals_x[k]*taxel.normals_x[k]+taxel.normals_y[k]*taxel.normals_y[k]);
01567                 double mag_force = sqrt(skin.forces[j].x*skin.forces[j].x + skin.forces[j].y*skin.forces[j].y);
01568 
01569                 double dot_prod = (taxel.normals_x[k]*skin.forces[j].x + taxel.normals_y[k]*skin.forces[j].y);
01570                 double cos_angle = dot_prod/(mag_norm*mag_force);
01571                 double abs_angle_deg = abs(acos(cos_angle) * 180.0 / PI);
01572 
01573                 if (mag_force < 0.01)
01574                     abs_angle_deg = 0.;
01575 
01576                 // here the force is close to a corner. only
01577                 // using taxels for which the angle b/w the normal
01578                 // and the force vector is less than some
01579                 // threshold.
01580                 if (abs_angle_deg <= 45.0)
01581                 {
01582                     // we are guaranteed to have atleast one such
01583                     // taxel, because we have at least one taxel
01584                     // on both the front and side surfaces of the
01585                     // arm.
01586                     min_distance = distance;
01587                     ind_buf = k;
01588                 }
01589             }
01590         }
01591 
01592         assert(ind_buf >= 0);
01593         f_ind.push_back(ind_buf);
01594     }
01595 
01596     for (unsigned int k = 0; k < taxel.centers_x.size(); k++)
01597     {
01598         taxel.forces_x.push_back(0.0);
01599         taxel.forces_y.push_back(0.0);
01600         taxel.forces_z.push_back(0.0);
01601     }
01602 
01603     for (unsigned int j = 0; j <  f_ind.size(); j++)
01604     {
01605         taxel.forces_x[f_ind[j]] = taxel.forces_x[f_ind[j]] + skin.forces[j].x;
01606         taxel.forces_y[f_ind[j]] = taxel.forces_y[f_ind[j]] + skin.forces[j].y;
01607         taxel.forces_z[f_ind[j]] = taxel.forces_z[f_ind[j]] + skin.forces[j].z;
01608     }
01609 
01610     f_ind.clear();
01611     /*************************************************************************************/
01612     /*end of really bad code at least, all code above for taxel stuff needs drastic rework*/
01613     /****************************************************************************************/
01614 }
01615 
01616 
01617 void inner_torque_loop()
01618 {
01619     q[0] = hinge1->getAngle();
01620     q[1] = hinge2->getAngle();
01621     q[2] = hinge3->getAngle();
01622 
01623     q_dot[0] = hinge1->getAngleRate();
01624     q_dot[1] = hinge2->getAngleRate();
01625     q_dot[2] = hinge3->getAngleRate();
01626 
01627     torques[0]=(-k_p[0]*(q[0]-jep[0]) - k_d[0]*q_dot[0]);       
01628     torques[1]=(-k_p[1]*(q[1]-jep[1]) - k_d[1]*q_dot[1]);       
01629     torques[2]=(-k_p[2]*(q[2]-jep[2]) - k_d[2]*q_dot[2]);      
01630 
01631     if (include_mobile_base != 0)
01632     {
01633         const dReal *base_pos, *base_vel;
01634         base_pos = body_mobile_base->getPosition();
01635         base_vel = body_mobile_base->getLinearVel();
01636 
01637         for(int i=0; i<3; i++)
01638             mobile_base_generalized_forces[i]=(-mobile_base_k_p[i]*(base_pos[i]-mobile_base_ep[i])
01639                     -mobile_base_k_d[i]*base_vel[i]);
01640 
01641         body_mobile_base->addRelForce(mobile_base_generalized_forces[0],
01642                 mobile_base_generalized_forces[1],
01643                 0);
01644         body_mobile_base->addRelTorque(0, 0, mobile_base_generalized_forces[2]);
01645     }
01646 }
01647 
01648 
01649 int main(int argc, char **argv)
01650 {
01651     ros::init(argc, argv, "sim_arm");
01652 
01653     ros::NodeHandle n;
01654     ros::NodeHandle private_nh("~");
01655 
01656     ros::Publisher skin_pub = n.advertise<hrl_haptic_manipulation_in_clutter_msgs::SkinContact>("/skin/contacts", 100);
01657     ros::Publisher jep_pub = n.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/jep", 100);
01658     ros::Publisher angles_pub = n.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/joint_angles", 100);
01659     ros::Publisher angle_rates_pub = n.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/joint_angle_rates", 100);
01660     ros::Publisher bodies_draw = n.advertise<hrl_haptic_manipulation_in_clutter_msgs::BodyDraw>("/sim_arm/body_visualization", 100);
01661     ros::Publisher taxel_pub = n.advertise<hrl_haptic_manipulation_in_clutter_msgs::TaxelArray>("/skin/taxel_array", 100);
01662     ros::Publisher imped_pub = n.advertise<hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams>("sim_arm/joint_impedance", 100);
01663 
01664     // x, y, angle of mobile base.
01665     tf::TransformBroadcaster br;
01666     tf::Transform tf_transform;
01667     ros::Publisher odom_pub = n.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/odometry", 100);
01668     ros::Publisher base_ep_pub = n.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/base_ep", 100);
01669     ROS_WARN("Not publishing angle in odometry\n");
01670 
01671     ros::Subscriber s1 = n.subscribe("/sim_arm/command/jep", 100, jep_cb);
01672     ros::Subscriber s2 = n.subscribe("/sim_arm/command/base_ep", 100, base_ep_cb);
01673     ros::Subscriber s3 = n.subscribe("/sim_arm/command/joint_impedance", 100, ROSCallback_impedance);
01674 
01675     const dReal timestep = 0.0005;
01676     ros::Publisher clock_pub = n.advertise<rosgraph_msgs::Clock>("/clock", 1/timestep);
01677 
01678     double cur_time = 0;
01679     int torque_step = 0;
01680     int q_pub_step = 0;
01681     int clock_pub_step = 0;
01682     int skin_step = 0;
01683 
01684     private_nh.param<int>("include_mobile_base", include_mobile_base, 0);
01685 
01686     int resol;
01687     //    float resolution;
01688     printf("waiting for /m3/software_testbed/resolution\n");
01689     while (n.getParam("/m3/software_testbed/resolution", resol) == false)
01690         sleep(0.1);
01691     printf("Done waiting\n");
01692 
01693     double resolution = (double) resol;
01694 
01695     dInitODE();
01696     // setup pointers to drawstuff callback functions
01697 
01698     space = new dSimpleSpace();
01699 
01700     world = new dWorld;
01701     world->setGravity(0, 0, -9.8);
01702 
01703     // make robot and go to starting configuration.
01704     create_robot(n);
01705     ROS_INFO("Before going to initial position");
01706     go_initial_position(n); // initial jep defined inside this function.
01707     ROS_INFO("After going to initial position");
01708 
01709     // add obstacles.
01710     create_movable_obstacles(n); //call this first for stupid ROS param server sync.
01711     create_compliant_obstacles(n);
01712     create_fixed_obstacles(n);
01713 
01714     printf("Starting Simulation now \n");
01715     double t_now = get_wall_clock_time() - timestep;
01716     double t_expected;
01717 
01718     while (ros::ok())    
01719     {
01720         space->collide(0, nearCallback);
01721 
01722         // simulation will not run faster than real-time.
01723         t_expected = t_now + timestep;
01724         t_now = get_wall_clock_time();
01725         if (t_now < t_expected)
01726             usleep((t_expected - t_now)*1000000. + 0.5);
01727 
01728         world->step(timestep);
01729         cur_time += timestep;
01730         rosgraph_msgs::Clock c;
01731         c.clock.sec = int(cur_time);
01732         c.clock.nsec = int(1000000000*(cur_time-int(cur_time)));
01733 
01734 
01735         geometry_msgs::Vector3 force;   
01736         geometry_msgs::Vector3 normal;  
01737 
01738         assert(fbnum <= MAX_FEEDBACKNUM);
01739 
01740         dVector3 sum = {0, 0, 0};
01741 
01742         for (int i=0; i<fbnum; i++) 
01743         {
01744             dReal *f = feedbacks[i].fb.f2;
01745             sum[0] += f[0] * force_sign[i];
01746             sum[1] += f[1] * force_sign[i];
01747             sum[2] += f[2] * force_sign[i];
01748 
01749             double f_mag = sqrt(sum[0]*sum[0]+sum[1]*sum[1]+sum[2]*sum[2]);
01750             if (i < fbnum-1)
01751             {
01752                 if (force_grouping[i] != force_grouping[i+1])
01753                 {
01754                     force.x = sum[0];
01755                     force.y = sum[1];
01756                     force.z = sum[2];
01757                     skin.forces.push_back(force);
01758 
01759                     // hacky code by Advait to add normals to the
01760                     // SkinContact message
01761                     normal.x = force.x / f_mag;
01762                     normal.y = force.y / f_mag;
01763                     normal.z = force.z / f_mag;
01764                     skin.normals.push_back(normal);
01765                     sum[0] = 0;
01766                     sum[1] = 0;
01767                     sum[2] = 0;
01768                 }
01769                 else
01770                 {
01771                     ROS_WARN("Advait believes that this should never happen\n");
01772                     exit(0);
01773                 }
01774             }                   
01775             else
01776             {
01777                 force.x = sum[0];
01778                 force.y = sum[1];
01779                 force.z = sum[2];
01780                 skin.forces.push_back(force);
01781 
01782                 // hacky code by Advait to add normals to the
01783                 // SkinContact message
01784                 normal.x = force.x / f_mag;
01785                 normal.y = force.y / f_mag;
01786                 normal.z = force.z / f_mag;
01787                 skin.normals.push_back(normal);
01788 
01789                 sum[0] = 0;
01790                 sum[1] = 0;
01791                 sum[2] = 0;
01792             }
01793         }
01794 
01795 
01796         torque_step++;
01797         q_pub_step++;
01798         clock_pub_step++;
01799         skin_step++;
01800 
01801         if (clock_pub_step >= 0.002/timestep)
01802         {
01803             clock_pub.publish(c);
01804             clock_pub_step = 0;
01805         }
01806 
01807         for (int l = 0; l<num_used_movable; l++)
01808         {
01809           const dReal *tot_force = dBodyGetForce(obstacles[l].id());
01810           dReal *fric_force = frict_feedbacks[l].fb.f1;
01811           double force_xy_mag = sqrt((tot_force[0]-fric_force[0])*(tot_force[0]-fric_force[0])
01812                                      +(tot_force[1]-fric_force[1])*(tot_force[1]-fric_force[1]));
01813           if (force_xy_mag>0)
01814             {
01815               dJointSetPlane2DXParam(plane2d_joint_ids[l], dParamFMax, 
01816                                      max_friction*abs(tot_force[0]-fric_force[0])/force_xy_mag);
01817               dJointSetPlane2DYParam(plane2d_joint_ids[l], dParamFMax, 
01818                                      max_friction*abs(tot_force[1]-fric_force[1])/force_xy_mag);
01819             }
01820           else
01821             {
01822               dJointSetPlane2DXParam(plane2d_joint_ids[l], dParamFMax, 
01823                                      max_friction*0.707);
01824               dJointSetPlane2DYParam(plane2d_joint_ids[l], dParamFMax, 
01825                                      max_friction*0.707);
01826             }
01827 
01828           dJointSetPlane2DAngleParam(plane2d_joint_ids[l], dParamFMax, max_tor_friction);
01829           dJointSetPlane2DXParam(plane2d_joint_ids[l], dParamVel, 0.0);
01830           dJointSetPlane2DYParam(plane2d_joint_ids[l], dParamVel, 0.0);
01831           dJointSetPlane2DAngleParam(plane2d_joint_ids[l], dParamVel, 0.0);
01832         }
01833 
01834         for (int l = 0; l<num_used_compliant; l++)
01835         {
01836           dJointSetPlane2DXParam(compliant_plane2d_joint_ids[l], dParamFMax, 0);
01837           dJointSetPlane2DYParam(compliant_plane2d_joint_ids[l], dParamFMax, 0);
01838           const dReal *cur_pos;
01839           cur_pos = dBodyGetPosition(compliant_obstacles[l].id());
01840           const dReal *cur_vel;
01841           cur_vel = dBodyGetLinearVel(compliant_obstacles[l].id());
01842         
01843           //this is the control law used to simulate compliant objects with critical damping
01844           dReal Fx = (obst_home[l][0]-cur_pos[0])*obst_stiffness[l] - cur_vel[0]*obst_damping[l];
01845           dReal Fy = (obst_home[l][1]-cur_pos[1])*obst_stiffness[l] - cur_vel[1]*obst_damping[l];
01846               
01847           dBodyAddForce(compliant_obstacles[l].id(), Fx, Fy, 0);
01848           dBodyAddForce(compliant_obstacles[l].id(), 0, 0, 0);
01849         }
01850 
01851 
01852         //computing torques at 1 kHz but applying at every simulation time step
01853         //because ode zeros the "accumulators" after every time step
01854         if (torque_step >= 0.001/timestep)
01855         {
01856             inner_torque_loop();
01857             torque_step = 0;
01858         }
01859         hinge1->addTorque(torques[0]);
01860         hinge2->addTorque(torques[1]);  
01861         hinge3->addTorque(torques[2]);          
01862         //-----------------------------------------------------------------------
01863 
01864 
01865         if (q_pub_step >= 0.01/timestep)
01866         {
01867             hrl_msgs::FloatArrayBare angles;
01868             hrl_msgs::FloatArrayBare angle_rates;
01869 
01870             angles.data = q;
01871             angle_rates.data = q_dot;
01872 
01873             angles_pub.publish(angles);
01874             angle_rates_pub.publish(angle_rates);
01875 
01876             hrl_msgs::FloatArrayBare jep_ros;
01877             jep_ros.data = jep;
01878             jep_pub.publish(jep_ros);
01879 
01880             q_pub_step = 0;
01881 
01882             if (include_mobile_base != 0)
01883             {
01884                 const dReal *base_pos, *base_vel;
01885                 base_pos = body_mobile_base->getPosition();
01886                 base_vel = body_mobile_base->getLinearVel();
01887 
01888                 hrl_msgs::FloatArrayBare bp;
01889                 bp.data.resize(3);
01890                 bp.data[0] = base_pos[0]; // x
01891                 bp.data[1] = base_pos[1]; // y
01892                 bp.data[2] = 0.; // ang
01893                 odom_pub.publish(bp);
01894 
01895                 tf_transform.setOrigin(tf::Vector3(base_pos[0],
01896                                              base_pos[1], 0.0));
01897                 tf_transform.setRotation(tf::Quaternion(base_pos[2], 0,
01898                                                         0));
01899                 br.sendTransform(tf::StampedTransform(tf_transform,
01900                                      ros::Time::now(), "/world",
01901                                      "/torso_lift_link"));
01902 
01903                 hrl_msgs::FloatArrayBare bep;
01904                 bep.data.resize(3);
01905                 bep.data[0] = mobile_base_ep[0];
01906                 bep.data[1] = mobile_base_ep[1];
01907                 bep.data[2] = mobile_base_ep[2];
01908                 base_ep_pub.publish(bep);
01909 
01910             }
01911             else
01912               {
01913                 tf_transform.setOrigin(tf::Vector3(0.0,
01914                                                    0.0, 
01915                                                    0.0));
01916                 tf_transform.setRotation(tf::Quaternion(0.0, 
01917                             0.0,
01918                             0.0));
01919                 br.sendTransform(tf::StampedTransform(tf_transform,
01920                                      ros::Time::now(), "/world",
01921                                      "/torso_lift_link"));
01922               }
01923 
01924 
01925 
01926 
01927         }
01928 
01929         if (skin_step >= 0.01/timestep)
01930         { 
01931             // this block of code gets executed every 10ms (100Hz)
01932             // simulator time.
01933 
01934             // unsure what this is.
01935             impedance_params.header.frame_id = "/world";
01936             impedance_params.header.stamp = ros::Time::now();
01937             impedance_params.k_p.data = k_p;
01938             impedance_params.k_d.data = k_d;
01939             imped_pub.publish(impedance_params);
01940 
01941             // publishing skin msg. (maybe perform the computation,
01942             // taxel resolution etc at a 100Hz as well?)
01943             skin.header.frame_id = "/torso_lift_link";
01944             skin.header.stamp = ros::Time::now();
01945             skin_pub.publish(skin);
01946 
01947             taxel_simulation_code(resolution);
01948             taxel_pub.publish(taxel);
01949 
01950             taxel.centers_x.clear();
01951             taxel.centers_y.clear();
01952             taxel.centers_z.clear();
01953             taxel.normals_x.clear();
01954             taxel.normals_y.clear();
01955             taxel.normals_z.clear();
01956             taxel.forces_x.clear();
01957             taxel.forces_y.clear();
01958             taxel.forces_z.clear();
01959             taxel.link_names.clear();
01960 
01961             //--------------------------------------------------------- 
01962             // visualize bodies in rviz.
01963             // will publish data that a python script will use to the
01964             // publish rviz markers.
01965             //---------------------------------------------------------
01966 
01967             hrl_msgs::FloatArrayBare link_pos_ar;
01968             hrl_msgs::FloatArrayBare link_rot_ar;
01969             hrl_msgs::FloatArrayBare obst_pos_ar;
01970             hrl_msgs::FloatArrayBare obst_rot_ar;
01971 
01972             const dReal *position = link1->getPosition();
01973             const dReal *rotation = link1->getRotation();
01974             std::vector<double> pos_vec(3);
01975             std::vector<double> rot_vec(12);
01976             for (int l = 0; l<3; l++)
01977             {
01978                 position = dBodyGetPosition(link_ids[l]);
01979                 rotation = dBodyGetRotation(link_ids[l]);
01980                 pos_vec[0] = position[0];
01981                 pos_vec[1] = position[1];
01982                 pos_vec[2] = position[2];
01983                 link_pos_ar.data = pos_vec;
01984                 draw.link_loc.push_back(link_pos_ar);
01985                 for (int k = 0; k<12; k++)
01986                 {
01987                     rot_vec[k] = rotation[k];
01988                 }
01989                 link_rot_ar.data = rot_vec;
01990                 draw.link_rot.push_back(link_rot_ar);
01991             }
01992 
01993             // movable obstacles.
01994             for (int l = 0; l<num_used_movable; l++)
01995             {
01996                 position = dBodyGetPosition(obstacles[l].id());
01997                 rotation = dBodyGetRotation(obstacles[l].id());
01998                 pos_vec[0] = position[0];
01999                 pos_vec[1] = position[1];
02000                 pos_vec[2] = position[2];
02001                 obst_pos_ar.data = pos_vec;
02002                 draw.obst_loc.push_back(obst_pos_ar);
02003 
02004                 for (int k = 0; k<12; k++)
02005                     rot_vec[k] = rotation[k];
02006 
02007                 obst_rot_ar.data = rot_vec;
02008                 draw.obst_rot.push_back(obst_rot_ar);
02009             }
02010 
02011             //compliant obstacles
02012             for (int l = 0; l<num_used_compliant; l++)
02013             {
02014                 position = dBodyGetPosition(compliant_obstacles[l].id());
02015                 rotation = dBodyGetRotation(compliant_obstacles[l].id());
02016                 pos_vec[0] = position[0];
02017                 pos_vec[1] = position[1];
02018                 pos_vec[2] = position[2];
02019                 obst_pos_ar.data = pos_vec;
02020                 draw.obst_loc.push_back(obst_pos_ar);
02021 
02022                 for (int k = 0; k<12; k++)
02023                     rot_vec[k] = rotation[k];
02024 
02025                 obst_rot_ar.data = rot_vec;
02026                 draw.obst_rot.push_back(obst_rot_ar);
02027             }
02028 
02029 
02030             // fixed obstacles.
02031             for (int l = 0; l<num_used_fixed; l++)
02032             {
02033                 position = dBodyGetPosition(fixed_obstacles[l].id());
02034                 rotation = dBodyGetRotation(fixed_obstacles[l].id());
02035                 pos_vec[0] = position[0];
02036                 pos_vec[1] = position[1];
02037                 pos_vec[2] = position[2];
02038                 obst_pos_ar.data = pos_vec;
02039                 draw.obst_loc.push_back(obst_pos_ar);
02040 
02041                 for (int k = 0; k<12; k++)
02042                     rot_vec[k] = rotation[k];
02043 
02044                 obst_rot_ar.data = rot_vec;
02045                 draw.obst_rot.push_back(obst_rot_ar);
02046             }
02047 
02048             draw.header.frame_id = "/world";
02049             draw.header.stamp = ros::Time::now();
02050             bodies_draw.publish(draw);
02051             skin_step = 0;
02052 
02053             // clear for the next time.
02054             draw.link_loc.clear();
02055             draw.link_rot.clear();
02056             draw.obst_loc.clear();
02057             draw.obst_rot.clear();
02058         }
02059 
02060         skin.pts_x.clear();
02061         skin.pts_y.clear();
02062         skin.pts_z.clear();
02063         skin.link_names.clear();
02064         skin.locations.clear();
02065         skin.forces.clear();
02066         skin.normals.clear();
02067         force_grouping.clear();
02068         force_sign.clear();
02069         joints.clear(); 
02070 
02071         fbnum = 0;
02072         force_group = 0;
02073 
02074         ros::spinOnce();
02075     }
02076 
02077     dCloseODE();
02078 }
02079 
02080 


hrl_software_simulation_darpa_m3
Author(s): Marc Killpack and Advait Jain. Advisor: Prof. Charlie Kemp. Healthcare Robotics Lab, Georgia Tech
autogenerated on Wed Nov 27 2013 11:35:07