Giter Club home page Giter Club logo

iiwa_ros's Introduction

iiwa_ros

ROS Stack for KUKA's IIWA robots

  • Use the Fast Research Interface (FRI) to connect to the robot
  • Integration with ROS control
  • Gazebo integration with gravity compensation (similar to real robot)

Requirements

iiwa_ros requires several packages to be installed in order to work properly:

Dependencies

KUKA FRI (Private)

This repo can be omitted if the robot is only used in simulation.

cd /source/directory
git clone https://github.com/epfl-lasa/kuka_fri.git
cd kuka_fri
# Apply SIMD patch:
wget https://gist.githubusercontent.com/matthias-mayr/0f947982474c1865aab825bd084e7a92/raw/244f1193bd30051ae625c8f29ed241855a59ee38/0001-Config-Disables-SIMD-march-native-by-default.patch
git am 0001-Config-Disables-SIMD-march-native-by-default.patch
# Build
./waf configure
./waf
sudo ./waf install

SpaceVecAlg

cd /source/directory
git clone --recursive https://github.com/jrl-umi3218/SpaceVecAlg.git
cd SpaceVecAlg
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release -DPYTHON_BINDING=OFF ..
make -j
sudo make install

To compile with SIMD flags (e.g. because you enabled it for robot_controllers and iiwa_ros), you can do cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-march=native -faligned-new" -DPYTHON_BINDING=OFF .. instead of the above. Also, instead of passing -DCMAKE_CXX_FLAGS="-march=native -faligned-new" for SpaceVecAlg, RBDyn and mc_rbdyn_urdf builds you can also set the CXXFLAGS environment variables and omit the option:

export CXXFLAGS="-march=native -faligned-new"

The same holds for RBDyn and mc_rbdyn_urdf.

RBDyn

cd /source/directory
git clone --recursive https://github.com/jrl-umi3218/RBDyn.git
cd RBDyn
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release -DPYTHON_BINDING=OFF ..
make -j
sudo make install

mc_rbdyn_urdf

cd /source/directory
git clone --recursive https://github.com/jrl-umi3218/mc_rbdyn_urdf.git
cd mc_rbdyn_urdf
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release -DPYTHON_BINDING=OFF ..
make -j
sudo make install

corrade

We are using a specific version of Corrade.

cd /source/directory
git clone https://github.com/mosra/corrade.git
cd corrade
git checkout 0d149ee9f26a6e35c30b1b44f281b272397842f5
mkdir build && cd build
cmake ..
make -j
sudo make install

robot_controllers

cd /source/directory
git clone https://github.com/epfl-lasa/robot_controllers.git
cd robot_controllers
mkdir build && cd build
cmake ..
make -j
sudo make install

Compilation

If a simulation-only setup is enough or you do not have access to KUKA FRI, the robot driver compilation can be disabled with

touch /path/to/ros_workspace/iiwa_ros/iiwa_driver/CATKIN_IGNORE

Build the workspace:

cd /path/to/ros_workspace
# source ros workspace
catkin_make

Sunrise Robot Application upload

You need a specific application to run on the robot side. 0. Make sure a Windows laptop is connected on the X66 Ethernet port and has IP 172.31.1.42 mask 255.255.0.0

  1. Create a new Sunrise project with Sunrise Workbench and setup an empty RobotApplication template
  2. Setup the safety configuration in SafetyConfiguration.sconf (example)
  3. Replace the empty template with the online app
  4. In StationSetup.cat, tab Software, active the FRI extension, push it to the robot with Installation and accept the reboot question
  5. Synchronise your new Sunrise project to the robot with icon Synchronize project
  6. On the Smartpad tablet, your app must be listed in [Applications] and you must also see a new [FRI] tab

Basic Usage

Bringup iiwa_driver

Control IIWA with FRI

  1. Make sure your Linux/ROS laptop is connected on the KONI Ethernet port and has IP 192.170.10.1 mask 255.255.255.0.
  2. On the Smartpad tablet:
  • Activate AUT mode (turn key right > AUT > key left)
  • In [Application], check yours in order to select it
  • Press the mechanical Play button ▶
  1. Within 10 seconds before the timeout, launch: roslaunch iiwa_driver iiwa_bringup.launch. This will connect to IIWA robot using FRI. 4 The Smartpad lets you select control mode and stiffness
  2. Check that everything works if /iiwa/joint_states is being published and reflects the actual robot state.
  3. The Smarpad'd [Application] tab must remain green. Otherwise you can press Play ▶ again to reconnect.

In case of a hard failure, unload the app by unchecking it in [Application] before retrying.

Gazebo Simulation

To launch Gazebo with IIWA

roslaunch iiwa_gazebo iiwa_gazebo.launch

