├── README.md
├── launch
    └── panda_kinematics_solver.launch
├── CMakeLists.txt
├── package.xml
├── include
    └── panda_kinematics_solver
    │   └── ik_fk_calculator.h
└── src
    └── ik_fk_calculator.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # panda_kinematics_solver
2 | Solving FK and IK for Franka-Emika panda robot in ROS
3 | 


--------------------------------------------------------------------------------
/launch/panda_kinematics_solver.launch:
--------------------------------------------------------------------------------
 1 |  
 2 | <launch>
 3 |   <include file="$(find panda_moveit_config)/launch/planning_context.launch">
 4 |     <arg name="load_robot_description" value="true"/>
 5 |   </include>
 6 | 
 7 |   <node name="ik_fk_calculator"
 8 |         pkg="panda_kinematics_solver"
 9 |         type="ik_fk_calculator"
10 |         respawn="false" output="screen">
11 |     <rosparam command="load"
12 |               file="$(find panda_moveit_config)/config/kinematics.yaml"/>
13 |   </node>
14 |   
15 | </launch>


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(panda_kinematics_solver)
 3 | 
 4 | add_compile_options(-std=c++11)
 5 | 
 6 | find_package(catkin REQUIRED
 7 |     roscpp
 8 |     moveit_ros_planning_interface
 9 |     sensor_msgs
10 | )
11 | 
12 | find_package(Eigen3 REQUIRED)
13 | find_package(Boost REQUIRED system filesystem date_time thread)
14 | 
15 | catkin_package(
16 |   INCLUDE_DIRS include 
17 |   LIBRARIES 
18 |   CATKIN_DEPENDS 
19 |     moveit_core
20 |   DEPENDS 
21 |     EIGEN3
22 | )
23 | 
24 | include_directories(
25 |  include
26 |  ${catkin_INCLUDE_DIRS}
27 |  ${EIGEN3_INCLUDE_DIRS}
28 |  ${Boost_INCLUDE_DIR}
29 | )
30 | 
31 | add_executable(ik_fk_calculator src/ik_fk_calculator.cpp)
32 | # add_dependencies(ik_fk_calculator panda_kinematics_solver_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
33 | target_link_libraries(ik_fk_calculator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
34 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>panda_kinematics_solver</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The panda_kinematics_solver package</description>
 6 | 
 7 |   <maintainer email="kosmas@todo.todo">kosmas</maintainer>
 8 | 
 9 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
10 |   <license>TODO</license>
11 | 
12 |   <!-- <url type="website">http://wiki.ros.org/panda_kinematics_solver</url> -->
13 | 
14 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
15 | 
16 | 
17 |   <buildtool_depend>catkin</buildtool_depend>
18 |   <depend>roscpp</depend>
19 |   <build_depend>eigen</build_depend>
20 |   <depend>moveit_core</depend>
21 |   <depend>sensor_msgs</depend>
22 |   
23 | 
24 | 
25 |   <!-- The export tag contains other, unspecified, tags -->
26 |   <export>
27 |     <!-- Other tools can request additional information be placed here -->
28 | 
29 |   </export>
30 | </package>
31 | 


--------------------------------------------------------------------------------
/include/panda_kinematics_solver/ik_fk_calculator.h:
--------------------------------------------------------------------------------
 1 | #ifndef IK_FK_CALCULATOR
 2 | #define IK_FK_CALCULATOR
 3 | 
 4 | #include <iostream>
 5 | #include <ros/ros.h>
 6 | 
 7 | // MoveIt
 8 | #include <moveit/robot_model_loader/robot_model_loader.h>
 9 | #include <moveit/robot_model/robot_model.h>
10 | #include <moveit/robot_state/robot_state.h>
11 | #include <moveit/robot_model/joint_model_group.h>
12 | 
13 | // Message types
14 | #include <sensor_msgs/JointState.h>
15 | #include <moveit_msgs/MoveGroupActionGoal.h>
16 | 
17 | 
18 | namespace panda_kinematics_solver{
19 | class IkFkCalculator
20 | {
21 |     private:
22 |         ros::NodeHandle _nh;
23 |         robot_model::RobotModelPtr _kinematic_model;
24 |         robot_state::RobotState* _kinematic_state;
25 |         std::vector<double> _joint_values;
26 |         std::vector<double> _goal_joint_values;
27 |         std::vector<std::string> _joint_names;
28 | 
29 |         robot_state::JointModelGroup* _joint_model_group;
30 | 
31 |         void jointCallback(const sensor_msgs::JointStateConstPtr &msg);
32 |         void getJointValues();  
33 |         Eigen::Isometry3d calculateFK();
34 |         void calculateIK(Eigen::Isometry3d end_effector_state);
35 |         void calculateJacobian();
36 | 
37 |     public:
38 |         IkFkCalculator();
39 |         ~IkFkCalculator();
40 | };
41 | }
42 | #endif


--------------------------------------------------------------------------------
/src/ik_fk_calculator.cpp:
--------------------------------------------------------------------------------
  1 | #include "panda_kinematics_solver/ik_fk_calculator.h"
  2 | 
  3 | namespace panda_kinematics_solver{
  4 | 
  5 | IkFkCalculator::IkFkCalculator(){
  6 |     // RobotModelLoader
  7 |     robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  8 |     _kinematic_model = robot_model_loader.getModel();
  9 | 
 10 |     ROS_INFO("Model frame: %s", _kinematic_model->getModelFrame().c_str());
 11 | 
 12 |     // We construct a RobotState that maintains the configuration of the robot
 13 |     // Joint states are set to default and then get a JointModelGroup, which represents the robot model for the panda_arm_hand group of the robot
 14 |     _kinematic_state = new robot_state::RobotState(_kinematic_model);
 15 |     _kinematic_state->setToDefaultValues(); // Set the joints in group to the position name defined in the SRDF
 16 |     _joint_model_group = _kinematic_model->getJointModelGroup("panda_arm");
 17 | 
 18 |     _joint_names = _joint_model_group->getVariableNames();
 19 | 
 20 |      getJointValues(); // from /joint_states ROS topic
 21 | 
 22 |     _kinematic_state->setJointGroupPositions(_joint_model_group, _joint_values);
 23 | 
 24 |     ROS_INFO_STREAM("Current state is " << (_kinematic_state->satisfiesBounds() ? "valid" : "not valid"));
 25 | 
 26 |      Eigen::Isometry3d end_effector_state = calculateFK();
 27 | 
 28 |      calculateIK(end_effector_state);
 29 | 
 30 |      calculateJacobian();
 31 | }
 32 | 
 33 | IkFkCalculator::~IkFkCalculator(){
 34 |     delete _kinematic_state;
 35 |     delete _joint_model_group;
 36 | }
 37 | 
 38 | void IkFkCalculator::getJointValues(){
 39 |     sensor_msgs::JointState js;
 40 |     try {
 41 |         js = *(ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states", _nh));
 42 |     }
 43 |     catch(const std::exception& e){
 44 |     ROS_WARN("Message from /joint_states not received\n");
 45 |     }
 46 | 
 47 |     _joint_values.clear();
 48 | 
 49 |     _joint_values.push_back(js.position[2]); // panda_joint1
 50 |     _joint_values.push_back(js.position[3]); // panda_joint2
 51 |     _joint_values.push_back(js.position[4]); // panda_joint3
 52 |     _joint_values.push_back(js.position[5]); // panda_joint4
 53 |     _joint_values.push_back(js.position[6]); // panda_joint5
 54 |     _joint_values.push_back(js.position[7]); // panda_joint6
 55 |     _joint_values.push_back(js.position[8]); // panda_joint7
 56 |     _joint_values.push_back(js.position[0]); // panda_finger_joint1
 57 |     _joint_values.push_back(js.position[1]); // panda_finger_joint2
 58 | 
 59 |     ROS_INFO("\nCurrent set of joint values:");
 60 | 
 61 |     for (std::size_t i=0; i<_joint_names.size(); ++i){
 62 |         ROS_INFO("Joint %s: %f", _joint_names[i].c_str(), _joint_values[i]);
 63 |     }
 64 | }
 65 | 
 66 | Eigen::Isometry3d IkFkCalculator::calculateFK(){
 67 |     ROS_INFO_STREAM("\n\n**Solving forward kinematics**\n");
 68 | 
 69 |     // _kinematic_state->setToRandomPositions(_joint_model_group);
 70 |     Eigen::Isometry3d end_effector_state = _kinematic_state->getGlobalLinkTransform("panda_link8");
 71 | 
 72 |     ROS_INFO_STREAM("End-effector pose (in the model frame) : ");
 73 |     ROS_INFO_STREAM("Translation: \n" << end_effector_state.translation() << "\n");
 74 |     ROS_INFO_STREAM("Rotation: \n" << end_effector_state.rotation() << "\n");
 75 | 
 76 |     return end_effector_state;
 77 | }
 78 | 
 79 | void IkFkCalculator::calculateIK(Eigen::Isometry3d end_effector_state){
 80 |     ROS_INFO_STREAM("\n\n**Solving inverse kinematics**\n");
 81 | 
 82 |     double timeout = 0.1;
 83 |     bool found_ik = _kinematic_state->setFromIK(_joint_model_group, end_effector_state, timeout);
 84 | 
 85 |     if (found_ik){
 86 |         _kinematic_state->copyJointGroupPositions(_joint_model_group, _joint_values);
 87 |         for (std::size_t i=0; i<_joint_names.size(); ++i){
 88 |             ROS_INFO("Joint %s: %f", _joint_names[i].c_str(), _joint_values[i]);
 89 |         }
 90 |     }
 91 |     else{
 92 |         ROS_INFO("Did not find IK solution\n");
 93 |     }
 94 | }
 95 | 
 96 | void IkFkCalculator::calculateJacobian(){
 97 |     // Each column in the Jacobian matrix represents the effect on end-effector velocities due to variation in each joint velocity.
 98 |     Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
 99 |     Eigen::MatrixXd jacobian;
100 |     _kinematic_state->getJacobian(_joint_model_group, _kinematic_state->getLinkModel(_joint_model_group->getLinkModelNames().back()), reference_point_position, jacobian);
101 |     ROS_INFO_STREAM("\n\n**Jacobian** \n" << jacobian << "\n");
102 | }
103 | 
104 | 
105 | } // namespace panda_kinematics_solver
106 | 
107 | int main(int argc, char** argv)
108 | {
109 |     ros::init(argc, argv, "ik_fk_calculator");
110 | 
111 |     panda_kinematics_solver::IkFkCalculator calc;
112 | 
113 |     ros::spin();
114 |     return 0;
115 | }
116 | 


--------------------------------------------------------------------------------