Hello, I wrote an impedance controller for my 6 DOF robot using based in EffortJointInterface and I'm strugglin while simulating it in gazebo. So far because of the sake keeping simplify I'm trying to just compensate gravity but the robot behaviour during the simulation is always the same, to go to the 0 position for every joint acting as spring, instead of just keeping the iniital position as it should be. Here my code for the controller, I know some term are based in personal classes but I will try to explain the problem:
This very same code (the mathematics I mean) has been tested in a real robot and its working so I was trying to simulate the robot now, but as I said the robot behavior is wrong. I'm just trying to compensate the gravity term through the tau = gMat;. So far I checked this:
initial position is ok
g term values are ok
position and velocity lectures are ok.
kp and kd initialization are ok
error calculations are ok
I printed this values while running the simulation in gazebo and for the first iteration seems to be ok, though the robot is always going upwards to the 0 position for every joint with a "spring" behavior (seems to be like a bad tuned PD).
As I checked my mathematics and tested in a real robot, I dont know what can it and I'm honestly lost here. my yaml file is the following, the idea is to set Kp and Kd gains and apply the m to the impedance matrixes, but as I'm trying to just compensate gravity I dont need them yet.
Hello, I wrote an impedance controller for my 6 DOF robot using based in EffortJointInterface and I'm strugglin while simulating it in gazebo. So far because of the sake keeping simplify I'm trying to just compensate gravity but the robot behaviour during the simulation is always the same, to go to the 0 position for every joint acting as spring, instead of just keeping the iniital position as it should be. Here my code for the controller, I know some term are based in personal classes but I will try to explain the problem:
This very same code (the mathematics I mean) has been tested in a real robot and its working so I was trying to simulate the robot now, but as I said the robot behavior is wrong. I'm just trying to compensate the gravity term through the
tau = gMat;
. So far I checked this:I printed this values while running the simulation in gazebo and for the first iteration seems to be ok, though the robot is always going upwards to the 0 position for every joint with a "spring" behavior (seems to be like a bad tuned PD). As I checked my mathematics and tested in a real robot, I dont know what can it and I'm honestly lost here. my yaml file is the following, the idea is to set Kp and Kd gains and apply the m to the impedance matrixes, but as I'm trying to just compensate gravity I dont need them yet.