Both of the above commands will launch IIWA in torque-control mode. To change the control mode (e.g., position-control), please edit the launch files to select the appropriate controller.

MoveIt planning

If everything looks in simulation or with the FRI driver, a next step might be to try out your robot with MoveIt.

Contributing

iiwa_ros is being actively developed. Please see CONTRIBUTING for more on how to help.

Acknowledgements

The URDF description files are copied and refactored from iiwa_stack (by Salvatore Virga and Marco Esposito).

Authors/Maintainers

Other Contributors

Citing iiwa_ros

@software{iiwa2019github,
  author = {Chatzilygeroudis, Konstantinos and Mayr, Matthias and Fichera, Bernardo and Billard, Aude},
  title = {iiwa_ros: A ROS Stack for KUKA's IIWA robots using the Fast Research Interface},
  url = {http://github.com/epfl-lasa/iiwa_ros},
  year = {2019},
}

iiwa_ros's People

Contributors

athapoly avatar costashatz avatar emielkemmm avatar gergondet avatar guptavaibhav0 avatar jimmydasilva avatar matthias-mayr avatar nash169 avatar ymollard avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

iiwa_ros's Issues

how to use iiwa_tool service

I try to use your code regarding FK, but something goes wrong.

I read your code iiwa_service.cpp, however I don't know waht you mean this line. What is the meaning of expression request.joints.layout.dim[0].size. And which correct value should it be set to

I start up the iiwa_gazebo.launch, then I subscribed the topic /iiwa/joint_states, then I I wrote the following code

class IiwaEeClient
{
public:
IiwaEeClient()
{
sub_ = n_.subscribe("/iiwa/joint_states",100, &IiwaEeClient::subStateCallback, this);
client = n_.serviceClient<iiwa_tools::GetFK>("/iiwa/iiwa_fk_server");
}

void subStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
    jointstate.name = msg->name;
    jointstate.header = msg->header;
    jointstate.position = msg->position;
    jointstate.velocity = msg->velocity;
    jointstate.effort = msg->effort;

    // ROS_INFO("The 6th joint %f", jointstate.position[5]);

}

void getIiwaEeState()
{
    iiwa_tools::GetFK srv;
    srv.request.joints.layout.dim.resize(2);
    srv.request.joints.layout.data_offset = 0;
    // srv.request.joints.layout.dim[0].label = "height";
    srv.request.joints.layout.dim[0].size = 1;
    srv.request.joints.layout.dim[0].stride = _n_joints;

    // srv.request.joints.layout.dim[1].label = "width";
    srv.request.joints.layout.dim[1].size = _n_joints;
    srv.request.joints.layout.dim[1].stride = 0;
    srv.request.joints.data.resize(_n_joints);

    for(int point = 0; point < 1; ++point)
    {
        for(size_t i = 0; i < _n_joints; i++)
        {
            srv.request.joints.data[i] = jointstate.position[i];
            ROS_INFO("The %dth position is %f ",i ,srv.request.joints.data[i]);
            // srv.request.joints.data.push_back(jointstate.position[i]);
        }
    }

    if(client.call(srv))
    {
        ROS_INFO("The iiwa_ee_state X: %f Y: %f, Z: %f", srv.response.poses[6].position.x, srv.response.poses[6].position.y, srv.response.poses[6].position.z);
    }
    else
    {
        ROS_ERROR("Failed to call service /iiwa/iiwa_fk_server");
    }
}

private:
ros::NodeHandle n_;
ros::Subscriber sub_;
ros::ServiceClient client;
sensor_msgs::JointState jointstate;
size_t _n_joints = 7; // the number of the joint

};
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "get_iiwa_ee_state");

//Create an object of class SubscribeAndPublish that will take care of everything
IiwaEeClient iiwa_ee_client;
iiwa_ee_client.getIiwaEeState();
// iiwa_ee_client.print_joint_states();

ros::spin();

return 0;
}

but It goes wrong, how should i modify

I think forwardkinematic is using a set of joint angle as input, and compute the poes of the endeffort, I also try rqt_service_caller tools as following and call it
2020-07-03 23-51-30屏幕截图
but it doesn't work correctly.

link iiwa and ros

I am a new user of iiwa. Can you descripe how to link iiwa and ros as you mentioned in you repo step by step. It looks like you do it with FRI. Thank you!

Example of higher-level torque control

I am working on setting up a higher-level torque controller in the sense that I will be using the iiwa_ros torque control mode to expose the /iiwa/TorqueController/command topic, and then my torque controller will be sending commands to that topic. I have some controllers that do this kind of thing, but I'm wondering if you have a higher-level controller you use that you could point me to for guidance. I'm specifically wondering if your higher-level controllers are also implemented in the ros_control framework, or does the usage of that stop with the iiwa_driver?

