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 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);
00066 std::vector<double> mobile_base_generalized_forces(3, 0);
00067
00068
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;
00076 float torso_half_width_side = 0;
00077 float upper_arm_length = 0.334;
00078 float upper_arm_width = 0;
00079 float forearm_length = 0;
00080 float forearm_width = 0;
00081
00083
00084
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
00104
00105
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
00143
00144
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
00159
00160
00161
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
00182 return;
00183 }
00184 }
00185
00186 if (b1_is_link && b2_is_link)
00187
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];
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
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
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
00299 mobile_base_ep[0] = 0;
00300 mobile_base_ep[1] = 0;
00301 mobile_base_ep[2] = 0;
00302
00303
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
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
00380
00381
00382
00383
00384
00385
00386 link1 = new dBody(*world);
00387 dMass mass_l1;
00388
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
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
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
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];
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
00441 if (include_mobile_base != 0)
00442 {
00443 mobile_base_plane2d_jt_id = dJointCreatePlane2D(world->id(), 0);
00444
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
00486 joints.create();
00487 }
00488
00489 void create_movable_obstacles(ros::NodeHandle n)
00490 {
00491 float obstacle_mass = 1.;
00492
00493
00494
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
00549
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
00573
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
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
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
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
00659
00660
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
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
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
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
01287 std::vector < int > f_ind;
01288 for (unsigned int j = 0; j < skin.forces.size(); j++)
01289 {
01290 float min_distance = 10000;
01291
01292
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
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
01349
01350
01351
01352 if (abs_angle_deg <= 45.0)
01353 {
01354
01355
01356
01357
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
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
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
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
01469
01470 space = new dSimpleSpace();
01471
01472 world = new dWorld;
01473 world->setGravity(0, 0, -9.8);
01474
01475
01476 create_robot(n);
01477 ROS_INFO("Before going to initial position");
01478 go_initial_position(n);
01479 ROS_INFO("After going to initial position");
01480
01481
01482 create_movable_obstacles(n);
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
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
01528
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
01551
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
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
01624
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];
01662 bp.data[1] = base_pos[1];
01663 bp.data[2] = 0.;
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
01703
01704
01705
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
01713
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
01734
01735
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
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
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
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
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