Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#include <ur_kinematics/ur_moveit_plugin.h>
#include <geometry_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <ur_kinematics/ur_kin.h>
//register URKinematicsPlugin as a KinematicsBase implementation
PLUGINLIB_EXPORT_CLASS(ur_kinematics::URKinematicsPlugin,kinematics::KinematicsBase);
namespace ur_kinematics
{
URKinematicsPlugin::URKinematicsPlugin() {}
//URKinematicsPlugin::~URKinematicsPlugin() {}
bool URKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options) const
{}
bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options) const
{}
bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &solution,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options) const
{}
bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options) const
{}
bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
const std::vector<double> &consistency_limits,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const kinematics::KinematicsQueryOptions &options) const
{}
bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const
{
double q[6];
for(int i=0; i<6; i++)
q[i] = joint_angles[i];
double T[6][16];
forward_all(q, T[0], T[1], T[2], T[3], T[4], T[5]);
for(int i=0; i<6; i++) {
aa
}
}
/**
* @brief Initialization function for the kinematics
* @return True if initialization was successful, false otherwise
*/
bool URKinematicsPlugin::initialize(const std::string& robot_description,
const std::string& group_name,
const std::string& base_frame,
const std::string& tip_frame,
double search_discretization)
{
setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
fk_solver_info_.joint_names.push_back("shoulder_pan_joint");
fk_solver_info_.joint_names.push_back("shoulder_lift_joint");
fk_solver_info_.joint_names.push_back("elbow_joint");
fk_solver_info_.joint_names.push_back("wrist_1_joint");
fk_solver_info_.joint_names.push_back("wrist_2_joint");
fk_solver_info_.joint_names.push_back("wrist_3_joint");
fk_solver_info_.link_names.push_back("base_link");
fk_solver_info_.link_names.push_back("shoulder_link");
fk_solver_info_.link_names.push_back("upper_arm_link");
fk_solver_info_.link_names.push_back("forearm_link");
fk_solver_info_.link_names.push_back("wrist_1_link");
fk_solver_info_.link_names.push_back("wrist_2_link");
fk_solver_info_.link_names.push_back("wrist_3_link");
fk_solver_info_.link_names.push_back("ee_link");
return true;
}
/**
* @brief Return all the joint names in the order they are used internally
*/
const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
{
return fk_solver_info_.joint_names;
}
/**
* @brief Return all the link names in the order they are represented internally
*/
const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
{
return fk_solver_info_.link_names;
}
}