One concern I have is if I have the KUKA in torque control mode with zero stiffness/damping and I'm sending my own torque commands, if my controller cuts out for some reason while the robot is in motion, it seems there is nothing to prevent the robot from continuing to move. Is there a good mechanism for this situation, that if my high-level controller stops sending torque commands, the robot could enter a relatively safe stop state?

Link with moveit to control real kuka iiwa robot

Hi,

Currently I am already able to send text data based command via rqt-gui to control the real robot using FRI. But I want to do it with visual monitoring. Can this repo mix with moveit to control the real robot by using the motion planning function? I only have the FRI without Serving motors packages in the KUKA side.

Thank you.

Publish commanded position/torque and interpolated setpoint position

Hi,

I am using the position control mode and will be needing the resulting commanded torque.
I think that both the commanded joint position and torque should be published along with the external torque as discussed in #19. The more information, the better.

By replicating what @costashatz did on the external_torque branch, I tested the functions:

  • getCommandedJointPosition
  • getCommandedTorque
  • getIpoJointPosition (could not get this one working)

@costashatz I don't know what plan you had to make your external_torque branch better so I didn't write a PR. But let me know if I can be of any help.

If interested, I made small changes to the external_torque branch on my fork: https://github.com/JimmyDaSilva/iiwa_ros/tree/external_torque

Many thanks,
Jimmy

ClientCommandMode "WRENCH" and "setWrench" in FRI

Hey,

right now the ClientCommandModes are limited to "POSITION" and "TORQUE". Having a look at the FRI documentation a "WRENCH" mode does exist and there is also a method in LBRCommand named "setWrench" to apply a translational force along x,y or z.
Are there any limitations why this couldn't work using iiwa_ros or has it simply not been added yet due to missing applications?

Thanks,
Chris

How to set up properly for realtime capabilities ?

Hi everyone,

I've been experiencing some "noises" or "jumps" on the robot's side when sending slow and smooth commands, in Position control mode.
I assume these jumps come from a bad communication quality because the noise and jumps get amplified (or more frequent) when I intentionally send commands at 199Hz instead of 200Hz.

I started to look at what UR and Franka do in their ROS drivers.
It seems they both recommend to use a realtime linux kernel, with PREEMPT-RT, and give a simple tutorial:
Universal Robot : Setting up Ubuntu with a PREEMPT_RT kernel
Franka Emika : Setting up the real-time kernel

So I have installed a RT kernel as well, and things are a little bit smoother now.
But I am still experiencing some jumps from time to time. And I am not sure if my ros nodes are using the full realtime capabilities of my kernel.

By looking a little bit further into their code, I see that it is required as well to change the thread priority higher by setting the scheduling to SCHED_FIFO:
UR : pthread_setschedparam(this_thread, SCHED_FIFO, &params)
FRANKA : const int thread_priority = sched_get_priority_max(SCHED_FIFO)

Now I am wondering how to do this properly in the context of this package...
iiwa_ros uses ROS controller_manager which seems to be hard-realtime capable as said here: http://wiki.ros.org/controller_manager
I guess we should only have to set the scheduling to SCHED_FIFO in iiwa_driver's thread?
But again I am quite new to these realtime aspects and thread priorities...

In case you are experiencing issues like I do, or have already solved them, I would greatly appreciate if you could reach out to me.
@costashatz I don't know how you are using iiwa_ros and if you require realtime control as I do, but would you be interested in improving this package in that direction?

Truly, I am really curious about what you iiwa_ros users have to say and how you control your IIWA.

Cheers,
Jimmy

FRI ck_compound_return_error

Hey everyone,

I am using FRI with the rosbridge and rossharp to publish some data to Unity while moving the robot. Recently I came across this "CK_COMPOUND_RETURN_ERROR" (see attached image for full log on the smartPAD). It states "possible connection problem" but the disconnect occurs randomly during my movements, sometimes after 30 seconds and sometimes after 10 minutes.
Without posting all of my code, does anyone have an idea where to look for the solution? What could be the cause of such a behaviour?

Best regards

compound error

Can't connect MoveIt! and gazebo

I want to use moveit and gazebo to do co-simulation. I plan the path in RVIZ with moveit, and then if gazebo is connected, the robot in gazebo will also move.

I use following command to open moveit and gazebo in this repository.

  • roslaunch iiwa_gazebo iiwa_gazebo.launch
  • roslaunch iiwa_moveit demo.launch driver:=true

image

The gazebo environment will not move. I think it is not connected properly. I saw this issue
#33

I tried the following command.

_In order to make the setup work for you, the only thing you need to change is this line to contain

