How to use low pass filter in your case?

[Chunting]

Hi,
Since there is so much vibration in my application, I want to use your low_pass_filter to reduce it,but I encounter some problems.
According to your example in kuka_driver.cc, I applied it into my project. This is the pseudo-code:

cutoff_hz = 250;
q_diff = currentJointPosition - previousJointPosition;

robot_dt = utime_now - utime_last_;  // 0.004s

joint_velocity_estimated = vel_filters_[i].filter(q_diff/robot_dt)

// kTimeStep =0.004s
diffToGoal = joint_velocity_estimated*kTimeStep;

utime_last_ = utime_now;

commandToSend = currentJointPosition + diffToGoal;

commandToSendPlusIPOJointPos = commandToSend + ipoJointPosition;

// Then send commandToSendPlusIPOJointPos to robot in every kTimeStep.

Is this procedure OK?
It seems strange to me that robot couldn't follow the path I planed.
I'm wondering if it is caused by diffToGoal = joint_velocity_estimated*kTimeStep, since I didn't find anything about how to integrate joint_velocity_estimated to commandPosition.
Would you please give me some suggestions or instructions?

Thanks,
Chunting

[siyuanfeng-tri]

there are a couple questions for you:
are you using a kuka iiwa arm?
robot_dt = utime_now - utime_last_ is this in seconds or micro seconds?

I don't quite understand what you are doing here. Isn't commandToSend coming directly from your desired trajectory, why does it have anything to do with measured velocity?
Also, we don't using the IPO commands, so I am not that familiar with what that does.

[Chunting]

Hi @siyuanfeng-tri ,
Yes, I'm using kuka iiwa LBR 820, and the Sunrise Workbench version is 1.11.
In my project, the driver has two levels.
The first one is to generate the goal position in every 50ms (In fact, the period is based on the update period of VREP Plugin, the range is 40-60 ms). The second one is to apply the interpolation algorithm according to the KUKA control period (4ms).

I'm trying to apply the low pass filter in the first level, and then pass the joint_velocity_estimated to the lower level.

The pseudo-code is different from the previous one above but like this:

cutoff_hz = 50;
FirstTimeStep = 0.05;                      //  The first level control period, in seconds

vel_filters_.resize(KUKA::LBRState::NUM_DOF, DiscreteTimeLowPassFilter<double>(cutoff_hz, FirstTimeStep));

robot_dt = utime_now - utime_last_;       // around 0.05 s

q_diff = currentJointPosition - previousJointPosition;

joint_velocity_estimated = vel_filters_[i].filter(q_diff/robot_dt)

utime_last_ = utime_now;

/// Pass the joint_velocity_estimated to the second level driver

secondTimeStep = 0.004 ;                // The KUKA control period, in seconds
diffToGoal = joint_velocity_estimated*secondTimeStep;

commandToSend = currentJointPosition + diffToGoal;

commandToSendPlusIPOJointPos = commandToSend + ipoJointPosition;

In #issue14, @sammy-tri mentioned that it had been working well for you, but in my case, the robot can't track the path I calculated in the first level driver(based on the inverse kinematics), so I'm wondering if it's because I didn't calculate the joint_velocity_estimated appropriately ? or I didn't pick up the appropriate numerical integration algorithm?

Also could you please tell me how to determine the cutoff_hz? is there any relationship between cutoff_hz and control frequency?

Thanks for your reply, and I appreciate your help.
Chunting

[siyuanfeng-tri]

I dont know what VREP is, and your desired position command seems odd to me, if I read it correctly, you are telling the robot to maintain the current measured joint velocity for the next 4ms.

Here is the bottom line. If your desired trajectory is computed independent of any real measured state, your kuka should be able to follow it well hassle free. On the other hand, if your desired is computed with any feedback signals in it, your robot shaking itself to death is probably coming from delays. Basically, the state you are getting on your computer side is X ms behind the real state, and the command kuka's controller receives is another Y ms behind your computer clock. I have yet to convince myself what's the magnitude of the delay. I think it's on the order of 40ms. Friends at harvard thinks it's around 5ms.