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
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
00023
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);
00067 std::vector<double> mobile_base_generalized_forces(3, 0);
00068
00069
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;
00077 float torso_half_width_side = 0;
00078 float upper_arm_length = 0.334;
00079 float upper_arm_width = 0;
00080 float forearm_length = 0;
00081 float forearm_width = 0;
00082
00084
00085
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
00105
00106
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
00147
00148
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
00163
00164
00165
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
00186 return;
00187 }
00188 }
00189
00190 if (b1_is_link && b2_is_link)
00191
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];
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
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
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
00303 mobile_base_ep[0] = 0;
00304 mobile_base_ep[1] = 0;
00305 mobile_base_ep[2] = 0;
00306
00307
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
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
00387
00388
00389
00390
00391
00392 dMatrix3 cyl_rotate = {1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0};
00393
00394
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
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
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];
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
00506 if (include_mobile_base != 0)
00507 {
00508 mobile_base_plane2d_jt_id = dJointCreatePlane2D(world->id(), 0);
00509
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
00551 joints.create();
00552 }
00553
00554 void create_movable_obstacles(ros::NodeHandle n)
00555 {
00556 float obstacle_mass = 1.;
00557
00558
00559
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
00614
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
00638
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
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
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
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
00724
00725
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
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
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
01185
01186
01187
01188
01189
01190
01191
01192
01193
01194
01195
01196
01197
01198
01199
01200
01201
01202
01203
01204
01205
01206
01207
01208
01209
01210
01211
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221
01222
01223
01224
01225
01226
01227
01228
01229
01230
01231
01232
01233
01234
01235
01236
01237
01238
01239
01240
01241
01242
01243
01244
01245
01246
01247
01248
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271
01272
01273
01274
01275
01276
01277
01278
01279
01280
01281
01282
01283
01284
01285
01286
01287
01288
01289
01290
01291
01292
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313
01314
01315
01316
01317
01318
01319
01320
01321
01322
01323
01324
01325
01326
01327
01328
01329
01330
01331
01332
01333
01334
01335
01336
01337
01338
01339
01340
01341
01342
01343
01344
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
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
01515 std::vector < int > f_ind;
01516 for (unsigned int j = 0; j < skin.forces.size(); j++)
01517 {
01518 float min_distance = 10000;
01519
01520
01521
01522
01523
01524
01525
01526
01527
01528
01529
01530
01531
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
01577
01578
01579
01580 if (abs_angle_deg <= 45.0)
01581 {
01582
01583
01584
01585
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
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
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
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
01697
01698 space = new dSimpleSpace();
01699
01700 world = new dWorld;
01701 world->setGravity(0, 0, -9.8);
01702
01703
01704 create_robot(n);
01705 ROS_INFO("Before going to initial position");
01706 go_initial_position(n);
01707 ROS_INFO("After going to initial position");
01708
01709
01710 create_movable_obstacles(n);
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
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
01760
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
01783
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
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
01853
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];
01891 bp.data[1] = base_pos[1];
01892 bp.data[2] = 0.;
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
01932
01933
01934
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
01942
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
01963
01964
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
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
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
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
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