as an alternative you can also run
roslaunch iiwa_gazebo iiwa_gazebo.launch controller:=PositionTrajectoryController_

But it still doesn't work, can anybody help me?

Compilation Issue in robot_controllers

Hi guys,
I am in the last step of the documentation for the robot_controllers dependency.
I am getting the following terminal output after the command cmake ..

I am pasting my terminal output here-

robotics@robotics-lab:~/kuka_ws/src/robot_controllers/build$ cmake ..
-- The C compiler identification is GNU 5.4.0
-- The CXX compiler identification is GNU 5.4.0
-- Check for working C compiler: /usr/bin/cc
-- Check for working C compiler: /usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Found PkgConfig: /usr/bin/pkg-config (found version "0.29.1") 
-- Checking for one of the modules 'eigen3'
CMake Warning (dev) at /usr/local/share/cmake/Corrade/FindCorrade.cmake:412 (if):
  Policy CMP0057 is not set: Support new IN_LIST if() operator.  Run "cmake
  --help-policy CMP0057" for policy details.  Use the cmake_policy command to
  set the policy and suppress this warning.

  IN_LIST will be interpreted as an operator when the policy is set to NEW.
  Since the policy is not set the OLD behavior will be used.
Call Stack (most recent call first):
  /usr/local/share/cmake/Corrade/CorradeConfig.cmake:26 (include)
  CMakeLists.txt:34 (find_package)
This warning is for project developers.  Use -Wno-dev to suppress it.

CMake Warning (dev) at /usr/local/share/cmake/Corrade/FindCorrade.cmake:412 (if):
  Policy CMP0057 is not set: Support new IN_LIST if() operator.  Run "cmake
  --help-policy CMP0057" for policy details.  Use the cmake_policy command to
  set the policy and suppress this warning.

  IN_LIST will be interpreted as an operator when the policy is set to NEW.
  Since the policy is not set the OLD behavior will be used.
Call Stack (most recent call first):
  /usr/local/share/cmake/Corrade/CorradeConfig.cmake:26 (include)
  CMakeLists.txt:34 (find_package)
This warning is for project developers.  Use -Wno-dev to suppress it.

CMake Error at /usr/local/share/cmake/Corrade/FindCorrade.cmake:412 (if):
  if given arguments:

    "_component" "IN_LIST" "_CORRADE_LIBRARY_COMPONENTS" "AND" "NOT" "_component" "IN_LIST" "_CORRADE_HEADER_ONLY_COMPONENTS"

  Unknown arguments specified
Call Stack (most recent call first):
  /usr/local/share/cmake/Corrade/CorradeConfig.cmake:26 (include)
  CMakeLists.txt:34 (find_package)


-- Configuring incomplete, errors occurred!
See also "/home/robotics/kuka_ws/src/robot_controllers/build/CMakeFiles/CMakeOutput.log".
robotics@robotics-lab:~/kuka_ws/src/robot_controllers/build$ make -j
make: *** No targets specified and no makefile found.  Stop.
robotics@robotics-lab:~/kuka_ws/src/robot_controllers/build$ sudo make install
make: *** No rule to make target 'install'.  Stop.

combine FRI function with smartservo

As far as I know, Fri library has the function of torque control,and the smartservo library has the function of cartesian pose and cartesian velocity. Both libraries are connected to the manipulator via IP. Is it possible to unify the two, establish a connection with the manipulator through a network port, and use moment control (FRI) and Cartesian control (smartservo) at the same time? If so, how to change the Java code?

Cmake error : Could not find FRI's kuka_fri

Hi @costashatz ,

I followed the instructions on my ROS Computer and compiled the workspace using catkin_make. I got the error on the terminal:


-- +++ processing catkin package: 'iiwa_driver'
-- ==> add_subdirectory(iiwa_ros/iiwa_driver)
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Could not find FRI's kuka_fri
CMake Error at /usr/share/cmake-3.5/Modules/FindPackageHandleStandardArgs.cmake:148 (message):
  Could NOT find FRI (missing: FRI_LIBRARY FRI_INCLUDE_DIR
  FRI_FOUND_COMPONENTS)
Call Stack (most recent call first):
  /usr/share/cmake-3.5/Modules/FindPackageHandleStandardArgs.cmake:388 (_FPHSA_FAILURE_MESSAGE)
  iiwa_ros/iiwa_driver/cmake/FindFRI.cmake:67 (find_package_handle_standard_args)
  iiwa_ros/iiwa_driver/CMakeLists.txt:44 (find_package)


-- Configuring incomplete, errors occurred!
See also "/home/robotics/kuka_ws/build/CMakeFiles/CMakeOutput.log".
See also "/home/robotics/kuka_ws/build/CMakeFiles/CMakeError.log".

