#include "ros/ros.h"
#include "kinematics_msgs/GetPositionIK.h"
#include <string>
#include <vector>
#include <ostream>
#include "ros/serialization.h"
#include "ros/builtin_message_traits.h"
#include "ros/message_operations.h"
#include "ros/message.h"
#include "ros/time.h"
#include "ros/service_traits.h"
#include "std_msgs/Header.h"
#include "geometry_msgs/Pose.h"
#include "motion_planning_msgs/RobotState.h"
#include "segment.hpp"
#include "chain.hpp"
#include <map>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "tinystr.h"
#include <boost/function.hpp>
#include "link.h"
#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/PoseStamped.h"
#include "btMatrix3x3.h"
#include "ros/console.h"
#include "utilities/kdl-config.h"
#include "utilities/utility.h"
Go to the source code of this file.
Functions | |
void | baseTwistCallback (const nav_msgs::Odometry::ConstPtr &msg) |
void | cartTwistCallback (const geometry_msgs::Twist::ConstPtr &msg) |
void | controllerStateCallback (const sensor_msgs::JointState::ConstPtr &msg) |
KDL::Twist | getTwist (KDL::Frame F_ist) |
int | main (int argc, char **argv) |
void | Manipulability_potentialfield_lookup () |
JntArray | parseJointStates (std::vector< std::string > names, std::vector< double > positions) |
void | sendVel (JntArray q_t, JntArray q_dot) |
bool | SyncMMTrigger (cob_srvs::Trigger::Request &request, cob_srvs::Trigger::Response &response) |
Variables | |
ros::Publisher | arm_pub_ |
ros::Publisher | base_pub_ |
KDL::Twist | baseTwist |
KDL::Chain | chain |
KDL::Chain | chain_base_arm0 |
KDL::Twist | extTwist |
ChainFkSolverPos_recursive * | fksolver1 |
ChainIkSolverVel_pinv * | iksolver1v |
ChainIkSolverPos_NR * | iksolverpos |
ros::Time | last |
double | lastgradx = 0.0 |
double | lastgrady = 0.0 |
KDL::JntArray | q |
bool | RunSyncMM = false |
bool | started = false |
KDL::JntArray | VirtualQ |
void baseTwistCallback | ( | const nav_msgs::Odometry::ConstPtr & | msg | ) |
Definition at line 87 of file kuka_test.cpp.
void cartTwistCallback | ( | const geometry_msgs::Twist::ConstPtr & | msg | ) |
Definition at line 76 of file kuka_test.cpp.
void controllerStateCallback | ( | const sensor_msgs::JointState::ConstPtr & | msg | ) |
Definition at line 234 of file kuka_test.cpp.
KDL::Twist getTwist | ( | KDL::Frame | F_ist | ) |
Definition at line 129 of file kuka_test.cpp.
int main | ( | int | argc, | |
char ** | argv | |||
) |
Definition at line 259 of file kuka_test.cpp.
void Manipulability_potentialfield_lookup | ( | ) |
Definition at line 142 of file kuka_test.cpp.
JntArray parseJointStates | ( | std::vector< std::string > | names, | |
std::vector< double > | positions | |||
) |
Definition at line 54 of file kuka_test.cpp.
void sendVel | ( | JntArray | q_t, | |
JntArray | q_dot | |||
) |
Definition at line 199 of file kuka_test.cpp.
bool SyncMMTrigger | ( | cob_srvs::Trigger::Request & | request, | |
cob_srvs::Trigger::Response & | response | |||
) |
Definition at line 187 of file kuka_test.cpp.
ros::Publisher arm_pub_ |
Definition at line 49 of file kuka_test.cpp.
ros::Publisher base_pub_ |
Definition at line 50 of file kuka_test.cpp.
KDL::Twist baseTwist |
Definition at line 38 of file kuka_test.cpp.
KDL::Chain chain |
Definition at line 32 of file kuka_test.cpp.
KDL::Chain chain_base_arm0 |
Definition at line 33 of file kuka_test.cpp.
KDL::Twist extTwist |
Definition at line 37 of file kuka_test.cpp.
ChainFkSolverPos_recursive* fksolver1 |
Definition at line 43 of file kuka_test.cpp.
ChainIkSolverVel_pinv* iksolver1v |
Definition at line 44 of file kuka_test.cpp.
ChainIkSolverPos_NR* iksolverpos |
Definition at line 45 of file kuka_test.cpp.
ros::Time last |
Definition at line 39 of file kuka_test.cpp.
double lastgradx = 0.0 |
Definition at line 40 of file kuka_test.cpp.
double lastgrady = 0.0 |
Definition at line 41 of file kuka_test.cpp.
KDL::JntArray q |
Definition at line 35 of file kuka_test.cpp.
bool RunSyncMM = false |
Definition at line 48 of file kuka_test.cpp.
bool started = false |
Definition at line 36 of file kuka_test.cpp.
KDL::JntArray VirtualQ |
Definition at line 34 of file kuka_test.cpp.