Beaucay's profile - activity

When you have a KDL::Chain, is it possible to modify it?

I have a chain that I extracted from a tree. Feeding it to a KDL::ChainFkSolverPos_recursive successfully computes a pose. Now I would like to modify some of the parameters. But when I get a Segment or a Joint from a Segment, all values I could possibly extract are equal to zero and Identity frames:

KDL::Chain chain = getChain();
KDL::ChainFkSolverPos_recursive fkSolver(chain);
KDL::JntArray q(chain.getNrOfJoints());

for(int i = 0; i<chain.getNrOfJoints(); i++)
q(i) = 0;

KDL::Frame fr;
fkSolver.JntToCart(q, fr); //fr.p becomes [0, 0.43, 0.30]


for(int i = 0; i<chain.getNrOfSegments(); i++){
KDL::Segment segment = chain.getSegment(i);

KDL::Frame frame = segment.getFrameToTip();
KDL::Frame frame1 = segment.pose(0);

std::cout<<segment.getName(); //gives the segment's name e.g. "world"

KDL::Joint joint = segment.getJoint();

KDL::Frame frame2 = joint.pose(0);

KDL::Vector vector = joint.JointOrigin();

std::cout<<"a"<<frame.M.data[0]<<"\n"; //always 1
std::cout<<"b"<<frame1.M.data[0]<<"\n"; //always 1
std::cout<<"c"<<frame.p.y()<<"\n"; //always 0
std::cout<<"d"<<frame1.p.y()<<"\n"; //always 0

std::cout<<"e"<<frame2.M.data[0]<<"\n"; //always 1
std::cout<<"g"<<frame2.p.y()<<"\n"; //always 0


std::cout<<vector.x()<<"\n"; //always 0
}

I found a solution, but I doubt it's the right one. It looks more like a workaround to me. Anyway:

    KDL::SegmentMap::const_iterator current_segment = tree.getRootSegment();
    chain.addSegment(current_segment->second.segment);
    std::vector<KDL::SegmentMap::const_iterator> children_vector = current_segment->second.children;

    while(children_vector.size()>0){
        current_segment = current_segment->second.children[0];
        chain.addSegment(current_segment->second.segment);
        children_vector = current_segment->second.children;
    }

This means, I build up the Chain elementwise instead of using the getChain-function from tree.

did you figure out the answer to the first question? I'm having the same problem..

In python I was able to read in a urdf calculate the end effector pose coming from joint angles and later manipulating the urdf by accessing the urdf.joints directly:

urdf = URDF.from_xml_file(PATH)
kdl_kin = KDLKinematics(urdf, urdf.links[0].name, urdf.links[-1].name)
pose = kdl_kin.forward(joints.position)

urdf.joints[0].origin.xyz = [1, 2, 3]

Now I would like to achieve the same in C++. I am currently trying to use the KDL library to do this:

KDL::Tree tree;
kdl_parser::treeFromFile(PATH);

As I see it, I should extract the chain from the tree and then use the ChainFkSolverPos to get a pose. But for extracting the chain I need the names of segments, which I can not as easily access as I did in Python. How do you get the first and last sections' names? Or is my approach totally wrong?

In python I was able to read in a urdf calculate the end effector pose coming from joint angles and later manipulating the urdf by accessing the urdf.joints directly:

urdf = URDF.from_xml_file(PATH)
kdl_kin = KDLKinematics(urdf, urdf.links[0].name, urdf.links[-1].name)
pose = kdl_kin.forward(joints.position)

urdf.joints[0].origin.xyz = [1, 2, 3]

Now I would like to achieve the same in C++. I am currently trying to use the KDL library to do this:

KDL::Tree tree;
kdl_parser::treeFromFile(PATH);

As I see it, I should extract the chain from the tree and then use the ChainFkSolverPos to get a pose. But for extracting the chain I need the names of segments, which I can not as easily access as I did in Python. How do you get the first and last sections' names? Or is my approach totally wrong?

thank you very much! adding this to the CMakeLists.txt and package.xml fixed the problem

I tried adapting the Actionlib Client-Tutorial to a action I already used in some python scripts but as soon as I insert

add_executable(urdf_optimizer_beta src/urdf_optimizer_beta.cpp)
target_link_libraries(urdf_optimizer_beta ${catkin_LIBRARIES})
add_dependencies(urdf_optimizer_beta pilatus_EXPORTED_TARGETS)

into the CMakeLists.txt i get the following error when trying to catkin_make