My guess is that I have to add the FRI package provided by KUKA somewhere in my workspace. Can you help me with this please?

Thanks,
Pranav

Publisher Node for Position Control

Hello !
I have been successful in establishing FRI using ROS. I am able to read joint_states and rostopic list gives me the topic /iiwa/TorqueController/command. I am trying to write a publisher node for this topic using python to be able to move the robot.
Following is the code-

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64MultiArray

rospy.init_node('test_node')
pub=rospy.Publisher("/iiwa/TorqueController/command", Float64MultiArray, queue_size=10)

msg=Float64MultiArray()
msg.data = [0]*7

rate = rospy.Rate(10)
while not rospy.is_shutdown():
    pub.publish(msg)
    print("publishing")
    rate.sleep()

The robot does not move and the terminal output shows

publishing
publishing
publishing

I guess something is wrong with the message definition?

ROS has been disconnected

Hi,

I am using a torque controller for my control research. iiwa always disconnects with PC without any warning or error. The smartPad only show 'ROS has been disconnected'.
ROSDISCONNECT
However, when I switch to another application on smartPad and run it, I get this error

ROSDISCONNECT1

Does anyone have clue about the cause of my problem? Thank you!

how to fix the FK gap between urdf model and real robot?

Hi,
With the help of your work , I can communicate iiwa7 with FRI.

As we know, Fri only support joint relate information.

One way to get end effector pos is doing a forward kinematics (FK) with URDF model.

However, the URDF model in this ros package is a idea model and is different from the real iwwa7 model, which has been calibrated and more accurate.

As a result, the value solved by iiwa_tools is different from it was solved by real iwwa7.

For example ,
the FK x, y, z result solved by iiwa_tools is 0.2270, 0.3588, 0.6772,
the FK x, y, z result solved by real iiwa is 0.23197, 0.36651, 0.65290.

So I am wondering how you fix the FK gap between iiwa_tools and real iiwa?

Best,
William

Control 7th DOF in Position Control Mode

Hi,

thanks for developing the driver, it works smoothly in our lab for the LBR Med!

I was wondering in there is a way to specify the elbow position in your calculation of the IK. Could you comment on that?

Best,
Martin

torque information

  • As you mentioned in #18 τ = τ_gc + τ_msg , and you implemented gravity compensation here
    if (_iiwa_gravity_client.call(_gravity_srv)) {
       for (size_t i = 0; i < n_dof_; i++) {
           C[i] = _gravity_srv.response.compensation_torques[i];
       }
   }

I read your code, in iiwa_tool package, you computed the gravity compensation part by

    rbd::forwardKinematics(rbdyn_urdf.mb, rbdyn_urdf.mbc);
    rbd::forwardVelocity(rbdyn_urdf.mb, rbdyn_urdf.mbc);
    fd.computeC(rbdyn_urdf.mb, rbdyn_urdf.mbc);
   // Get gravity and Coriolis forces
    return -fd.C();

but the function fd.C return the non linear effect vector (coriolis, gravity, external force), not just gravity.

  • Also, we send the torque message by the topic of /iiwa/TorqueController/command, the torque is the desired torque which needs some time to achieve or immediate response?

  • Can you provide a test example, such as tracking a planned trajectory, to test the torque controller work correctly?

[iiwa/iiwa_service-2] process has died

I successfully connect with the robot via FRI.

When I run 'roslaunch iiwa_driver iiwa_bringup.launch', I met this error

But when I run 'roslaunch iiwa_driver iiwa_bringup.launch'. I met this error.
22

And when I run 'roslaunch iiwa_gazebo iiwa_gazebo.launch', I met this error.
33

Maybe I haven't installed the dependencies correctly. Could you please help me?

IP configuration and connection issues

Hallo

I have the following questions regarding the IP config:

  1. I am not able to ping the robot with IPv4 and Robot IP as mentioned here.
  2. Do we need to change the IP here to Linux /ROS Master
  3. I connected a monitor to KONI and checked the IPv4 configuration of two adopters first one 172.31.1.147 (Here I connect my Sunrise to sync the and load the app, etc) and second IPv4 config 192.168.0.2. In my guess, the second adopter is KONI IPv4 config.
  4. Also, I am confused about whether to change the KONI IP in setup.cat. Currently, it is set to 192.168.0.7

Thank you

What is the difference between Position/Torque mode in FRIOverlay?

I see in the FRIOverlay code that if you select torque control mode, it will zero out all the damping terms. Is that the only difference when I select torque mode on pendant instead of position mode? Because when I bring up the driver on my PC, I can select between the position and torque controller for ROS control and that will offer a corresponding command topic. I'm just wondering if there is any other change induced from the FRIOverlay on the KUKA side other than zeroing out damping terms in torque mode that I'm missing.

