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


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