CMakeFiles/urdf_optimizer_beta.dir/src/urdf_optimizer_beta.cpp.o: In function `actionlib::GoalManager<pilatus::DrivePosesAction_<std::allocator<void> > >::GoalManager(boost::shared_ptr<actionlib::DestructionGuard> const&)':
/opt/ros/indigo/include/actionlib/client/client_helpers.h:80: undefined reference to `actionlib::GoalIDGenerator::GoalIDGenerator()'
CMakeFiles/urdf_optimizer_beta.dir/src/urdf_optimizer_beta.cpp.o: In function `actionlib::ActionClient<pilatus::DrivePosesAction_<std::allocator<void> > >::initClient(ros::CallbackQueueInterface*)':
/opt/ros/indigo/include/actionlib/client/action_client.h:214: undefined reference to `actionlib::ConnectionMonitor::ConnectionMonitor(ros::Subscriber&, ros::Subscriber&)'
/opt/ros/indigo/include/actionlib/client/action_client.h:217: undefined reference to `actionlib::ConnectionMonitor::goalDisconnectCallback(ros::SingleSubscriberPublisher const&)'
/opt/ros/indigo/include/actionlib/client/action_client.h:217: undefined reference to `actionlib::ConnectionMonitor::goalConnectCallback(ros::SingleSubscriberPublisher const&)'
/opt/ros/indigo/include/actionlib/client/action_client.h:221: undefined reference to `actionlib::ConnectionMonitor::cancelDisconnectCallback(ros::SingleSubscriberPublisher const&)'
/opt/ros/indigo/include/actionlib/client/action_client.h:221: undefined reference to `actionlib::ConnectionMonitor::cancelConnectCallback(ros::SingleSubscriberPublisher const&)'
CMakeFiles/urdf_optimizer_beta.dir/src/urdf_optimizer_beta.cpp.o: In function `actionlib::ActionClient<pilatus::DrivePosesAction_<std::allocator<void> > >::statusCb(ros::MessageEvent<actionlib_msgs::GoalStatusArray_<std::allocator<void> > const> const&)':
/opt/ros/indigo/include/actionlib/client/action_client.h:266: undefined reference to `actionlib::ConnectionMonitor::processStatus(boost::shared_ptr<actionlib_msgs::GoalStatusArray_<std::allocator<void> > const> const&, std::string const&)'
collect2: error: ld returned 1 exit status
make[2]: *** [devel/lib/pilatus/urdf_optimizer_beta] Error 1
make[1]: *** [CMakeFiles/urdf_optimizer_beta.dir/all] Error 2
make: *** [all] Error 2

The code that causes this crash is rather simple:

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <pilatus/DrivePosesAction.h>

int main (int argc, char **argv)
{
    ros::init(argc, argv, "urdf_optimizer_beta");

    actionlib::SimpleActionClient<pilatus::DrivePosesAction> ac("drive_poses", true);

    return 0;
}

my CMakeLists.txt :

cmake_minimum_required(VERSION 2.8.3)
project(pilatus)

## Find catkin and any catkin packages
find_package(
        catkin
        REQUIRED COMPONENTS
        roscpp
        rospy
        std_msgs
        genmsg
        actionlib_msgs
        sensor_msgs
        geometry_msgs
)

## Declare ROS messages and services
add_action_files(
        DIRECTORY action
        FILES DrivePoses.action
)

## Generate added messages and services

generate_messages(
        DEPENDENCIES actionlib_msgs std_msgs sensor_msgs geometry_msgs
)

## Declare a catkin package
catkin_package(
        CATKIN_DEPENDS actionlib_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker pilatus_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener    pilatus_generate_messages_cpp)

add_executable(urdf_optimizer_beta src/urdf_optimizer_beta.cpp)
target_link_libraries(urdf_optimizer_beta ${catkin_LIBRARIES})
add_dependencies(urdf_optimizer_beta pilatus_EXPORTED_TARGETS)

Because of my python scripts which use the "DrivePose.action" successfully, I know that the action per se works. I therefore assume that the error must lie somewhere within the CMakeLists.txt . Any ideas what this could be?

Hi everyone

I'm trying to install industrial calibration and I therefore cloned the git repository into my catkin_ws and tried catkin_make.

I then got the following error:

CMake Warning at /opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake:76 (find_package):
  Could not find a package configuration file provided by "openni2_camera"
  with any of the following names:

    openni2_cameraConfig.cmake
    openni2_camera-config.cmake

  Add the installation prefix of "openni2_camera" to CMAKE_PREFIX_PATH or set
  "openni2_camera_DIR" to a directory containing one of the above files.  If
  "openni2_camera" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  industrial_calibration/rgbd_depth_correction/CMakeLists.txt:4 (find_package)


-- Could not find the required component 'openni2_camera'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/indigo/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "openni2_camera"
  with any of the following names:

    openni2_cameraConfig.cmake
    openni2_camera-config.cmake

  Add the installation prefix of "openni2_camera" to CMAKE_PREFIX_PATH or set
  "openni2_camera_DIR" to a directory containing one of the above files.  If
  "openni2_camera" provides a separate development package or SDK, be sure it
  has been installed.
Call Stack (most recent call first):
  industrial_calibration/rgbd_depth_correction/CMakeLists.txt:4 (find_package)


-- Configuring incomplete, errors occurred!
See also "/home/user/catkin_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/user/catkin_ws/build/CMakeFiles/CMakeError.log".
Invoking "cmake" failed

so I tried to install openni2:

user@hyperchicken:~/catkin_ws/src$ sudo apt-get install ros-indigo-openni2-camera 
[sudo] password for user: 
Reading package lists... Done
Building dependency tree       
Reading state information... Done
You might want to run 'apt-get -f install' to correct these:
The following packages have unmet dependencies:
 ros-indigo-openni2-camera : Depends: libopenni2-0 but it is not going to be installed
                             Depends: libopenni2-dev but it is not going to be installed
 ros-indigo-ur-kinematics : Depends: ros-indigo-moveit-kinematics but it is not going to be installed
E: Unmet dependencies. Try 'apt-get -f install' with no packages (or specify a solution).

I would be happy for any hint how to try fixing this problem.

Thank you!