How to start Hand handGuiding mode?

[GuancongLuo]

Hi jack

Thank you for your help about #198.

Recently, I want to try the IIWA handGuiding mode by ROS. But I have no idea how to do it.

I see the IFL-CAMP/iiwa_stack ROSBaseApplication.java have the fakeHandGuidanceMode. And I search jacknlliu/iiwa_stack repo, but not find the handGuiding mode.

And I see you had want to finish that.

In this mode, we will send current position continuously.

Could you get me some advice?

[jacknlliu]

You can switch the control mode to joint impedance mode, and set the joint impedance parameters as all joint_stiffness=0.0 and all joint_damping=0.7.

Please see here.

[GuancongLuo]

Thank you for your reply.

As you said, I try it and success run in gazebo.
[ INFO] [1570524580.324489094]: SmartServo Service successfully called.
But it is fail in real robot and I get the follow errors.
[ERROR] [1570522664.702721411]: Config failed, Java error: java.lang.IllegalStateException: Load Mass Validation failed variance[0] of LBRExtTorqueObs@LBR_iiwa_14_R820_1 is 17.415440967754687 should be less than 10

I guess IIWA Load Mass Validation from Sunrise. But I didn't find it. I have no idea how to change the Mass Validation.

This is my test code. Is it right?

#include "ros/ros.h"
#include "std_msgs/String.h"

#include "iiwa_msgs/ControlMode.h"
#include "iiwa_msgs/JointImpedanceControlMode.h"
#include "iiwa_msgs/ConfigureControlMode.h"

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

ros::NodeHandle n;

ros::ServiceClient client = n.serviceClient<iiwa_msgs::ConfigureControlMode>("/iiwa/configuration/ConfigureControlMode");

iiwa_msgs::ConfigureControlMode config;

config.request.control_mode = iiwa_msgs::ControlMode::JOINT_IMPEDANCE; //iiwa_msgs/CartesianImpedanceControlMode cartesian_impedance


iiwa_msgs::JointImpedanceControlMode handmodel;

handmodel.joint_stiffness.a1=0;
handmodel.joint_stiffness.a2=0;
handmodel.joint_stiffness.a3=0;
handmodel.joint_stiffness.a4=0;
handmodel.joint_stiffness.a5=0;
handmodel.joint_stiffness.a6=0;
handmodel.joint_stiffness.a7=0;

handmodel.joint_damping.a1=0.7;
handmodel.joint_damping.a2=0.7;
handmodel.joint_damping.a3=0.7;
handmodel.joint_damping.a4=0.7;
handmodel.joint_damping.a5=0.7;
handmodel.joint_damping.a6=0.7;
handmodel.joint_damping.a7=0.7;


if (client.call(config))
{
    if (!config.response.success)
    {
        ROS_ERROR_STREAM("Config failed, Java error: " << config.response.error);
    }
    else
    {
        ROS_INFO_STREAM("SmartServo Service successfully called.");
    }
}
else
{
    ROS_ERROR_STREAM("Config failed - service could not be called - QUITTING NOW !");
}

}

[jacknlliu]

please try it with a non-singularity pose

[GuancongLuo]

请尝试使用非奇异姿势

Thank you for your reply.

When iiwa in the non-singularity pose, I can use the handGuding mode.

But this mode performance is bad. When I start this mode, the joint 5 will rapid run and lock in limit angle ever not anything tool in iiwa arm. And when the hand stop guiding it, it also continues move.

I think this is the problem configure caused.
IFL-CAMP#228 (comment)
https://github.com/IFL-CAMP/iiwa_stack/wiki/attachtool

This two answer is so simple for me that I don't know how to change configure in sunrise.

I want to get better gravity compensation performance.
Could you give me some advice?

[jacknlliu]

When I start this mode, the joint 5 will rapid run and lock in limit angle ever not anything tool in iiwa arm.

just add the damping value for joint 5, and hold the joint 5 with your hand if possible.

And when the hand stop guiding it, it also continues move.

Before stop guiding, please switch the mode to position mode.