00001 #include "ros/ros.h"
00002 #include <boost/thread/mutex.hpp>
00003 #include "hrl_haptic_manipulation_in_clutter_msgs/SkinContact.h"
00004 #include "hrl_haptic_manipulation_in_clutter_msgs/BodyDraw.h"
00005 #include "hrl_haptic_manipulation_in_clutter_msgs/TaxelArray.h"
00006 #include "hrl_haptic_manipulation_in_clutter_msgs/MechanicalImpedanceParams.h"
00007 #include "hrl_msgs/FloatArrayBare.h"
00008 #include "std_msgs/String.h"
00009 #include <tf/transform_broadcaster.h>
00010 #include "rosgraph_msgs/Clock.h"
00011 #include <cmath>
00012 #include <vector>
00013 #include <string>
00014 #include <iostream>
00015 #include <set>
00016 #include <algorithm>
00017 #include <functional>
00018 #include <ode/ode.h>
00019 #include <sstream>
00020
00021 #ifdef dDOUBLE
00022 #define MAX_CONTACTS 20 // maximum number of contact points per body
00023 #define MAX_FEEDBACKNUM 100
00024 #define NUM_OBST 1000
00025 #define MAX_NUM_REV 30
00026 #define MAX_NUM_PRISM 30
00027 #define PI 3.14159265
00028 #endif
00029
00030 struct MyFeedback {
00031 dJointFeedback fb;
00032 };
00033
00034
00035 class Simulator{
00036 public:
00037
00038 Simulator(ros::NodeHandle &nh);
00039 ~Simulator();
00040 void JepCallback(const hrl_msgs::FloatArrayBare msg);
00041 void ImpedanceCallback(const hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams msg);
00042 void BaseEpCallback(const hrl_msgs::FloatArrayBare msg);
00043 void create_fixed_obstacles();
00044 void create_movable_obstacles();
00045 void create_compliant_obstacles();
00046 void create_robot();
00047 void sense_forces();
00048 void go_initial_position();
00049 void calc_torques();
00050 void set_torques();
00051 static void nearCallback (void *data, dGeomID o1, dGeomID o2);
00052 void classCallback (dGeomID o1, dGeomID o2);
00053 void publish_angle_data();
00054 void publish_imped_skin_viz();
00055 void update_linkage_viz();
00056 void inner_torque_loop();
00057 void update_friction_and_obstacles();
00058 void get_joint_data();
00059 void clear();
00060 void update_taxel_simulation();
00061 void update_proximity_simulation();
00062 double get_dist(double x1, double y1, double x2, double y2, double radius);
00063 void setup_current_taxel_config(hrl_haptic_manipulation_in_clutter_msgs::TaxelArray &taxel);
00064 ros::Publisher clock_pub;
00065 dSimpleSpace space;
00066 dWorld world;
00067 static const dReal timestep=0.0005;
00068 double cur_time;
00069
00070 protected:
00071 dHingeJoint manip_rev_jts[MAX_NUM_REV];
00072 dHingeJoint base_rev_jts[MAX_NUM_REV];
00073 dSliderJoint manip_pris_jts[MAX_NUM_PRISM];
00074 dSliderJoint base_pris_jts[MAX_NUM_PRISM];
00075 dBody obstacles[NUM_OBST];
00076 dBody fixed_obstacles[NUM_OBST];
00077 dBody compliant_obstacles[NUM_OBST];
00078 double obst_home[NUM_OBST][3];
00079 double obst_stiffness[NUM_OBST];
00080 double obst_damping[NUM_OBST];
00081
00082 dBody links_arr[100];
00083 double links_dim[100][3];
00084 std::string links_shape[100];
00085 dBox g_link_box[100];
00086 dCapsule g_link_cap[100];
00087 dBodyID link_ids[100];
00088 dBodyID fixed_obst_ids[NUM_OBST];
00089 dJointID plane2d_joint_ids[NUM_OBST];
00090 dJointID compliant_plane2d_joint_ids[NUM_OBST];
00091 dJointID fixed_joint_ids[NUM_OBST];
00092
00093 dBody *body_mobile_base;
00094 dBox *geom_mobile_base;
00095 dJointID mobile_base_plane2d_jt_id;
00096
00097 hrl_msgs::FloatArrayBare angles;
00098 hrl_msgs::FloatArrayBare angle_rates;
00099 hrl_msgs::FloatArrayBare jep_ros;
00100
00101 bool use_prox_sensor;
00102 int num_links;
00103 int num_jts;
00104 double resolution;
00105 ros::NodeHandle nh_;
00106 MyFeedback feedbacks[MAX_FEEDBACKNUM];
00107 MyFeedback frict_feedbacks[NUM_OBST];
00108 int fbnum;
00109 int force_group;
00110 double max_friction;
00111 double max_tor_friction;
00112 std::vector<double> q;
00113 std::vector<double> q_dot;
00114 std::vector<double> jep;
00115 std::vector<double> k_p;
00116 std::vector<double> k_d;
00117 std::vector<double> torques;
00118
00119
00120
00121
00122
00123
00124
00125 std::vector<int> force_grouping;
00126 std::vector<int> force_sign;
00127 std::vector<double> pt_x;
00128 std::vector<double> pt_y;
00129 std::vector<double> pt_z;
00130
00131
00132 std::vector<double> x_c;
00133 std::vector<double> y_c;
00134 std::vector<double> z_c;
00135 std::vector<double> x_n;
00136 std::vector<double> y_n;
00137 std::vector<double> z_n;
00138
00139 dJointGroup joints;
00140 dBody *env;
00141
00142 hrl_haptic_manipulation_in_clutter_msgs::SkinContact skin;
00143 hrl_haptic_manipulation_in_clutter_msgs::BodyDraw draw;
00144 hrl_haptic_manipulation_in_clutter_msgs::TaxelArray force_taxel;
00145 hrl_haptic_manipulation_in_clutter_msgs::TaxelArray proximity_taxel;
00146 hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams impedance_params;
00147
00148 dSliderJoint *slider_x;
00149 dSliderJoint *slider_y;
00150 dFixedJoint *fixed_joint;
00151 dContactJoint *jt_contact;
00152 ros::Publisher angles_pub;
00153 ros::Publisher angle_rates_pub;
00154 ros::Publisher bodies_draw;
00155 ros::Publisher force_taxel_pub;
00156 ros::Publisher proximity_taxel_pub;
00157 ros::Publisher imped_pub;
00158 ros::Publisher skin_pub;
00159 ros::Publisher jep_pub;
00160
00161 int num_used_movable;
00162 int num_used_fixed;
00163 int num_used_compliant;
00164
00165 boost::mutex m;
00166
00167 };
00168
00169 Simulator::Simulator(ros::NodeHandle &nh) :
00170 nh_(nh)
00171 {
00172 num_used_movable = NUM_OBST;
00173 num_used_fixed = NUM_OBST;
00174 num_used_compliant = NUM_OBST;
00175
00176
00177 cur_time = 0.0;
00178
00179 resolution = 0;
00180 use_prox_sensor = false;
00181 num_links = 0;
00182 num_jts = 0;
00183 while (nh_.getParam("/m3/software_testbed/linkage/num_links", num_links) == false)
00184 sleep(0.1);
00185 while (nh_.getParam("/m3/software_testbed/joints/num_joints", num_jts) == false)
00186 sleep(0.1);
00187 while (nh_.getParam("/use_prox_sensor", use_prox_sensor) == false)
00188 sleep(0.1);
00189 while (nh_.getParam("/m3/software_testbed/resolution", resolution) == false)
00190 sleep(0.1);
00191 fbnum=0;
00192 force_group=0;
00193 max_friction = 2;
00194 max_tor_friction = 0.5;
00195 angles_pub = nh_.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/joint_angles", 100);
00196 angle_rates_pub = nh_.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/joint_angle_rates", 100);
00197 bodies_draw = nh_.advertise<hrl_haptic_manipulation_in_clutter_msgs::BodyDraw>("/sim_arm/bodies_visualization", 100);
00198 force_taxel_pub = nh_.advertise<hrl_haptic_manipulation_in_clutter_msgs::TaxelArray>("/skin/taxel_array", 100);
00199 proximity_taxel_pub = nh_.advertise<hrl_haptic_manipulation_in_clutter_msgs::TaxelArray>("/haptic_mpc/simulation/proximity/taxel_array", 100);
00200 imped_pub = nh_.advertise<hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams>("sim_arm/joint_impedance", 100);
00201 skin_pub = nh_.advertise<hrl_haptic_manipulation_in_clutter_msgs::SkinContact>("/skin/contacts", 100);
00202 jep_pub = nh_.advertise<hrl_msgs::FloatArrayBare>("/sim_arm/jep", 100);
00203 clock_pub = nh_.advertise<rosgraph_msgs::Clock>("/clock", 1/timestep);
00204
00205 m.lock();
00206 for (int ii = 0; ii < num_jts; ii++)
00207 {
00208 q.push_back(0);
00209 q_dot.push_back(0);
00210 jep.push_back(0);
00211 k_p.push_back(0);
00212 k_d.push_back(0);
00213 torques.push_back(0);
00214 }
00215 m.unlock();
00216 }
00217
00218 Simulator::~Simulator()
00219 {
00220 }
00221
00222 using namespace std;
00223
00224
00225 void Simulator::JepCallback(const hrl_msgs::FloatArrayBare msg)
00226 {
00227 m.lock();
00228 jep = msg.data;
00229 m.unlock();
00230 }
00231
00232 void Simulator::ImpedanceCallback(const hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams msg)
00233 {
00234 m.lock();
00235 k_p = msg.k_p.data;
00236 k_d = msg.k_d.data;
00237 m.unlock();
00238 }
00239
00240 void Simulator::nearCallback(void *data, dGeomID o1, dGeomID o2)
00241 {
00242 Simulator* obj = (Simulator*) data;
00243 int i;
00244 dBodyID b1 = dGeomGetBody(o1);
00245 dBodyID b2 = dGeomGetBody(o2);
00246
00247 if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
00248
00249 bool b1_is_link = false;
00250 bool b2_is_link = false;
00251
00252 for (int i=0; i<obj->num_links; i++)
00253 {
00254 if (obj->link_ids[i] == b1)
00255 b1_is_link = true;
00256 if (obj->link_ids[i] == b2)
00257 b2_is_link = true;
00258 }
00259
00260
00261 if (b1_is_link && b2_is_link)
00262 return;
00263
00264 bool arm_contact = false;
00265 stringstream ss;
00266 bool is_b1 = false;
00267
00268 for (int i=0; i<obj->num_links; i++)
00269 {
00270 if (obj->link_ids[i] == b1 || obj->link_ids[i] == b2)
00271 {
00272 arm_contact = true;
00273 ss <<"link"<<i+1;
00274 if (obj->link_ids[i] == b1)
00275 is_b1 = true;
00276 else
00277 is_b1 = false;
00278 }
00279 }
00280
00281 dContact contact[MAX_CONTACTS];
00282 int numc = dCollide (o1, o2, MAX_CONTACTS, &(contact[0].geom), sizeof(dContact));
00283 if (numc > 0)
00284 {
00285 if (arm_contact == false)
00286 {
00287 for (int i=0; i<numc; i++)
00288 {
00289 contact[i].surface.mode = dContactSoftCFM | dContactApprox1 | dContactSoftERP;
00290 contact[i].surface.mu = 0.2;
00291 contact[i].surface.mu2 = 0.2;
00292
00293 contact[i].surface.soft_cfm = 0.04;
00294 contact[i].surface.soft_erp = 0.96;
00295
00296 dJointID c = dJointCreateContact (obj->world,obj->joints.id(), &contact[i]);
00297 dJointAttach (c, dGeomGetBody(contact[i].geom.g1),
00298 dGeomGetBody(contact[i].geom.g2));
00299 }
00300 }
00301 else
00302 {
00303 dVector3 contact_loc = {0, 0, 0};
00304 obj->skin.link_names.push_back(ss.str());
00305
00306 for (i=0; i<numc; i++)
00307 {
00308 contact[i].surface.mode = dContactSoftCFM | dContactApprox1 | dContactSoftERP;
00309 contact[i].surface.mu = 0.4;
00310 contact[i].surface.mu2 = 0.4;
00311
00312 contact[i].surface.soft_cfm = 0.04;
00313 contact[i].surface.soft_erp = 0.96;
00314
00315 contact_loc[0] = contact_loc[0]+contact[i].geom.pos[0];
00316 contact_loc[1] = contact_loc[1]+contact[i].geom.pos[1];
00317 contact_loc[2] = contact_loc[2]+contact[i].geom.pos[2];
00318
00319 obj->pt_x.push_back(contact[i].geom.pos[0]);
00320 obj->pt_y.push_back(contact[i].geom.pos[1]);
00321 obj->pt_z.push_back(contact[i].geom.pos[2]);
00322
00323 dJointID c = dJointCreateContact (obj->world,obj->joints.id(),&contact[i]);
00324 dJointAttach (c,b1,b2);
00325
00326 if (obj->fbnum < MAX_FEEDBACKNUM)
00327 {
00328 dJointSetFeedback (c, &obj->feedbacks[obj->fbnum++].fb);
00329 obj->force_grouping.push_back(obj->force_group);
00330 if (is_b1 == true)
00331 obj->force_sign.push_back(1);
00332 else
00333 obj->force_sign.push_back(-1);
00334 }
00335 }
00336
00337 contact_loc[0] = contact_loc[0]/numc;
00338 contact_loc[1] = contact_loc[1]/numc;
00339 contact_loc[2] = contact_loc[2]/numc;
00340 geometry_msgs::Point con_pt;
00341 con_pt.x = contact_loc[0];
00342 con_pt.y = contact_loc[1];
00343 con_pt.z = contact_loc[2];
00344
00345 obj->skin.locations.push_back(con_pt);
00346 hrl_msgs::FloatArrayBare pts_x_ar;
00347 pts_x_ar.data = obj->pt_x;
00348 obj->skin.pts_x.push_back(pts_x_ar);
00349 hrl_msgs::FloatArrayBare pts_y_ar;
00350 pts_y_ar.data = obj->pt_y;
00351 obj->skin.pts_y.push_back(pts_y_ar);
00352 hrl_msgs::FloatArrayBare pts_z_ar;
00353 pts_z_ar.data = obj->pt_z;
00354 obj->skin.pts_z.push_back(pts_z_ar);
00355 obj->pt_x.clear();
00356 obj->pt_y.clear();
00357 obj->pt_z.clear();
00358 obj->force_group += 1;
00359 }
00360 }
00361 }
00362
00363 void Simulator::publish_angle_data()
00364 {
00365 angle_rates_pub.publish(angle_rates);
00366 angles_pub.publish(angles);
00367 jep_pub.publish(jep_ros);
00368 }
00369
00370 void Simulator::go_initial_position()
00371 {
00372 XmlRpc::XmlRpcValue init_angle;
00373 while (nh_.getParam("/m3/software_testbed/joints/init_angle", init_angle) == false)
00374 sleep(0.1);
00375
00376
00377
00378
00379
00380
00381 m.lock();
00382 for (int ii = 0; ii < num_jts ; ii++)
00383 {
00384 jep[ii] = (double)init_angle[ii];
00385 }
00386 m.unlock();
00387
00388 float error = 1.;
00389 float error_thresh = 0.005;
00390 int torque_step = 0;
00391
00392 while (error > error_thresh)
00393 {
00394 world.step(timestep);
00395 torque_step++;
00396
00397 get_joint_data();
00398 calc_torques();
00399 set_torques();
00400
00401 error = 0.;
00402 for (int ii = 0; ii < num_jts ; ii++)
00403 {
00404 q[ii] = manip_rev_jts[ii].getAngle();
00405 error += (q[ii]-jep[ii])*(q[ii]-jep[ii]);
00406 }
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 }
00429 }
00430
00431 void Simulator::sense_forces()
00432 {
00433 geometry_msgs::Vector3 force;
00434 geometry_msgs::Vector3 normal;
00435 if (fbnum>MAX_FEEDBACKNUM)
00436 {
00437 printf("joint feedback buffer overflow!\n");
00438 assert(false);
00439 }
00440 else
00441 {
00442 dVector3 sum = {0, 0, 0};
00443 for (int i=0; i<fbnum; i++)
00444 {
00445 dReal *f = feedbacks[i].fb.f2;
00446
00447
00448 sum[0] += f[0] * force_sign[i];
00449 sum[1] += f[1] * force_sign[i];
00450 sum[2] += f[2] * force_sign[i];
00451 if (i < fbnum-1)
00452 {
00453 if (force_grouping[i] != force_grouping[i+1])
00454 {
00455 force.x = sum[0];
00456 force.y = sum[1];
00457 force.z = sum[2];
00458 skin.forces.push_back(force);
00459
00460
00461
00462
00463 double f_mag = sqrt(sum[0]*sum[0]+sum[1]*sum[1]+sum[2]*sum[2]);
00464 normal.x = force.x / f_mag;
00465 normal.y = force.y / f_mag;
00466 normal.z = force.z / f_mag;
00467 skin.normals.push_back(normal);
00468 sum[0] = 0;
00469 sum[1] = 0;
00470 sum[2] = 0;
00471 }
00472 else
00473 {
00474 ROS_WARN("Advait believes that this should never happen\n");
00475 exit(0);
00476 }
00477 }
00478 else
00479 {
00480 force.x = sum[0];
00481 force.y = sum[1];
00482 force.z = sum[2];
00483 skin.forces.push_back(force);
00484
00485
00486
00487 double f_mag = sqrt(sum[0]*sum[0]+sum[1]*sum[1]+sum[2]*sum[2]);
00488 normal.x = force.x / f_mag;
00489 normal.y = force.y / f_mag;
00490 normal.z = force.z / f_mag;
00491 skin.normals.push_back(normal);
00492
00493 sum[0] = 0;
00494 sum[1] = 0;
00495 sum[2] = 0;
00496 }
00497 }
00498
00499 }
00500 }
00501
00502 void Simulator::create_robot()
00503 {
00504
00505 XmlRpc::XmlRpcValue link_dimensions;
00506 while (nh_.getParam("/m3/software_testbed/linkage/dimensions", link_dimensions) == false)
00507 sleep(0.1);
00508 XmlRpc::XmlRpcValue link_pos;
00509 while (nh_.getParam("/m3/software_testbed/linkage/positions", link_pos) == false)
00510 sleep(0.1);
00511 XmlRpc::XmlRpcValue link_shapes;
00512 while (nh_.getParam("/m3/software_testbed/linkage/shapes", link_shapes) == false)
00513 sleep(0.1);
00514
00515 XmlRpc::XmlRpcValue link_masses;
00516 while (nh_.getParam("/m3/software_testbed/linkage/mass", link_masses) == false)
00517 sleep(0.1);
00518
00519 while (nh_.getParam("/m3/software_testbed/linkage/num_links", num_links) == false)
00520 sleep(0.1);
00521
00522
00523 while (nh_.getParam("/m3/software_testbed/joints/num_joints", num_jts) == false)
00524 sleep(0.1);
00525
00526 XmlRpc::XmlRpcValue jt_stiffness;
00527 while (nh_.getParam("/m3/software_testbed/joints/imped_params_stiffness", jt_stiffness) == false)
00528 sleep(0.1);
00529
00530 XmlRpc::XmlRpcValue jt_damping;
00531 while (nh_.getParam("/m3/software_testbed/joints/imped_params_damping", jt_damping) == false)
00532 sleep(0.1);
00533
00534 if ((uint)jt_stiffness.size() != k_p.size())
00535 {
00536 std::cerr << "THE JOINT STIFFNESS VECTOR AND NUMBER OF JOINTS IS NOT EQUAL" << std::endl;
00537 assert(false);
00538 }
00539
00540 if ((uint)jt_damping.size() != k_d.size())
00541 {
00542 std::cerr << "THE JOINT DAMPING VECTOR AND NUMBER OF JOINTS IS NOT EQUAL" << std::endl;
00543 assert(false);
00544 }
00545
00546 m.lock();
00547 for (int ii = 0; ii < num_jts; ii++)
00548 {
00549 k_p[ii] = (double)jt_stiffness[ii];
00550 k_d[ii] = (double)jt_damping[ii];
00551 }
00552 m.unlock();
00553
00554 XmlRpc::XmlRpcValue jt_min;
00555 while (nh_.getParam("/m3/software_testbed/joints/min", jt_min) == false)
00556 sleep(0.1);
00557 XmlRpc::XmlRpcValue jt_max;
00558 while (nh_.getParam("/m3/software_testbed/joints/max", jt_max) == false)
00559 sleep(0.1);
00560 XmlRpc::XmlRpcValue jt_axes;
00561 while (nh_.getParam("/m3/software_testbed/joints/axes", jt_axes) == false)
00562 sleep(0.1);
00563 XmlRpc::XmlRpcValue jt_anchor;
00564 while (nh_.getParam("/m3/software_testbed/joints/anchor", jt_anchor) == false)
00565 sleep(0.1);
00566 XmlRpc::XmlRpcValue jt_attach;
00567 while (nh_.getParam("m3/software_testbed/joints/attach", jt_attach) == false)
00568 sleep(0.1);
00569
00570
00571 dMatrix3 body_rotate = {1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0};
00572
00573 for (int ii = 0; ii < num_links; ii++)
00574 {
00575
00576 dMass mass;
00577 links_shape[ii] = (string)link_shapes[ii];
00578 links_arr[ii].create(world);
00579 links_arr[ii].setPosition((double)link_pos[ii][0], (double)link_pos[ii][1], (double)link_pos[ii][2]);
00580 links_dim[ii][0] = (double)link_dimensions[ii][0];
00581 links_dim[ii][1] = (double)link_dimensions[ii][1];
00582 links_dim[ii][2] = (double)link_dimensions[ii][2];
00583
00584
00585 if (links_shape[ii] == "cube")
00586 {
00587 dMassSetBoxTotal(&mass, (double)link_masses[ii], (double)link_dimensions[ii][0], (double)link_dimensions[ii][1], (double)link_dimensions[ii][2]);
00588 links_arr[ii].setMass(mass);
00589 g_link_box[ii].create(space, (double)link_dimensions[ii][0], (double)link_dimensions[ii][1], (double)link_dimensions[ii][2]);
00590 g_link_box[ii].setBody(links_arr[ii]);
00591
00592 }
00593 else if (links_shape[ii] == "capsule")
00594 {
00595 dMassSetCapsuleTotal(&mass, (double)link_masses[ii], 3, (double)link_dimensions[ii][0]/2.0, (double)link_dimensions[ii][2]);
00596 links_arr[ii].setMass(mass);
00597 g_link_cap[ii].create(space, (double)link_dimensions[ii][0]/2.0, (double)link_dimensions[ii][2]);
00598 g_link_cap[ii].setBody(links_arr[ii]);
00599 }
00600 else
00601 {
00602 std::cerr<<"wrong type of link shape was defined in config file,"<<links_shape[ii]<<" does not exist \n";
00603 assert(false);
00604 }
00605 links_arr[ii].setRotation(body_rotate);
00606 link_ids[ii] = links_arr[ii].id();
00607 }
00608
00609 for (int ii = 0; ii < num_jts; ii++)
00610 {
00611 if (true)
00612 {
00613 manip_rev_jts[ii].create(world);
00614 int attach1, attach2;
00615
00616 attach1 = (int)jt_attach[ii][0];
00617 attach2 = (int)jt_attach[ii][1];
00618 if (attach1 == -1 or attach2 == -1)
00619 {
00620 if (attach1 == -1)
00621 {
00622 manip_rev_jts[ii].attach(0, links_arr[attach2]);
00623 }
00624 else
00625 {
00626 manip_rev_jts[ii].attach(links_arr[attach1], 0);
00627 }
00628 }
00629 else
00630 {
00631 manip_rev_jts[ii].attach(links_arr[attach1], links_arr[attach2]);
00632 }
00633
00634 manip_rev_jts[ii].setAnchor((double)jt_anchor[ii][0], (double)jt_anchor[ii][1], (double)jt_anchor[ii][2]);
00635 manip_rev_jts[ii].setAxis((double)jt_axes[ii][0], (double)jt_axes[ii][1], (double)jt_axes[ii][2]);
00636 dJointSetHingeParam(manip_rev_jts[ii].id(),dParamLoStop, (double)jt_min[ii]);
00637 dJointSetHingeParam(manip_rev_jts[ii].id(),dParamHiStop, (double)jt_max[ii]);
00638
00639
00640
00641
00642
00643 }
00644 else if (false)
00645 {
00646 manip_rev_jts[ii].create(world);
00647 int attach1, attach2;
00648 attach1 = (int)jt_attach[ii][0];
00649 attach2 = (int)jt_attach[ii][1];
00650 if (attach1 == -1 or attach2 == -1)
00651 {
00652 if (attach1 == -1)
00653 {
00654 manip_rev_jts[ii].attach(0, links_arr[attach2]);
00655 }
00656 else
00657 {
00658 manip_rev_jts[ii].attach(links_arr[attach1], 0);
00659 }
00660 }
00661 else
00662 {
00663 manip_rev_jts[ii].attach(links_arr[attach1], links_arr[attach2]);
00664 }
00665 manip_rev_jts[ii].setAnchor((double)jt_anchor[ii][0], (double)jt_anchor[ii][1], (double)jt_anchor[ii][2]);
00666 manip_rev_jts[ii].setAxis((double)jt_axes[ii][0], (double)jt_axes[ii][1], (double)jt_axes[ii][2]);
00667 dJointSetHingeParam(manip_rev_jts[ii].id(),dParamLoStop, (double)jt_min[ii]);
00668 dJointSetHingeParam(manip_rev_jts[ii].id(),dParamHiStop, (double)jt_max[ii]);
00669
00670 }
00671 }
00672
00673 joints.create();
00674 }
00675
00676 void Simulator::calc_torques()
00677 {
00678 m.lock();
00679 for (int ii = 0; ii < num_jts; ii++)
00680 {
00681 torques[ii]=(-k_p[ii]*(q[ii]-jep[ii]) - k_d[ii]*q_dot[ii]);
00682 }
00683 m.unlock();
00684 }
00685
00686 void Simulator::set_torques()
00687 {
00688
00689 for (int ii = 0; ii < num_jts ; ii++)
00690 {
00691 manip_rev_jts[ii].addTorque(torques[ii]);
00692 }
00693
00694
00695
00696 }
00697
00698 void Simulator::update_linkage_viz()
00699 {
00700 hrl_msgs::FloatArrayBare link_pos_ar;
00701 hrl_msgs::FloatArrayBare link_rot_ar;
00702
00703 const dReal *position = links_arr[0].getPosition();
00704 const dReal *rotation = links_arr[0].getRotation();
00705 std::vector<double> pos_vec(3);
00706 std::vector<double> rot_vec(12);
00707 for (int l = 0; l<num_links; l++)
00708 {
00709 position = dBodyGetPosition(link_ids[l]);
00710 rotation = dBodyGetRotation(link_ids[l]);
00711 pos_vec[0] = position[0];
00712 pos_vec[1] = position[1];
00713 pos_vec[2] = position[2];
00714 link_pos_ar.data = pos_vec;
00715 draw.link_loc.push_back(link_pos_ar);
00716 for (int k = 0; k<12; k++)
00717 {
00718 rot_vec[k] = rotation[k];
00719 }
00720 link_rot_ar.data = rot_vec;
00721 draw.link_rot.push_back(link_rot_ar);
00722 }
00723 }
00724
00725 void Simulator::update_friction_and_obstacles()
00726 {
00727 hrl_msgs::FloatArrayBare obst_pos_ar;
00728 hrl_msgs::FloatArrayBare obst_rot_ar;
00729 const dReal *position;
00730 const dReal *rotation;
00731
00732 std::vector<double> pos_vec(3);
00733 std::vector<double> rot_vec(12);
00734
00735 for (int l = 0; l<num_used_movable; l++)
00736 {
00737 const dReal *tot_force = dBodyGetForce(obstacles[l].id());
00738 dReal *fric_force = frict_feedbacks[l].fb.f1;
00739 double force_xy_mag = sqrt((tot_force[0]-fric_force[0])*(tot_force[0]-fric_force[0])
00740 +(tot_force[1]-fric_force[1])*(tot_force[1]-fric_force[1]));
00741 if (force_xy_mag>0)
00742 {
00743 dJointSetPlane2DXParam(plane2d_joint_ids[l], dParamFMax,
00744 max_friction*abs(tot_force[0]-fric_force[0])/force_xy_mag);
00745 dJointSetPlane2DYParam(plane2d_joint_ids[l], dParamFMax,
00746 max_friction*abs(tot_force[1]-fric_force[1])/force_xy_mag);
00747 }
00748 else
00749 {
00750 dJointSetPlane2DXParam(plane2d_joint_ids[l], dParamFMax,
00751 max_friction*0.707);
00752 dJointSetPlane2DYParam(plane2d_joint_ids[l], dParamFMax,
00753 max_friction*0.707);
00754 }
00755
00756 dJointSetPlane2DAngleParam(plane2d_joint_ids[l], dParamFMax, max_tor_friction);
00757 dJointSetPlane2DXParam(plane2d_joint_ids[l], dParamVel, 0.0);
00758 dJointSetPlane2DYParam(plane2d_joint_ids[l], dParamVel, 0.0);
00759 dJointSetPlane2DAngleParam(plane2d_joint_ids[l], dParamVel, 0.0);
00760
00761 position = dBodyGetPosition(obstacles[l].id());
00762 rotation = dBodyGetRotation(obstacles[l].id());
00763 pos_vec[0] = position[0];
00764 pos_vec[1] = position[1];
00765 pos_vec[2] = position[2];
00766 obst_pos_ar.data = pos_vec;
00767 draw.obst_loc.push_back(obst_pos_ar);
00768 for (int k = 0; k<12; k++)
00769 {
00770 rot_vec[k] = rotation[k];
00771 }
00772 obst_rot_ar.data = rot_vec;
00773 draw.obst_rot.push_back(obst_rot_ar);
00774 }
00775
00776 for (int l = 0; l<num_used_compliant; l++)
00777 {
00778 dJointSetPlane2DXParam(compliant_plane2d_joint_ids[l], dParamFMax, 0);
00779 dJointSetPlane2DYParam(compliant_plane2d_joint_ids[l], dParamFMax, 0);
00780 const dReal *cur_pos;
00781 cur_pos = dBodyGetPosition(compliant_obstacles[l].id());
00782 const dReal *cur_vel;
00783 cur_vel = dBodyGetLinearVel(compliant_obstacles[l].id());
00784 rotation = dBodyGetRotation(compliant_obstacles[l].id());
00785
00786
00787 dReal Fx = (obst_home[l][0]-cur_pos[0])*obst_stiffness[l] - cur_vel[0]*obst_damping[l];
00788 dReal Fy = (obst_home[l][1]-cur_pos[1])*obst_stiffness[l] - cur_vel[1]*obst_damping[l];
00789
00790 dBodyAddForce(compliant_obstacles[l].id(), Fx, Fy, 0);
00791
00792
00793 pos_vec[0] = cur_pos[0];
00794 pos_vec[1] = cur_pos[1];
00795 pos_vec[2] = cur_pos[2];
00796
00797 obst_pos_ar.data = pos_vec;
00798 draw.obst_loc.push_back(obst_pos_ar);
00799
00800 for (int k = 0; k<12; k++)
00801 {
00802 rot_vec[k] = rotation[k];
00803 }
00804
00805 obst_rot_ar.data = rot_vec;
00806 draw.obst_rot.push_back(obst_rot_ar);
00807 }
00808
00809 for (int l = 0; l<num_used_fixed; l++)
00810 {
00811 position = dBodyGetPosition(fixed_obstacles[l].id());
00812 rotation = dBodyGetRotation(fixed_obstacles[l].id());
00813 pos_vec[0] = position[0];
00814 pos_vec[1] = position[1];
00815 pos_vec[2] = position[2];
00816 obst_pos_ar.data = pos_vec;
00817 draw.obst_loc.push_back(obst_pos_ar);
00818 for (int k = 0; k<12; k++)
00819 {
00820 rot_vec[k] = rotation[k];
00821 }
00822 obst_rot_ar.data = rot_vec;
00823 draw.obst_rot.push_back(obst_rot_ar);
00824 }
00825 }
00826
00827 void Simulator::publish_imped_skin_viz()
00828 {
00829 impedance_params.header.frame_id = "/world";
00830 impedance_params.header.stamp = ros::Time::now();
00831 m.lock();
00832 impedance_params.k_p.data = k_p;
00833 impedance_params.k_d.data = k_d;
00834 m.unlock();
00835 imped_pub.publish(impedance_params);
00836 skin.header.frame_id = "/torso_lift_link";
00837 skin.header.stamp = ros::Time::now();
00838 skin_pub.publish(skin);
00839 draw.header.frame_id = "/world";
00840 draw.header.stamp = ros::Time::now();
00841 bodies_draw.publish(draw);
00842 force_taxel_pub.publish(force_taxel);
00843 proximity_taxel_pub.publish(proximity_taxel);
00844 }
00845
00846 void Simulator::setup_current_taxel_config(hrl_haptic_manipulation_in_clutter_msgs::TaxelArray &taxel)
00847 {
00848 taxel.header.frame_id = "/world";
00849 taxel.header.stamp = ros::Time::now();
00850 std::vector < string > taxel_links;
00851
00852 for (int ii = 0; ii<num_links; ii++)
00853 {
00854 int k = 0;
00855 float link_width = links_dim[ii][0];
00856 float link_length = links_dim[ii][2];
00857 int num = floor(resolution*links_dim[ii][2])+1;
00858 float step = 1.0/resolution;
00859 std::stringstream link_name;
00860 link_name << "link" << (ii+1);
00861 dVector3 global_pt;
00862 dVector3 global_n;
00863
00864 while (k < num)
00865 {
00866 if (k < 2)
00867 {
00868 dBodyGetRelPointPos(links_arr[ii].id(), link_width/2.0, 0.0, (k/2)*step, global_pt);
00869 taxel.centers_x.push_back(global_pt[0]);
00870 taxel.centers_y.push_back(global_pt[1]);
00871 taxel.centers_z.push_back(global_pt[2]);
00872 taxel.link_names.push_back(link_name.str());
00873 taxel_links.push_back(link_name.str());
00874
00875 dBodyGetRelPointPos(links_arr[ii].id(), -link_width/2.0, 0.0, (k/2)*step, global_pt);
00876 taxel.centers_x.push_back(global_pt[0]);
00877 taxel.centers_y.push_back(global_pt[1]);
00878 taxel.centers_z.push_back(global_pt[2]);
00879 taxel.link_names.push_back(link_name.str());
00880 taxel_links.push_back(link_name.str());
00881
00882 dBodyVectorToWorld (links_arr[ii].id(), 1.0, 0.0, 0.0, global_n);
00883 taxel.normals_x.push_back(global_n[0]);
00884 taxel.normals_y.push_back(global_n[1]);
00885 taxel.normals_z.push_back(global_n[2]);
00886
00887 dBodyVectorToWorld (links_arr[ii].id(), -1.0, 0.0, 0.0, global_n);
00888 taxel.normals_x.push_back(global_n[0]);
00889 taxel.normals_y.push_back(global_n[1]);
00890 taxel.normals_z.push_back(global_n[2]);
00891 }
00892 else
00893 {
00894 dBodyGetRelPointPos(links_arr[ii].id(), link_width/2.0, 0.0, (k/2)*step, global_pt);
00895 taxel.centers_x.push_back(global_pt[0]);
00896 taxel.centers_y.push_back(global_pt[1]);
00897 taxel.centers_z.push_back(global_pt[2]);
00898 taxel.link_names.push_back(link_name.str());
00899 taxel_links.push_back(link_name.str());
00900
00901 dBodyGetRelPointPos(links_arr[ii].id(), -link_width/2.0, 0.0, (k/2)*step, global_pt);
00902 taxel.centers_x.push_back(global_pt[0]);
00903 taxel.centers_y.push_back(global_pt[1]);
00904 taxel.centers_z.push_back(global_pt[2]);
00905 taxel.link_names.push_back(link_name.str());
00906 taxel_links.push_back(link_name.str());
00907
00908
00909 dBodyGetRelPointPos(links_arr[ii].id(), link_width/2.0, 0.0, -(k/2)*step, global_pt);
00910 taxel.centers_x.push_back(global_pt[0]);
00911 taxel.centers_y.push_back(global_pt[1]);
00912 taxel.centers_z.push_back(global_pt[2]);
00913 taxel.link_names.push_back(link_name.str());
00914 taxel_links.push_back(link_name.str());
00915
00916 dBodyGetRelPointPos(links_arr[ii].id(), -link_width/2.0, 0.0, -(k/2)*step, global_pt);
00917 taxel.centers_x.push_back(global_pt[0]);
00918 taxel.centers_y.push_back(global_pt[1]);
00919 taxel.centers_z.push_back(global_pt[2]);
00920 taxel.link_names.push_back(link_name.str());
00921 taxel_links.push_back(link_name.str());
00922
00923 dBodyVectorToWorld (links_arr[ii].id(), 1.0, 0.0, 0.0, global_n);
00924 taxel.normals_x.push_back(global_n[0]);
00925 taxel.normals_y.push_back(global_n[1]);
00926 taxel.normals_z.push_back(global_n[2]);
00927
00928 dBodyVectorToWorld (links_arr[ii].id(), -1.0, 0.0, 0.0, global_n);
00929 taxel.normals_x.push_back(global_n[0]);
00930 taxel.normals_y.push_back(global_n[1]);
00931 taxel.normals_z.push_back(global_n[2]);
00932
00933 dBodyVectorToWorld (links_arr[ii].id(), 1.0, 0.0, 0.0, global_n);
00934 taxel.normals_x.push_back(global_n[0]);
00935 taxel.normals_y.push_back(global_n[1]);
00936 taxel.normals_z.push_back(global_n[2]);
00937
00938 dBodyVectorToWorld (links_arr[ii].id(), -1.0, 0.0, 0.0, global_n);
00939 taxel.normals_x.push_back(global_n[0]);
00940 taxel.normals_y.push_back(global_n[1]);
00941 taxel.normals_z.push_back(global_n[2]);
00942 }
00943 k = k+2;
00944 }
00945
00946 if (links_shape[ii] == "cube")
00947 {
00948 k = 0;
00949 num = floor(resolution*(link_width+0.0001))+1;
00950
00951 while (k < num)
00952 {
00953 if (k < 2)
00954 {
00955 dBodyGetRelPointPos(links_arr[ii].id(), (k/2)*step, 0.0, link_length/2.0, global_pt);
00956 taxel.centers_x.push_back(global_pt[0]);
00957 taxel.centers_y.push_back(global_pt[1]);
00958 taxel.centers_z.push_back(global_pt[2]);
00959 taxel.link_names.push_back(link_name.str());
00960 taxel_links.push_back(link_name.str());
00961
00962 dBodyGetRelPointPos(links_arr[ii].id(), (k/2)*step, 0.0, -link_length/2.0, global_pt);
00963 taxel.centers_x.push_back(global_pt[0]);
00964 taxel.centers_y.push_back(global_pt[1]);
00965 taxel.centers_z.push_back(global_pt[2]);
00966 taxel.link_names.push_back(link_name.str());
00967 taxel_links.push_back(link_name.str());
00968
00969 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, 1.0, global_n);
00970 taxel.normals_x.push_back(global_n[0]);
00971 taxel.normals_y.push_back(global_n[1]);
00972 taxel.normals_z.push_back(global_n[2]);
00973
00974 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, -1.0, global_n);
00975 taxel.normals_x.push_back(global_n[0]);
00976 taxel.normals_y.push_back(global_n[1]);
00977 taxel.normals_z.push_back(global_n[2]);
00978 }
00979 else
00980 {
00981 dBodyGetRelPointPos(links_arr[ii].id(), (k/2)*step, 0.0, link_length/2.0, global_pt);
00982 taxel.centers_x.push_back(global_pt[0]);
00983 taxel.centers_y.push_back(global_pt[1]);
00984 taxel.centers_z.push_back(global_pt[2]);
00985 taxel.link_names.push_back(link_name.str());
00986 taxel_links.push_back(link_name.str());
00987
00988 dBodyGetRelPointPos(links_arr[ii].id(), (k/2)*step, 0.0, -link_length/2.0, global_pt);
00989 taxel.centers_x.push_back(global_pt[0]);
00990 taxel.centers_y.push_back(global_pt[1]);
00991 taxel.centers_z.push_back(global_pt[2]);
00992 taxel.link_names.push_back(link_name.str());
00993 taxel_links.push_back(link_name.str());
00994
00995
00996 dBodyGetRelPointPos(links_arr[ii].id(), -(k/2)*step, 0.0, link_length/2.0, global_pt);
00997 taxel.centers_x.push_back(global_pt[0]);
00998 taxel.centers_y.push_back(global_pt[1]);
00999 taxel.centers_z.push_back(global_pt[2]);
01000 taxel.link_names.push_back(link_name.str());
01001 taxel_links.push_back(link_name.str());
01002
01003 dBodyGetRelPointPos(links_arr[ii].id(), -(k/2)*step, 0.0, -link_length/2.0, global_pt);
01004 taxel.centers_x.push_back(global_pt[0]);
01005 taxel.centers_y.push_back(global_pt[1]);
01006 taxel.centers_z.push_back(global_pt[2]);
01007 taxel.link_names.push_back(link_name.str());
01008 taxel_links.push_back(link_name.str());
01009
01010 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, 1.0, global_n);
01011 taxel.normals_x.push_back(global_n[0]);
01012 taxel.normals_y.push_back(global_n[1]);
01013 taxel.normals_z.push_back(global_n[2]);
01014
01015 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, -1.0, global_n);
01016 taxel.normals_x.push_back(global_n[0]);
01017 taxel.normals_y.push_back(global_n[1]);
01018 taxel.normals_z.push_back(global_n[2]);
01019
01020 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, 1.0, global_n);
01021 taxel.normals_x.push_back(global_n[0]);
01022 taxel.normals_y.push_back(global_n[1]);
01023 taxel.normals_z.push_back(global_n[2]);
01024
01025 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, -1.0, global_n);
01026 taxel.normals_x.push_back(global_n[0]);
01027 taxel.normals_y.push_back(global_n[1]);
01028 taxel.normals_z.push_back(global_n[2]);
01029 }
01030 k = k+2;
01031 }
01032 }
01033 else if (links_shape[ii] == "capsule")
01034 {
01035 float radius = link_width/2.0;
01036 k = 0;
01037 num = floor(resolution*(PI*radius))+1;
01038 float ang_step = PI/(num);
01039 while (k < num)
01040 {
01041 if (k < 2)
01042 {
01043 dBodyGetRelPointPos(links_arr[ii].id(), 0.0, 0.0, link_length/2.0+radius, global_pt);
01044 taxel.centers_x.push_back(global_pt[0]);
01045 taxel.centers_y.push_back(global_pt[1]);
01046 taxel.centers_z.push_back(global_pt[2]);
01047 taxel.link_names.push_back(link_name.str());
01048 taxel_links.push_back(link_name.str());
01049
01050 dBodyGetRelPointPos(links_arr[ii].id(), 0.0, 0.0, -link_length/2.0-radius, global_pt);
01051 taxel.centers_x.push_back(global_pt[0]);
01052 taxel.centers_y.push_back(global_pt[1]);
01053 taxel.centers_z.push_back(global_pt[2]);
01054 taxel.link_names.push_back(link_name.str());
01055 taxel_links.push_back(link_name.str());
01056
01057 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, 1.0, global_n);
01058 taxel.normals_x.push_back(global_n[0]);
01059 taxel.normals_y.push_back(global_n[1]);
01060 taxel.normals_z.push_back(global_n[2]);
01061
01062 dBodyVectorToWorld (links_arr[ii].id(), 0.0, 0.0, -1.0, global_n);
01063 taxel.normals_x.push_back(global_n[0]);
01064 taxel.normals_y.push_back(global_n[1]);
01065 taxel.normals_z.push_back(global_n[2]);
01066 }
01067 else
01068 {
01069 dBodyGetRelPointPos(links_arr[ii].id(), radius*sin(ang_step*(k/2)), 0.0, link_length/2.0+radius*cos(ang_step*(k/2)), global_pt);
01070 taxel.centers_x.push_back(global_pt[0]);
01071 taxel.centers_y.push_back(global_pt[1]);
01072 taxel.centers_z.push_back(global_pt[2]);
01073 taxel.link_names.push_back(link_name.str());
01074 taxel_links.push_back(link_name.str());
01075
01076 dBodyGetRelPointPos(links_arr[ii].id(), radius*sin(ang_step*(k/2)), 0.0, -link_length/2.0-radius*cos(ang_step*(k/2)), global_pt);
01077 taxel.centers_x.push_back(global_pt[0]);
01078 taxel.centers_y.push_back(global_pt[1]);
01079 taxel.centers_z.push_back(global_pt[2]);
01080 taxel.link_names.push_back(link_name.str());
01081 taxel_links.push_back(link_name.str());
01082
01083 dBodyGetRelPointPos(links_arr[ii].id(), radius*sin(-ang_step*(k/2)), 0.0, link_length/2.0+radius*cos(-ang_step*(k/2)), 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(link_name.str());
01088 taxel_links.push_back(link_name.str());
01089
01090 dBodyGetRelPointPos(links_arr[ii].id(), radius*sin(-ang_step*(k/2)), 0.0, -link_length/2.0-radius*cos(-ang_step*(k/2)), global_pt);
01091 taxel.centers_x.push_back(global_pt[0]);
01092 taxel.centers_y.push_back(global_pt[1]);
01093 taxel.centers_z.push_back(global_pt[2]);
01094 taxel.link_names.push_back(link_name.str());
01095 taxel_links.push_back(link_name.str());
01096
01097 dBodyVectorToWorld (links_arr[ii].id(), sin(ang_step*(k/2)), 0.0, cos(ang_step*(k/2)), global_n);
01098 taxel.normals_x.push_back(global_n[0]);
01099 taxel.normals_y.push_back(global_n[1]);
01100 taxel.normals_z.push_back(global_n[2]);
01101
01102 dBodyVectorToWorld (links_arr[ii].id(), sin(ang_step*(k/2)), 0.0, -cos(ang_step*(k/2)), global_n);
01103 taxel.normals_x.push_back(global_n[0]);
01104 taxel.normals_y.push_back(global_n[1]);
01105 taxel.normals_z.push_back(global_n[2]);
01106
01107 dBodyVectorToWorld (links_arr[ii].id(), sin(-ang_step*(k/2)), 0.0, cos(-ang_step*(k/2)), global_n);
01108 taxel.normals_x.push_back(global_n[0]);
01109 taxel.normals_y.push_back(global_n[1]);
01110 taxel.normals_z.push_back(global_n[2]);
01111
01112 dBodyVectorToWorld (links_arr[ii].id(), sin(-ang_step*(k/2)), 0.0, -cos(-ang_step*(k/2)), global_n);
01113 taxel.normals_x.push_back(global_n[0]);
01114 taxel.normals_y.push_back(global_n[1]);
01115 taxel.normals_z.push_back(global_n[2]);
01116 }
01117 k = k+2;
01118 }
01119 }
01120 }
01121 }
01122
01123 double Simulator::get_dist(double x1, double y1, double x2, double y2, double radius)
01124 {
01125 return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)) - radius;
01126 }
01127
01128 void Simulator::update_proximity_simulation()
01129 {
01130 if (use_prox_sensor == true)
01131 {
01132 setup_current_taxel_config(proximity_taxel);
01133 proximity_taxel.sensor_type = "distance";
01134
01135 const dReal *position;
01136 for (uint i=0; i < proximity_taxel.link_names.size(); i++)
01137 {
01138 double min_dist = 0.35;
01139 double x_taxel = proximity_taxel.centers_x[i];
01140 double y_taxel = proximity_taxel.centers_y[i];
01141 double nrml_x = proximity_taxel.normals_x[i];
01142 double nrml_y = proximity_taxel.normals_y[i];
01143
01144
01145 double radius = 0.01;
01146
01147
01148 for (int j=0; j < num_used_movable ; j++)
01149 {
01150 position = dBodyGetPosition(obstacles[j].id());
01151 double cur_dist = get_dist(position[0], position[1], x_taxel, y_taxel, radius);
01152 if ( cur_dist< min_dist)
01153 {
01154
01155
01156 double dot_product = ((position[0]-x_taxel)*nrml_x + (position[1]-y_taxel)*nrml_y)/sqrt(pow(position[0]-x_taxel, 2) + pow(position[1]-y_taxel, 2));
01157
01158 if (dot_product > 0.7071)
01159 {
01160 min_dist = cur_dist;
01161 }
01162 }
01163 }
01164 for (int j=0; j < num_used_fixed ; j++)
01165 {
01166 position = dBodyGetPosition(fixed_obstacles[j].id());
01167 double cur_dist = get_dist(position[0], position[1], x_taxel, y_taxel, radius);
01168 if ( cur_dist< min_dist)
01169 {
01170 double dot_product = ((position[0]-x_taxel)*nrml_x + (position[1]-y_taxel)*nrml_y)/sqrt(pow(position[0]-x_taxel, 2) + pow(position[1]-y_taxel, 2));
01171 if (dot_product > 0.7071)
01172 {
01173 min_dist = cur_dist;
01174 }
01175 }
01176 }
01177 for (int j=0; j < num_used_compliant ; j++)
01178 {
01179 position = dBodyGetPosition(compliant_obstacles[j].id());
01180 double cur_dist = get_dist(position[0], position[1], x_taxel, y_taxel, radius);
01181 if ( cur_dist< min_dist)
01182 {
01183 double dot_product = ((position[0]-x_taxel)*nrml_x + (position[1]-y_taxel)*nrml_y)/sqrt(pow(position[0]-x_taxel, 2) + pow(position[1]-y_taxel, 2));
01184 if (dot_product > 0.7071)
01185 {
01186 min_dist = cur_dist;
01187 }
01188 }
01189 }
01190
01191 proximity_taxel.values_x.push_back(min_dist*nrml_x);
01192 proximity_taxel.values_y.push_back(min_dist*nrml_y);
01193 proximity_taxel.values_z.push_back(0.);
01194 }
01195 }
01196 }
01197
01198 void Simulator::update_taxel_simulation()
01199 {
01200 setup_current_taxel_config(force_taxel);
01201 force_taxel.sensor_type = "force";
01202
01203
01204 std::vector < int > f_ind;
01205 for (unsigned int j = 0; j < skin.forces.size(); j++)
01206 {
01207 float min_distance = 10000;
01208
01209
01210
01211
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221 int ind_buf = -134241;
01222
01223 for (unsigned int k = 0; k < force_taxel.centers_x.size(); k++)
01224 {
01225 float distance = sqrt(pow((force_taxel.centers_x[k]-skin.locations[j].x),2)+pow((force_taxel.centers_y[k]-skin.locations[j].y),2)+pow((force_taxel.centers_z[k]-skin.locations[j].z),2));
01226 if (distance < min_distance && force_taxel.link_names[k] == skin.link_names[j])
01227 {
01228 double mag_norm = sqrt(force_taxel.normals_x[k]*force_taxel.normals_x[k]+force_taxel.normals_y[k]*force_taxel.normals_y[k]);
01229 double mag_force = sqrt(skin.forces[j].x*skin.forces[j].x + skin.forces[j].y*skin.forces[j].y);
01230
01231 double dot_prod = (force_taxel.normals_x[k]*skin.forces[j].x + force_taxel.normals_y[k]*skin.forces[j].y);
01232 double cos_angle = dot_prod/(mag_norm*mag_force);
01233 double abs_angle_deg = abs(acos(cos_angle) * 180.0 / PI);
01234
01235 if (mag_force < 0.01)
01236 abs_angle_deg = 0.;
01237
01238
01239
01240
01241
01242 if (abs_angle_deg <= 45.0)
01243 {
01244
01245
01246
01247
01248 min_distance = distance;
01249 ind_buf = k;
01250 }
01251 }
01252 }
01253
01254 assert(ind_buf >= 0);
01255 f_ind.push_back(ind_buf);
01256 }
01257
01258 for (unsigned int k = 0; k < force_taxel.centers_x.size(); k++)
01259 {
01260 force_taxel.values_x.push_back(0.0);
01261 force_taxel.values_y.push_back(0.0);
01262 force_taxel.values_z.push_back(0.0);
01263 }
01264
01265 for (unsigned int j = 0; j < f_ind.size(); j++)
01266 {
01267 force_taxel.values_x[f_ind[j]] = force_taxel.values_x[f_ind[j]] + skin.forces[j].x;
01268 force_taxel.values_y[f_ind[j]] = force_taxel.values_y[f_ind[j]] + skin.forces[j].y;
01269 force_taxel.values_z[f_ind[j]] = force_taxel.values_z[f_ind[j]] + skin.forces[j].z;
01270 }
01271
01272 f_ind.clear();
01273
01274
01275
01276 }
01277
01278 void Simulator::clear()
01279 {
01280 draw.link_loc.clear();
01281 draw.link_rot.clear();
01282 draw.obst_loc.clear();
01283 draw.obst_rot.clear();
01284
01285 force_taxel.centers_x.clear();
01286 force_taxel.centers_y.clear();
01287 force_taxel.centers_z.clear();
01288 force_taxel.normals_x.clear();
01289 force_taxel.normals_y.clear();
01290 force_taxel.normals_z.clear();
01291 force_taxel.values_x.clear();
01292 force_taxel.values_y.clear();
01293 force_taxel.values_z.clear();
01294 force_taxel.link_names.clear();
01295
01296 proximity_taxel.centers_x.clear();
01297 proximity_taxel.centers_y.clear();
01298 proximity_taxel.centers_z.clear();
01299 proximity_taxel.normals_x.clear();
01300 proximity_taxel.normals_y.clear();
01301 proximity_taxel.normals_z.clear();
01302 proximity_taxel.values_x.clear();
01303 proximity_taxel.values_y.clear();
01304 proximity_taxel.values_z.clear();
01305 proximity_taxel.link_names.clear();
01306
01307 skin.pts_x.clear();
01308 skin.pts_y.clear();
01309 skin.pts_z.clear();
01310 skin.link_names.clear();
01311 skin.locations.clear();
01312 skin.forces.clear();
01313 skin.normals.clear();
01314 force_grouping.clear();
01315 force_sign.clear();
01316 joints.clear();
01317
01318 fbnum = 0;
01319 force_group = 0;
01320 }
01321
01322 void Simulator::get_joint_data()
01323 {
01324 for (int ii = 0; ii < num_jts ; ii++)
01325 {
01326 q[ii] = manip_rev_jts[ii].getAngle();
01327 q_dot[ii] = manip_rev_jts[ii].getAngleRate();
01328 }
01329
01330 angles.data = q;
01331 angle_rates.data = q_dot;
01332 m.lock();
01333 jep_ros.data = jep;
01334 m.unlock();
01335 }
01336
01337 void Simulator::create_movable_obstacles()
01338 {
01339 float obstacle_mass = 1.;
01340
01341
01342
01343 XmlRpc::XmlRpcValue cylinders_dim;
01344 while (nh_.getParam("/m3/software_testbed/movable_dimen", cylinders_dim) == false)
01345 sleep(0.1);
01346
01347 XmlRpc::XmlRpcValue cylinders_pos;
01348 while (nh_.getParam("/m3/software_testbed/movable_position", cylinders_pos) == false)
01349 sleep(0.1);
01350
01351 while(nh_.getParam("/m3/software_testbed/num_movable", num_used_movable) == false)
01352 sleep(0.1);
01353
01354 XmlRpc::XmlRpcValue cylinders_max_force;
01355 bool got_max_force;
01356 got_max_force = nh_.getParam("/m3/software_testbed/movable_max_force", cylinders_max_force);
01357
01358 for (int i = 0; i < num_used_movable; i++)
01359 {
01360 dMass m_obst;
01361 dCapsule *geom_cyl;
01362
01363 dMassSetCapsuleTotal(&m_obst, obstacle_mass, 3,
01364 (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
01365
01366 obstacles[i].create(world);
01367 obstacles[i].setPosition((double)cylinders_pos[i][0], (double)cylinders_pos[i][1], (double)cylinders_pos[i][2]);
01368 obstacles[i].setMass(&m_obst);
01369 plane2d_joint_ids[i] = dJointCreatePlane2D(world.id(), 0);
01370 dJointAttach(plane2d_joint_ids[i], obstacles[i].id(), 0);
01371
01372 if (got_max_force == false)
01373 {
01374 dJointSetPlane2DXParam(plane2d_joint_ids[i], dParamFMax, max_friction*0.707);
01375 dJointSetPlane2DYParam(plane2d_joint_ids[i], dParamFMax, max_friction*0.707);
01376 dJointSetPlane2DAngleParam(plane2d_joint_ids[i], dParamFMax, max_tor_friction);
01377 }
01378 else
01379 {
01380 dJointSetPlane2DXParam(plane2d_joint_ids[i], dParamFMax, (double)cylinders_max_force[i]*0.707);
01381 dJointSetPlane2DYParam(plane2d_joint_ids[i], dParamFMax, (double)cylinders_max_force[i]*0.707);
01382 dJointSetPlane2DAngleParam(plane2d_joint_ids[i], dParamFMax, max_tor_friction);
01383 }
01384
01385 dJointSetPlane2DXParam(plane2d_joint_ids[i], dParamVel, 0.0);
01386 dJointSetPlane2DYParam(plane2d_joint_ids[i], dParamVel, 0.0);
01387 dJointSetPlane2DAngleParam(plane2d_joint_ids[i], dParamVel, 0.0);
01388
01389 geom_cyl = new dCapsule(space, (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
01390 geom_cyl->setBody(obstacles[i]);
01391
01392 dJointSetFeedback(plane2d_joint_ids[i], &frict_feedbacks[i].fb);
01393
01394 }
01395 }
01396
01397 void Simulator::create_compliant_obstacles()
01398 {
01399 float obstacle_mass = 1.;
01400
01401 XmlRpc::XmlRpcValue cylinders_dim;
01402 while (nh_.getParam("/m3/software_testbed/compliant_dimen", cylinders_dim) == false)
01403 sleep(0.1);
01404
01405 XmlRpc::XmlRpcValue cylinders_pos;
01406 nh_.getParam("/m3/software_testbed/compliant_position", cylinders_pos);
01407
01408 XmlRpc::XmlRpcValue cylinders_stiffness;
01409 bool got_stiffness;
01410 got_stiffness = nh_.getParam("/m3/software_testbed/compliant_stiffness_value", cylinders_stiffness);
01411
01412 nh_.getParam("/m3/software_testbed/num_compliant", num_used_compliant);
01413
01414 for (int i = 0; i < num_used_compliant; i++)
01415 {
01416 dMass m_obst;
01417 dCapsule *geom_cyl;
01418
01419 if(got_stiffness == true)
01420 {
01421 obst_stiffness[i]= (double)cylinders_stiffness[i];
01422
01423
01424
01425 obst_damping[i] = 2*sqrt(obst_stiffness[i]*obstacle_mass);
01426 }
01427 else
01428 {
01429 obst_stiffness[i]= -1;
01430 }
01431
01432 dMassSetCapsuleTotal(&m_obst, obstacle_mass, 3, (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
01433 compliant_obstacles[i].create(world);
01434 compliant_obstacles[i].setPosition((double)cylinders_pos[i][0], (double)cylinders_pos[i][1], (double)cylinders_pos[i][2]);
01435 obst_home[i][0] = (double)cylinders_pos[i][0];
01436 obst_home[i][1] = (double)cylinders_pos[i][1];
01437 obst_home[i][2] = (double)cylinders_pos[i][2];
01438 compliant_obstacles[i].setMass(&m_obst);
01439 compliant_plane2d_joint_ids[i] = dJointCreatePlane2D(world.id(), 0);
01440 dJointAttach(compliant_plane2d_joint_ids[i], compliant_obstacles[i].id(), 0);
01441 dJointSetPlane2DXParam(compliant_plane2d_joint_ids[i], dParamVel, 0.0);
01442 dJointSetPlane2DYParam(compliant_plane2d_joint_ids[i], dParamVel, 0.0);
01443 dJointSetPlane2DAngleParam(compliant_plane2d_joint_ids[i], dParamVel, 0.0);
01444
01445 geom_cyl = new dCapsule(space, (double)cylinders_dim[i][0], (double)cylinders_dim[i][2]);
01446 geom_cyl->setBody(compliant_obstacles[i]);
01447 }
01448 }
01449
01450 void Simulator::create_fixed_obstacles()
01451 {
01452 float obstacle_mass = 1.;
01453
01454 while(nh_.getParam("/m3/software_testbed/num_fixed", num_used_fixed) == false)
01455 sleep(0.1);
01456
01457 XmlRpc::XmlRpcValue fixed_pos;
01458 nh_.getParam("/m3/software_testbed/fixed_position", fixed_pos);
01459 XmlRpc::XmlRpcValue fixed_dim;
01460 nh_.getParam("/m3/software_testbed/fixed_dimen", fixed_dim);
01461 XmlRpc::XmlRpcValue fixed_ctype;
01462 nh_.getParam("/m3/software_testbed/fixed_ctype", fixed_ctype);
01463
01464
01465 for (int i = 0; i < num_used_fixed; i++)
01466 {
01467 dMass m_obst;
01468
01469 dMassSetCapsuleTotal(&m_obst, obstacle_mass, 3,
01470 (double)fixed_dim[i][0], (double)fixed_dim[i][2]);
01471
01472 fixed_obstacles[i].create(world);
01473 fixed_obstacles[i].setPosition((double)fixed_pos[i][0], (double)fixed_pos[i][1], (double)fixed_pos[i][2]);
01474 fixed_obstacles[i].setMass(&m_obst);
01475
01476 if (static_cast<std::string>(fixed_ctype[i]) == "wall")
01477 {
01478 dBox *geom_fixed;
01479 double theta = (double)fixed_pos[i][3];
01480 dMatrix3 obstacle_rotate = {cos(theta),-sin(theta),0,0,sin(theta),cos(theta),0,0,0,0,1.0,0};
01481 fixed_obstacles[i].setRotation(obstacle_rotate);
01482
01483 fixed_joint_ids[i] = dJointCreateFixed(world.id(), 0);
01484 dJointAttach(fixed_joint_ids[i], fixed_obstacles[i].id(), 0);
01485 dJointSetFixed(fixed_joint_ids[i]);
01486
01487 geom_fixed = new dBox(space, (double)fixed_dim[i][0], (double)fixed_dim[i][1], (double)fixed_dim[i][2]);
01488 geom_fixed->setBody(fixed_obstacles[i]);
01489 }
01490 else
01491 {
01492 dCapsule *geom_fixed;
01493
01494 fixed_joint_ids[i] = dJointCreateFixed(world.id(), 0);
01495 dJointAttach(fixed_joint_ids[i], fixed_obstacles[i].id(), 0);
01496 dJointSetFixed(fixed_joint_ids[i]);
01497
01498 geom_fixed = new dCapsule(space, (double)fixed_dim[i][0], (double)fixed_dim[i][2]);
01499 geom_fixed->setBody(fixed_obstacles[i]);
01500 }
01501
01502
01503 }
01504 }