#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>

Include dependency graph for demo_kinematic.cpp:

Classes
struct	MyFeedback
Functions
void	base_ep_cb (const hrl_msgs::FloatArrayBare msg)
void	create_compliant_obstacles (ros::NodeHandle n)
void	create_fixed_obstacles (ros::NodeHandle n)
void	create_movable_obstacles (ros::NodeHandle n)
void	create_robot (ros::NodeHandle n)
double	get_wall_clock_time ()
void	go_initial_position (ros::NodeHandle n)
void	inner_torque_loop ()
std::vector< double >	jep (3, 0)
void	jep_cb (const hrl_msgs::FloatArrayBare msg)
std::vector< double >	k_d (3, 0)
std::vector< double >	k_p (3, 0)
int	main (int argc, char **argv)
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)
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)
void	taxel_simulation_code (double resolution)
std::vector< double >	torques (3, 0)
Variables
dBody *	body_mobile_base
dBody	compliant_obstacles [NUM_OBST]
dJointID	compliant_plane2d_joint_ids [NUM_OBST]
hrl_haptic_manipulation_in_clutter_msgs::BodyDraw	draw
dBody *	env
static int	fbnum = 0
static MyFeedback	feedbacks [MAX_FEEDBACKNUM]
dJointID	fixed_joint_ids [NUM_OBST]
dBodyID	fixed_obst_ids [NUM_OBST]
dBody	fixed_obstacles [NUM_OBST]
static int	force_group = 0
std::vector< int >	force_grouping
std::vector< int >	force_sign
float	forearm_length = 0
float	forearm_width = 0
static MyFeedback	frict_feedbacks [NUM_OBST]
dBox *	g_link1
dBox *	g_link2
dBox *	g_link3
dBox *	geom_mobile_base
dHingeJoint *	hinge1
dHingeJoint *	hinge2
dHingeJoint *	hinge3
hrl_haptic_manipulation_in_clutter_msgs::MechanicalImpedanceParams	impedance_params
int	include_mobile_base
dJointGroup	joints
dBody *	link1
dBody *	link2
dBody *	link3
dBodyID	link_ids [3]
double	max_friction = 2
double	max_tor_friction = 0.5
dJointID	mobile_base_plane2d_jt_id
int	num_used_compliant = NUM_OBST
int	num_used_fixed = NUM_OBST
int	num_used_movable = NUM_OBST
double	obst_damping [NUM_OBST]
double	obst_home [NUM_OBST][3]
double	obst_stiffness [NUM_OBST]
dBody	obstacles [NUM_OBST]
dJointID	plane2d_joint_ids [NUM_OBST]
std::vector< double >	pt_x
std::vector< double >	pt_y
std::vector< double >	pt_z
hrl_haptic_manipulation_in_clutter_msgs::SkinContact	skin
dSpace *	space
hrl_haptic_manipulation_in_clutter_msgs::TaxelArray	taxel
float	torso_half_width = 0.196
float	torso_half_width_side = 0
float	upper_arm_length = 0.334
float	upper_arm_width = 0
dWorld *	world
std::vector< double >	x_c
std::vector< double >	x_n
std::vector< double >	y_c
std::vector< double >	y_n
std::vector< double >	z_c
std::vector< double >	z_n