Implement integral gain

[Fabian Gerlinghaus]

in the OP point task

[Samir Menon]

Implemented for gc controllers as of acbad472ffeba3f3cde32ef6ad1b667dc9023ba0

[Fabian Gerlinghaus]

Great work Samir! Thanks!

Are you also going to implement it for the OP Task Controller?

[Samir Menon]

Done. b8e57ea21f2b8faaad43d4bd33454e8db5b472f3

Still needs a few updates to be compatible with the scl_eg_ctrl app.

[Samir Menon]

Fixed as of : 832a8db5e01c20a43a9972bbf715e3d38d08c83b

Try it using:

$./scl_eg_ctrl ../../specs/Puma/PumaCfg.xml PumaBot opcpida -op handpida

[Samir Menon]

Fixed.

[Fabian Gerlinghaus]

Hey Samir,

I just tried to use your PIDA controller in my code. The program worked fine before. Now it crashes once it reaches the console without giving an error.

I changed my KukaCfg.xml in the same way you did. And then I'm calling: sudo ./kuka_scl_controller kuka_specs/KukaCfg.xml KukaBot opcpida -op EndEffectorPositionPIDA

Do I need to change anything in my code?

Here's the ConfigFile for the KUKA. I'm posting it here, because it's not tracked by GIT:

<?xml version="1.0"?>
<!DOCTYPE LOTUS SYSTEM "scl.dtd">

<!-- Units are in Std International System -->
<scl>
    <robot name="KukaBot"> 
        <spec>Kuka</spec>
        <gravity>0.000   0.000   -9.81</gravity> 
        <file>kuka_specs/Kuka.xml</file>
        <root_link>
            <link_name>base</link_name>
            <position_in_parent>-0.10   -0.100   -0.85000</position_in_parent> 
            <orientation_in_parent>0.000000 0.000000 0.000000 1.000000</orientation_in_parent>
            <graphics>
               <obj_file>
                    <name>kuka_specs/graphics/kuka-swiwel.obj</name>
                    <position_in_parent>0.000000 0.000000 0.000000</position_in_parent>
                    <orientation_in_parent>0.707106 0.00000 0.000000 0.707106</orientation_in_parent>
                    <collision_type>0</collision_type>
                    <scaling>0.001 0.001 0.001</scaling>
                </obj_file>
            </graphics>
        </root_link>

        <!-- These flags can control the simulation's default behavior. ADDED BY FAGE-->
        <flag_gc_damping>false</flag_gc_damping>
        <flag_gc_limits>false</flag_gc_limits> <!-- Apply gc position limits (joint limits) -->
        <flag_actuator_force_limits>true</flag_actuator_force_limits>
        <flag_actuator_vel_limits>true</flag_actuator_vel_limits>
        <flag_actuator_acc_limits>true</flag_actuator_acc_limits>
        <flag_controller_on>true</flag_controller_on> <!-- Controller enabled at the start -->
        <flag_logging_on>false</flag_logging_on>
        <flag_wireframe_on>false</flag_wireframe_on>
        <option_axis_frame_size>0.0</option_axis_frame_size> <!-- All options are optional -->
    </robot>

    <graphics name="KukaBotStdView">
        <camera>
            <pos>2.0   0.0   -0.975</pos>
            <lookat>0.0   0.0   0.0</lookat>
            <up>0   0   1</up>
            <clip>0 10</clip>
            <background>0 0 0</background>
        </camera>
        <light>
            <pos>2.0   0.0   0.0</pos>
            <lookat>0.0   0.0   0.5</lookat>
        </light>
        <light>
            <pos>-2.0   0.0   0.0</pos>
            <lookat>0.0   0.0   0.0</lookat>
        </light>
        <background_color>0.0 0.0 0.0</background_color>
    </graphics>

    <controller name="KukaGcCtrl">
        <type>gc</type>
        <must_use_robot>KukaBot</must_use_robot>
        <kp>100</kp>
        <kv>10</kv>
        <ka>0</ka>
        <ki>0</ki>
        <force_max>10</force_max>
        <force_min>-10</force_min>
    </controller>

    <controller name="opcpida">
        <type>task</type>
        <must_use_robot>KukaBot</must_use_robot>

        <task name="EndEffectorPositionPIDA">
        <type>TaskOpPosPIDA1OrderInfTime</type>
            <parent_link>end-effector</parent_link>
            <pos_in_parent>0.001 0.001 0.05</pos_in_parent>
            <priority>0</priority>
            <task_dof>3</task_dof>
        <kp>80</kp>
            <kv>5</kv>
            <ka>0</ka>
            <ki>10</ki>
            <integral_time_constt>0.1</integral_time_constt>
            <integral_time_max>0.05</integral_time_max>
            <force_max>40</force_max>
            <force_min>-40</force_min>
        </task>

        <task name="GcTaskPIDA">
            <type>TaskGc</type>
            <priority>1</priority>
            <task_dof>0</task_dof> 
            <kp>5</kp>
            <kv>4</kv>
            <ka>0</ka>
            <ki>0</ki>
            <force_max>100</force_max>
            <force_min>-100</force_min>
        </task>

        <!-- 0 task_dof means a gc task, ie full dofs -->
        <task name="NullSpaceDampingTaskPIDA">
            <type>TaskNullSpaceDamping</type>
            <priority>1</priority>
            <task_dof>0</task_dof> 
            <kp>0</kp>
            <!-- <kv>4</kv> ROBERT'S ORIGINAL VALUE -->
        <kv>20</kv>
            <ka>0</ka>
            <ki>0</ki>
            <force_max>100</force_max>
            <force_min>-100</force_min>
        </task>
    </controller>
</scl>