Thanks!

err in communication between gazebo and moveit

roslaunch iiwa_gazebo iiwa_gazebo.launch
roslaunch iiwa_moveit demo.launch
when I press plan and execute in moveit,

[ERROR] [1593402180.815820616, 72.486000000]: Action client not connected: PositionTrajectoryController/follow_joint_trajectory

[ERROR] [1593404460.733921626, 2339.063000000]: Unable to identify any set of controllers that can actuate the specified joints: [ iiwa_joint_1 iiwa_joint_2 iiwa_joint_3 iiwa_joint_4 iiwa_joint_5 iiwa_joint_6 iiwa_joint_7 ]

what is here source/directory

I am new to ROS, I have trouble installing all the packages. I am sure there must be something wrong because I can't communicate with the robot. May I ask, what does source/directory mean?
image

Should the structure look like this?
image
image

Motion paused during PositionControl

Hi, all

I am using PositionController from repo. I wrote a python code to let robot move from joint angle [0, 0, 0, 0, 0, 0, 0] to joint angle [0.5, 0.2, 0.5, 0.3, 0.5, -0.5, 0.5]. My code works fine on Gazebo. But when I tried to run same code on iiwa, robot paused. The smart pad shows me:
image
Why does this happen and how do I solve it or avoid it?

Thank you

How to use REALLY Pure Postion mode with FRI?

Hi,

Thank you for your great job !

I had already use your driver to move my iiwa7 .

Now, I can use moveit in Rivz to drag the simulate robot with PositionTratectoryController and the real iiwa can follow the drag trajectory.

The question is :
the robot is not actually in Pure Postion Mode, even the stiffness is set to high value, and the robot was not accurate acchieve the goal which effected by the gravity.

the JAVA code in robot side is

        // configure and start FRI session
        FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName);
        // for torque mode, there has to be a command value at least all 5ms
        friConfiguration.setSendPeriodMilliSec(5);
        friConfiguration.setReceiveMultiplier(1);

        getLogger().info("Creating FRI connection to " + friConfiguration.getHostName());
        getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |"
                + " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier());

        FRISession friSession = new FRISession(friConfiguration);

        // wait until FRI session is ready to switch to command mode
        try
        {
            friSession.await(10, TimeUnit.SECONDS);
        }
        catch (final TimeoutException e)
        {
            getLogger().error(e.getLocalizedMessage());
            friSession.close();
            return;
        }

        getLogger().info("FRI connection established.");

        ClientCommandMode mode = ClientCommandMode.POSITION;
        FRIJointOverlay jointOverlay = new FRIJointOverlay(friSession, mode);
        double stiffness = 500.;

        // start PositionHold with overlay
        JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(stiffness, stiffness, stiffness, stiffness, stiffness, stiffness, stiffness);

        try {
            PositionHold posHold = new PositionHold(ctrMode, -1, TimeUnit.SECONDS);
            getLogger().info("Robot is ready for ROS control.");
            _lbr.move(posHold.addMotionOverlay(jointOverlay));
        }
        catch(final CommandInvalidException e) {
            getLogger().error("ROS has been disconnected.");
        }

        // done
        friSession.close();

How can I just use pure postion mode to do position trajectory task?

Best
William

control iiwa 14 with torquecontrol mode

I try to control the real robot with the torquecontrol mode.When I public the message through the command line:`rostopic pub -1 /iiwa/PositionController/command std_msgs/Float64MultiArray "layout:
dim:

  • label: ''
    size: 0
    stride: 0
    data_offset: 0
    data:
  • -0.8 -6.1 0. 5.8 -0.3 -0.3 -0.1`
    It doesn't move and the terminal shows :
    [WARN] [1573559821.210509]: Inbound TCP/IP connection failed: <class 'struct.error'>: 'required argument is not a float' when writing 'label: ''
    size: 0
    stride: 0'
    I don't know why.What's more,is there any different with the input data:-0.8 -6.1 0. 5.8 -0.3 -0.3 -0.1 and the Joint Torques data show on the smartPAD?

Can you provide an example to control the real robot with the torquecontrol mode.I will apprivate it !

Using iiwa_tools requires to compile with SIMD

I am currently exploring to use iiwa_tools for FK and the Jacobian in my code.

I always had crashes upon loading the controller in Gazebo when it parses the urdf to feed it to RBDyn.
It finally turned out, that the issue was, that I did not compile my new controller code with SIMD turned on.

I guess it would be possible to compile all the dependencies as well as iiwa_tools without SIMD and that would work as well.
But since it's the default and not directly intuitive that it is needed for derived code as well, I will leave this here as a note.

