#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 134 of file demo_kinematic.cpp.
void create_compliant_obstacles | ( | ros::NodeHandle | n | ) |
Definition at line 544 of file demo_kinematic.cpp.
void create_fixed_obstacles | ( | ros::NodeHandle | n | ) |
Definition at line 615 of file demo_kinematic.cpp.
void create_movable_obstacles | ( | ros::NodeHandle | n | ) |
Definition at line 489 of file demo_kinematic.cpp.
void create_robot | ( | ros::NodeHandle | n | ) |
Definition at line 330 of file demo_kinematic.cpp.
double get_wall_clock_time | ( | ) |
Definition at line 645 of file demo_kinematic.cpp.
void go_initial_position | ( | ros::NodeHandle | n | ) |
Definition at line 288 of file demo_kinematic.cpp.
void inner_torque_loop | ( | ) |
Definition at line 1389 of file demo_kinematic.cpp.
std::vector<double> jep | ( | 3 | , |
0 | |||
) |
void jep_cb | ( | const hrl_msgs::FloatArrayBare | msg | ) |
Definition at line 139 of file demo_kinematic.cpp.
std::vector<double> k_d | ( | 3 | , |
0 | |||
) |
std::vector<double> k_p | ( | 3 | , |
0 | |||
) |
int main | ( | int | argc, |
char ** | argv | ||
) |
Definition at line 1421 of file demo_kinematic.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 155 of file demo_kinematic.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 147 of file demo_kinematic.cpp.
void taxel_simulation_code | ( | double | resolution | ) |
Definition at line 655 of file demo_kinematic.cpp.
std::vector<double> torques | ( | 3 | , |
0 | |||
) |
dBody* body_mobile_base |
Definition at line 121 of file demo_kinematic.cpp.
dBody compliant_obstacles[NUM_OBST] |
Definition at line 87 of file demo_kinematic.cpp.
dJointID compliant_plane2d_joint_ids[NUM_OBST] |
Definition at line 94 of file demo_kinematic.cpp.
Definition at line 127 of file demo_kinematic.cpp.
dBody* env |
Definition at line 98 of file demo_kinematic.cpp.
int fbnum = 0 [static] |
Definition at line 41 of file demo_kinematic.cpp.
MyFeedback feedbacks[MAX_FEEDBACKNUM] [static] |
Definition at line 39 of file demo_kinematic.cpp.
dJointID fixed_joint_ids[NUM_OBST] |
Definition at line 95 of file demo_kinematic.cpp.
dBodyID fixed_obst_ids[NUM_OBST] |
Definition at line 91 of file demo_kinematic.cpp.
dBody fixed_obstacles[NUM_OBST] |
Definition at line 92 of file demo_kinematic.cpp.
int force_group = 0 [static] |
Definition at line 42 of file demo_kinematic.cpp.
Definition at line 53 of file demo_kinematic.cpp.
std::vector<int> force_sign |
Definition at line 54 of file demo_kinematic.cpp.
float forearm_length = 0 |
Definition at line 79 of file demo_kinematic.cpp.
float forearm_width = 0 |
Definition at line 80 of file demo_kinematic.cpp.
MyFeedback frict_feedbacks[NUM_OBST] [static] |
Definition at line 40 of file demo_kinematic.cpp.
dBox* g_link1 |
Definition at line 108 of file demo_kinematic.cpp.
dBox* g_link2 |
Definition at line 112 of file demo_kinematic.cpp.
dBox* g_link3 |
Definition at line 116 of file demo_kinematic.cpp.
dBox* geom_mobile_base |
Definition at line 122 of file demo_kinematic.cpp.
dHingeJoint* hinge1 |
Definition at line 109 of file demo_kinematic.cpp.
dHingeJoint* hinge2 |
Definition at line 113 of file demo_kinematic.cpp.
dHingeJoint* hinge3 |
Definition at line 117 of file demo_kinematic.cpp.
Definition at line 129 of file demo_kinematic.cpp.
Definition at line 119 of file demo_kinematic.cpp.
dJointGroup joints |
Definition at line 97 of file demo_kinematic.cpp.
dBody* link1 |
Definition at line 107 of file demo_kinematic.cpp.
dBody* link2 |
Definition at line 111 of file demo_kinematic.cpp.
dBody* link3 |
Definition at line 115 of file demo_kinematic.cpp.
dBodyID link_ids[3] |
Definition at line 85 of file demo_kinematic.cpp.
double max_friction = 2 |
Definition at line 43 of file demo_kinematic.cpp.
double max_tor_friction = 0.5 |
Definition at line 44 of file demo_kinematic.cpp.
dJointID mobile_base_plane2d_jt_id |
Definition at line 123 of file demo_kinematic.cpp.
int num_used_compliant = NUM_OBST |
Definition at line 47 of file demo_kinematic.cpp.
int num_used_fixed = NUM_OBST |
Definition at line 46 of file demo_kinematic.cpp.
int num_used_movable = NUM_OBST |
Definition at line 45 of file demo_kinematic.cpp.
double obst_damping[NUM_OBST] |
Definition at line 90 of file demo_kinematic.cpp.
double obst_home[NUM_OBST][3] |
Definition at line 88 of file demo_kinematic.cpp.
double obst_stiffness[NUM_OBST] |
Definition at line 89 of file demo_kinematic.cpp.
dBody obstacles[NUM_OBST] |
Definition at line 86 of file demo_kinematic.cpp.
dJointID plane2d_joint_ids[NUM_OBST] |
Definition at line 93 of file demo_kinematic.cpp.
std::vector<double> pt_x |
Definition at line 55 of file demo_kinematic.cpp.
std::vector<double> pt_y |
Definition at line 56 of file demo_kinematic.cpp.
std::vector<double> pt_z |
Definition at line 57 of file demo_kinematic.cpp.
Definition at line 126 of file demo_kinematic.cpp.
dSpace* space |
Definition at line 100 of file demo_kinematic.cpp.
Definition at line 128 of file demo_kinematic.cpp.
float torso_half_width = 0.196 |
Definition at line 75 of file demo_kinematic.cpp.
float torso_half_width_side = 0 |
Definition at line 76 of file demo_kinematic.cpp.
float upper_arm_length = 0.334 |
Definition at line 77 of file demo_kinematic.cpp.
float upper_arm_width = 0 |
Definition at line 78 of file demo_kinematic.cpp.
dWorld* world |
Definition at line 99 of file demo_kinematic.cpp.
std::vector<double> x_c |
Definition at line 69 of file demo_kinematic.cpp.
std::vector<double> x_n |
Definition at line 72 of file demo_kinematic.cpp.
std::vector<double> y_c |
Definition at line 70 of file demo_kinematic.cpp.
std::vector<double> y_n |
Definition at line 73 of file demo_kinematic.cpp.
std::vector<double> z_c |
Definition at line 71 of file demo_kinematic.cpp.
std::vector<double> z_n |
Definition at line 74 of file demo_kinematic.cpp.