#include <sys/time.h>
#include "ros/ros.h"
#include "hrl_haptic_manipulation_in_clutter_msgs/SkinContact.h"
#include "hrl_haptic_manipulation_in_clutter_msgs/BodyDraw.h"
#include "hrl_haptic_manipulation_in_clutter_msgs/TaxelArray.h"
#include "hrl_haptic_manipulation_in_clutter_msgs/MechanicalImpedanceParams.h"
#include "hrl_msgs/FloatArrayBare.h"
#include "std_msgs/String.h"
#include "rosgraph_msgs/Clock.h"
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <set>
#include <algorithm>
#include <functional>
#include <ode/ode.h>
#include <sstream>
#include <tf/transform_broadcaster.h>
Go to the source code of this file.
void base_ep_cb | ( | const hrl_msgs::FloatArrayBare | msg | ) |
Definition at line 138 of file demo_kinematic_intermed_cyl.cpp.
void create_compliant_obstacles | ( | ros::NodeHandle | n | ) |
Definition at line 609 of file demo_kinematic_intermed_cyl.cpp.
void create_fixed_obstacles | ( | ros::NodeHandle | n | ) |
Definition at line 680 of file demo_kinematic_intermed_cyl.cpp.
void create_movable_obstacles | ( | ros::NodeHandle | n | ) |
Definition at line 554 of file demo_kinematic_intermed_cyl.cpp.
void create_robot | ( | ros::NodeHandle | n | ) |
Definition at line 334 of file demo_kinematic_intermed_cyl.cpp.
double get_wall_clock_time | ( | ) |
Definition at line 710 of file demo_kinematic_intermed_cyl.cpp.
void go_initial_position | ( | ros::NodeHandle | n | ) |
Definition at line 292 of file demo_kinematic_intermed_cyl.cpp.
void inner_torque_loop | ( | ) |
std::vector<double> jep | ( | 3 | , |
0 | |||
) |
void jep_cb | ( | const hrl_msgs::FloatArrayBare | msg | ) |
Definition at line 143 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> k_d | ( | 3 | , |
0 | |||
) |
std::vector<double> k_p | ( | 3 | , |
0 | |||
) |
int main | ( | int | argc, |
char ** | argv | ||
) |
Definition at line 1649 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> mobile_base_ep | ( | 3 | , |
0 | |||
) |
std::vector<double> mobile_base_generalized_forces | ( | 3 | , |
0 | |||
) |
std::vector<double> mobile_base_k_d | ( | 3 | , |
0 | |||
) |
std::vector<double> mobile_base_k_p | ( | 3 | , |
0 | |||
) |
std::vector<std::string> names | ( | MAX_FEEDBACKNUM | ) |
static void nearCallback | ( | void * | data, |
dGeomID | o1, | ||
dGeomID | o2 | ||
) | [static] |
Definition at line 159 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> q | ( | 3 | , |
0 | |||
) |
std::vector<double> q_dot | ( | 3 | , |
0 | |||
) |
void ROSCallback_impedance | ( | const hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams | msg | ) |
Definition at line 151 of file demo_kinematic_intermed_cyl.cpp.
void taxel_simulation_code | ( | double | resolution | ) |
Definition at line 720 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> torques | ( | 3 | , |
0 | |||
) |
dBody* body_mobile_base |
Definition at line 125 of file demo_kinematic_intermed_cyl.cpp.
dBody compliant_obstacles[NUM_OBST] |
Definition at line 88 of file demo_kinematic_intermed_cyl.cpp.
dJointID compliant_plane2d_joint_ids[NUM_OBST] |
Definition at line 95 of file demo_kinematic_intermed_cyl.cpp.
Definition at line 131 of file demo_kinematic_intermed_cyl.cpp.
dBody* env |
Definition at line 99 of file demo_kinematic_intermed_cyl.cpp.
int fbnum = 0 [static] |
Definition at line 42 of file demo_kinematic_intermed_cyl.cpp.
MyFeedback feedbacks[MAX_FEEDBACKNUM] [static] |
Definition at line 40 of file demo_kinematic_intermed_cyl.cpp.
dJointID fixed_joint_ids[NUM_OBST] |
Definition at line 96 of file demo_kinematic_intermed_cyl.cpp.
dBodyID fixed_obst_ids[NUM_OBST] |
Definition at line 92 of file demo_kinematic_intermed_cyl.cpp.
dBody fixed_obstacles[NUM_OBST] |
Definition at line 93 of file demo_kinematic_intermed_cyl.cpp.
int force_group = 0 [static] |
Definition at line 43 of file demo_kinematic_intermed_cyl.cpp.
Definition at line 54 of file demo_kinematic_intermed_cyl.cpp.
std::vector<int> force_sign |
Definition at line 55 of file demo_kinematic_intermed_cyl.cpp.
float forearm_length = 0 |
Definition at line 80 of file demo_kinematic_intermed_cyl.cpp.
float forearm_width = 0 |
Definition at line 81 of file demo_kinematic_intermed_cyl.cpp.
MyFeedback frict_feedbacks[NUM_OBST] [static] |
Definition at line 41 of file demo_kinematic_intermed_cyl.cpp.
dBox* g_link1_box |
Definition at line 109 of file demo_kinematic_intermed_cyl.cpp.
dCapsule* g_link1_cap |
Definition at line 110 of file demo_kinematic_intermed_cyl.cpp.
dBox* g_link2_box |
Definition at line 114 of file demo_kinematic_intermed_cyl.cpp.
dCapsule* g_link2_cap |
Definition at line 115 of file demo_kinematic_intermed_cyl.cpp.
dBox* g_link3_box |
Definition at line 119 of file demo_kinematic_intermed_cyl.cpp.
dCapsule* g_link3_cap |
Definition at line 120 of file demo_kinematic_intermed_cyl.cpp.
dBox* geom_mobile_base |
Definition at line 126 of file demo_kinematic_intermed_cyl.cpp.
dHingeJoint* hinge1 |
Definition at line 111 of file demo_kinematic_intermed_cyl.cpp.
dHingeJoint* hinge2 |
Definition at line 116 of file demo_kinematic_intermed_cyl.cpp.
dHingeJoint* hinge3 |
Definition at line 121 of file demo_kinematic_intermed_cyl.cpp.
Definition at line 133 of file demo_kinematic_intermed_cyl.cpp.
Definition at line 123 of file demo_kinematic_intermed_cyl.cpp.
dJointGroup joints |
Definition at line 98 of file demo_kinematic_intermed_cyl.cpp.
dBody* link1 |
Definition at line 108 of file demo_kinematic_intermed_cyl.cpp.
dBody* link2 |
Definition at line 113 of file demo_kinematic_intermed_cyl.cpp.
dBody* link3 |
Definition at line 118 of file demo_kinematic_intermed_cyl.cpp.
dBodyID link_ids[3] |
Definition at line 86 of file demo_kinematic_intermed_cyl.cpp.
double max_friction = 2 |
Definition at line 44 of file demo_kinematic_intermed_cyl.cpp.
double max_tor_friction = 0.5 |
Definition at line 45 of file demo_kinematic_intermed_cyl.cpp.
dJointID mobile_base_plane2d_jt_id |
Definition at line 127 of file demo_kinematic_intermed_cyl.cpp.
int num_used_compliant = NUM_OBST |
Definition at line 48 of file demo_kinematic_intermed_cyl.cpp.
int num_used_fixed = NUM_OBST |
Definition at line 47 of file demo_kinematic_intermed_cyl.cpp.
int num_used_movable = NUM_OBST |
Definition at line 46 of file demo_kinematic_intermed_cyl.cpp.
double obst_damping[NUM_OBST] |
Definition at line 91 of file demo_kinematic_intermed_cyl.cpp.
double obst_home[NUM_OBST][3] |
Definition at line 89 of file demo_kinematic_intermed_cyl.cpp.
double obst_stiffness[NUM_OBST] |
Definition at line 90 of file demo_kinematic_intermed_cyl.cpp.
dBody obstacles[NUM_OBST] |
Definition at line 87 of file demo_kinematic_intermed_cyl.cpp.
dJointID plane2d_joint_ids[NUM_OBST] |
Definition at line 94 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> pt_x |
Definition at line 56 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> pt_y |
Definition at line 57 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> pt_z |
Definition at line 58 of file demo_kinematic_intermed_cyl.cpp.
Definition at line 130 of file demo_kinematic_intermed_cyl.cpp.
dSpace* space |
Definition at line 101 of file demo_kinematic_intermed_cyl.cpp.
Definition at line 132 of file demo_kinematic_intermed_cyl.cpp.
float torso_half_width = 0.196 |
Definition at line 76 of file demo_kinematic_intermed_cyl.cpp.
float torso_half_width_side = 0 |
Definition at line 77 of file demo_kinematic_intermed_cyl.cpp.
float upper_arm_length = 0.334 |
Definition at line 78 of file demo_kinematic_intermed_cyl.cpp.
float upper_arm_width = 0 |
Definition at line 79 of file demo_kinematic_intermed_cyl.cpp.
dWorld* world |
Definition at line 100 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> x_c |
Definition at line 70 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> x_n |
Definition at line 73 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> y_c |
Definition at line 71 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> y_n |
Definition at line 74 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> z_c |
Definition at line 72 of file demo_kinematic_intermed_cyl.cpp.
std::vector<double> z_n |
Definition at line 75 of file demo_kinematic_intermed_cyl.cpp.