How to get FRI library?

I'm trying to use FRI to connect our iiwa robots. But I can't connect my robot successfully.
It always in the monitoring state, like robot never get any message from my PC.
I would like to reference your code.
Would you mind sharing your private library code for me?
Thank you!

Connection issues, timeout before FRI connection quality reached good or excellent.

I set up the robot and met this problem on the smart pad.

timeout before FRI connection quality reached good or excellent.
c6c5e2a2aea83654ad6b72e6210cb7a
And this is the screenshot from the UBUNTU.
01
02
03

I tried the solution, which is provided by 921218316.
#17

But it still doesn't work, then I tried this solution, it still didn't work.
#48

Could you please help me? I assume it is because I am using ubuntu virtual machine on the same computer of the windows laptop. Should I use two separate computers?

compile issus

  • I use a new computer and do as your repo many times, but there is something wrong when I compile the code, and the terminator output as folloing:

2020-07-25 11-55-29屏幕截图
2020-07-25 11-55-44屏幕截图

  • When I startup gazebo roslaunch iiwa_gazebo iiwa_gazebo ,terminator outputs:
    2020-07-25 12-04-36屏幕截图
    how to fix it?

Compilation with iiwa_service

I have already install all the dependencies that the project needs, and when I "catkin_make" my ROS workspace, I have the error with iiwa_service, like this:

image

How can I figure out this problem? What happened to this? Could anyone give me a hand?

How to verify the torque controller is correct

There are two controller: PositionController and TorqueController. The PositionController is simple to understand, but how to verify the torque controller is correct? Is there any method in gazebo simulation environment?

make -j RBDyn

Hi,
When make -j is launched the PC blocks in a certain percentage of compilation and doesn't work
regards

Dynamic equation of the iiwa14 robot

The general dynamic equation of the robot is M(\Theta )\ddot{\Theta }+C(\Theta,\dot{\Theta })+G(\dot{\Theta })=\tau . In order to simulate the iiwa14 robot in gazebo or conrol the robot, we need to establish the dynamic equation. So, in which source file is the dynamic equation of robot expressed explicitly ? If there isn't any, how the dynamic equation is established and control the robot in your code?

Question for dynamic parameters of the robot

Hello,

This question is about the correctness of the inertia parameters of the kuka robot. I know that there was a similar discussion, and the correct/official inertia values from kuka is not known. However, I would like to know if the inertia values from the urdf is accurate enough to perform torque-based control, or should I anticipate any trouble if I use the inertia values from the urdf?

Thanks a lot.

Build kuka_fri with fPIC

I want to build kuka_fri with the -fPIC flag in order to use this code in the Orocos framework (seems it's necessary otherwise the linker complains on building). I saw there was a --shared option for the waf script so I tried that, but passing that option gives me

Waf: Entering directory `/home/ll4ma/source_code/iiwa_dependencies/kuka_fri/build'
[ 1/14] Compiling src/kuka/fri/LBRClient.cpp
[ 2/14] Compiling src/kuka/fri/MonitoringMessageDecoder.cpp
[ 3/14] Compiling src/kuka/fri/UdpConnection.cpp
[ 4/14] Compiling src/kuka/fri/CommandMessageEncoder.cpp
[ 5/14] Compiling src/kuka/fri/LBRCommand.cpp
[ 6/14] Compiling src/kuka/pb/pb_decode.c
[ 7/14] Compiling src/kuka/fri/FRIMessages.pb.c
[ 8/14] Compiling src/kuka/pb/pb_frimessages_callbacks.c
[ 9/14] Compiling src/kuka/fri/ClientApplication.cpp
[10/14] Compiling src/kuka/pb/pb_encode.c
[11/14] Compiling src/kuka/fri/LBRState.cpp
[12/14] Compiling src/example.cpp
[13/14] Linking build/libkuka_fri.so
/usr/bin/ld: src/kuka/pb/pb_encode.c.1.o: relocation R_X86_64_PC32 against symbol `pb_encode_tag' can not be used when making a shared object; recompile with -fPIC
/usr/bin/ld: final link failed: Bad value
collect2: error: ld returned 1 exit status

I tried manually adding the -fPIC flag in the configure function in wscript but I can't seem to be able to get around this error. Any ideas? I'm not at all familiar with waf so I'm a little clueless here. I might just write a Makefile unless there's a reason why cmake isn't being used to build this package.

safety error in safety configuration of sunrise workbench and in smartpad

I am following the tutorials to use the KUKA iiwa package.

  1. In the 'Sunrise Robot Application upload' section, I need to set the safety configurations. In my sunrise, I can only set 3 axises not 7 axises.
    image

  2. When I modify my sunrise file without the safety configuration, I synchronize the project to smartPAD. In my smart pad, the light for the safety is always red.
    I met the error
    Safety control
    position sensor not referenced(K1:A1,A2,A3,A4,A5,A6,A7).
    Emergency stop external is active.
    protective stop is activated

    3505bf3bcd8ba3e9aec788fc5165c47

access to kuka fri repository

I have a Kuka iiwa14. On the website you mentioned to contact you for getting access to this repository. How does that work? Let me know!

Thanks

catkin_make problem

Hi; I have a problem on compiling my workspace with ROS Melodic
[ 95%] Linking CXX executable /home/akliisma/catkin_ws/devel/lib/iiwa_driver/iiwa_driver
CMakeFiles/iiwa_service.dir/src/iiwa_service.cpp.o : Dans la fonction « mc_rbdyn_urdf::URDFParserResult::URDFParserResult() » :
iiwa_service.cpp:(.text._ZN13mc_rbdyn_urdf16URDFParserResultC2Ev[_ZN13mc_rbdyn_urdf16URDFParserResultC5Ev]+0x24) : référence indéfinie vers « rbd::MultiBody::MultiBody() »
iiwa_service.cpp:(.text._ZN13mc_rbdyn_urdf16URDFParserResultC2Ev[_ZN13mc_rbdyn_urdf16URDFParserResultC5Ev]+0x73) : référence indéfinie vers « rbd::MultiBodyGraph::MultiBodyGraph(std::__cxx11::basic_string<char, std::char_traits, std::allocator >) »
CMakeFiles/iiwa_service.dir/src/iiwa_service.cpp.o : Dans la fonction « mc_rbdyn_urdf::URDFParserResult::~URDFParserResult() » :
iiwa_service.cpp:(.text._ZN13mc_rbdyn_urdf16URDFParserResultD2Ev[_ZN13mc_rbdyn_urdf16URDFParserResultD5Ev]+0x50) : référence indéfinie vers « rbd::MultiBodyGraph::~MultiBodyGraph() »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::MultiBodyGraph::MultiBodyGraph(rbd::MultiBodyGraph const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::ForwardDynamics::computeC(rbd::MultiBody const&, rbd::MultiBodyConfig const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::forwardVelocity(rbd::MultiBody const&, rbd::MultiBodyConfig&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::MultiBodyConfig::zero(rbd::MultiBody const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::Jacobian::jacobianDot(rbd::MultiBody const&, rbd::MultiBodyConfig const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::Jacobian::Jacobian(rbd::MultiBody const&, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::MultiBodyGraph::operator=(rbd::MultiBodyGraph const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::InverseKinematics::inverseKinematics(rbd::MultiBody const&, rbd::MultiBodyConfig&, sva::PTransform const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::Jacobian::jacobian(rbd::MultiBody const&, rbd::MultiBodyConfig const&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::InverseKinematics::InverseKinematics(rbd::MultiBody const&, int) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « mc_rbdyn_urdf::rbdyn_from_urdf(std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, bool, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, bool, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, bool, std::__cxx11::basic_string<char, std::char_traits, std::allocator >) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::forwardKinematics(rbd::MultiBody const&, rbd::MultiBodyConfig&) »
/home/akliisma/catkin_ws/devel/lib/libiiwa_tools.so : référence indéfinie vers « rbd::ForwardDynamics::ForwardDynamics(rbd::MultiBody const&) »
collect2: error: ld returned 1 exit status
iiwa_ros/iiwa_tools/CMakeFiles/iiwa_service.dir/build.make:157: recipe for target '/home/akliisma/catkin_ws/devel/lib/iiwa_tools/iiwa_service' failed
make[2]: *** [/home/akliisma/catkin_ws/devel/lib/iiwa_tools/iiwa_service] Error 1
CMakeFiles/Makefile2:11349: recipe for target 'iiwa_ros/iiwa_tools/CMakeFiles/iiwa_service.dir/all' failed
make[1]: *** [iiwa_ros/iiwa_tools/CMakeFiles/iiwa_service.dir/all] Error 2
make[1]: *** Attente des tâches non terminées....
[ 95%] Built target iiwa_driver
[ 95%] Built target CustomEffortController
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

Thanks

P.S: sorry, the comments are in french

Question about Sunrise Robot Application upload

I am a beginner of using iiwa robot.

In repo, It mentioned: "Create a new Sunrise project with Sunrise Workbench". Where do I create this Sunrise project? On "a Windows laptop is connected on the X66 Ethernet port" or I can directly create on KUKA Sunrise Cabinet?

If I have to use a laptop, how do I get Sunrise Workbench?

Many thanks

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.