Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/robots/robot_interfaces/tests/test_process_action.cpp	Lines:	122	126	96.8 %
Date:	2020-04-15 11:50:02	Branches:	312	920	33.9 %


/**
 * @file
 * @brief Test for processing of robot actions
 * @copyright Copyright (c) 2019, New York University and Max Planck
 *            Gesellschaft.
 */
#include <gtest/gtest.h>
#include "robot_interfaces/n_joint_robot_functions.hpp"

/**
 * @brief Fixture for the tests of process_desired_action().
 */
class TestProcessDesiredAction : public ::testing::Test
{
protected:
    using Functions = robot_interfaces::NJointRobotFunctions<2>;
    using Types = Functions::Types;
    using Vector = Types::Vector;

    Types::Observation observation;
    double max_torque_Nm;
    Vector safety_kd;
    Vector default_position_control_kp;
    Vector default_position_control_kd;

    void SetUp() override
    {
        // set some arbitrary default values.  Tests can overwrite specific
        // values if needed.
        observation.position << 12.34, -0.42;
        observation.velocity << .4, -.2;
        observation.torque << 0.3, -0.25;

        max_torque_Nm = 0.3;
        safety_kd << 0.1, 0.1;

        default_position_control_kp << 3, 4;
        default_position_control_kd << .3, .4;
    }
};

/**
 * @brief Test if a valid torque command is returned unchanged (no safety).
 */
TEST_F(TestProcessDesiredAction, valid_torque_no_safety)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    Vector desired_torque;
    desired_torque << 0.1, 0.2;
    Types::Action action = Types::Action::Torque(desired_torque);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    ASSERT_EQ(desired_torque[0], resulting_action.torque[0]);
    ASSERT_EQ(desired_torque[1], resulting_action.torque[1]);
}

/**
 * @brief Test if clamping to max. torque is working.
 */
TEST_F(TestProcessDesiredAction, exceed_max_torque_no_safety)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    Vector desired_torque;
    desired_torque << 0.5, -1.2;
    Types::Action action = Types::Action::Torque(desired_torque);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    ASSERT_EQ(max_torque_Nm, resulting_action.torque[0]);
    ASSERT_EQ(-max_torque_Nm, resulting_action.torque[1]);
}

/**
 * @brief Test velocity damping with low velocity
 */
TEST_F(TestProcessDesiredAction, velocity_damping_low_velocity)
{
    Vector desired_torque;
    desired_torque << 0.1, 0.2;
    Types::Action action = Types::Action::Torque(desired_torque);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    // joint 0 has positive velocity so expecting a reduced torque.
    // joint 1 has negative velocity so expecting an increased torque.
    // both should remain within the max. torque range.
    ASSERT_LT(resulting_action.torque[0], desired_torque[0]);
    ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
    ASSERT_GT(resulting_action.torque[1], desired_torque[1]);
    ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
}

/**
 * @brief Test velocity damping with very high velocity.
 *
 * This test ensures that torques cannot exceed the allowed max. torque due to
 * the velocity damping.
 */
TEST_F(TestProcessDesiredAction, velocity_damping_high_velocity)
{
    // set very high velocity
    observation.velocity << 4000, -2000;

    Vector desired_torque;
    desired_torque << 0.1, 0.2;
    Types::Action action = Types::Action::Torque(desired_torque);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    // joint 0 has positive velocity so expecting a reduced torque.
    // joint 1 has negative velocity so expecting an increased torque.
    // both should remain within the max. torque range.
    ASSERT_LT(resulting_action.torque[0], desired_torque[0]);
    ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
    ASSERT_GT(resulting_action.torque[1], desired_torque[1]);
    ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
}

/**
 * @brief Test basic position controller (default gains, no velocity)
 */
TEST_F(TestProcessDesiredAction, position_controller_basic)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    observation.position << 0, 0;
    observation.velocity << 0, 0;

    Vector desired_position;
    desired_position << -1, 1;
    Types::Action action = Types::Action::Position(desired_position);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    // Just testing here if the torque command goes in the right direction

    ASSERT_LT(resulting_action.torque[0], 0.0);
    ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);

    ASSERT_GT(resulting_action.torque[1], 0.0);
    ASSERT_LE(resulting_action.torque[1], max_torque_Nm);

    // verify that position and gains are set correctly in the returned action
    ASSERT_EQ(desired_position[0], resulting_action.position[0]);
    ASSERT_EQ(desired_position[1], resulting_action.position[1]);
    ASSERT_EQ(default_position_control_kp[0], resulting_action.position_kp[0]);
    ASSERT_EQ(default_position_control_kp[1], resulting_action.position_kp[1]);
    ASSERT_EQ(default_position_control_kd[0], resulting_action.position_kd[0]);
    ASSERT_EQ(default_position_control_kd[1], resulting_action.position_kd[1]);
}

/**
 * @brief Test position controller for only one joint.
 */
TEST_F(TestProcessDesiredAction, position_controller_one_joint_only)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    observation.position << 0, 0;
    observation.velocity << 0, 0;

    Vector desired_position;
    desired_position << -1, std::numeric_limits<double>::quiet_NaN();
    Types::Action action = Types::Action::Position(desired_position);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    // Just testing here if the torque command goes in the right direction
    ASSERT_LT(resulting_action.torque[0], 0.0);
    ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);

    // desired position for joint 1 is nan, so no torque should be generated
    ASSERT_EQ(0.0, resulting_action.torque[1]);
}

/**
 * @brief Test position control with velocity (i.e. verifying D-gain is working)
 */
TEST_F(TestProcessDesiredAction, position_controller_with_velocity)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    observation.position << 0, 0;
    observation.velocity << 0, 0;

    Vector desired_position;
    // use low position change to not saturate torques
    desired_position << -.01, .01;

    Types::Action result_action_without_velocity =
        Functions::process_desired_action(
            Types::Action::Position(desired_position),
            observation,
            max_torque_Nm,
            safety_kd,
            default_position_control_kp,
            default_position_control_kd);

    observation.velocity << -.01, .01;

    Types::Action result_action_with_velocity =
        Functions::process_desired_action(
            Types::Action::Position(desired_position),
            observation,
            max_torque_Nm,
            safety_kd,
            default_position_control_kp,
            default_position_control_kd);

    // Since velocity has same sign as position error, the resulting
    // (absolute) torques should be lower (since it is
    //   torque = P * position_error - D * velocity )
    EXPECT_LT(result_action_without_velocity.torque[0],
              result_action_with_velocity.torque[0]);
    EXPECT_GT(result_action_without_velocity.torque[1],
              result_action_with_velocity.torque[1]);
}

/**
 * @brief Test position control with custom gains.
 */
TEST_F(TestProcessDesiredAction, position_controller_custom_gains)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    observation.position << 0, 0;
    observation.velocity << 0, 0;

    Vector desired_position, kp, kd;
    // use low position change to not saturate torques with lower gains
    desired_position << -.01, .01;
    // set gains significantly higher than the default gains above
    kp << 10, 10;
    kd << 5, 5;

    Types::Action resulting_action_default_gains =
        Functions::process_desired_action(
            Types::Action::Position(desired_position),
            observation,
            max_torque_Nm,
            safety_kd,
            default_position_control_kp,
            default_position_control_kd);

    Types::Action resulting_action_custom_gains =
        Functions::process_desired_action(
            Types::Action::Position(desired_position, kp, kd),
            observation,
            max_torque_Nm,
            safety_kd,
            default_position_control_kp,
            default_position_control_kd);

    // verify that custom gains are set in resulting action
    EXPECT_EQ(kp[0], resulting_action_custom_gains.position_kp[0]);
    EXPECT_EQ(kp[1], resulting_action_custom_gains.position_kp[1]);
    EXPECT_EQ(kd[0], resulting_action_custom_gains.position_kd[0]);
    EXPECT_EQ(kd[1], resulting_action_custom_gains.position_kd[1]);

    // Since custom gains are higher, the resulting (absolute) torques should be
    // higher
    EXPECT_GT(resulting_action_default_gains.torque[0],
              resulting_action_custom_gains.torque[0]);
    EXPECT_LT(resulting_action_default_gains.torque[1],
              resulting_action_custom_gains.torque[1]);
}

/**
 * @brief Test combined torque and position control.
 */
TEST_F(TestProcessDesiredAction, position_controller_and_torque)
{
    // disable velocity damping for this test
    safety_kd << 0., 0.;

    observation.position << 0, 0;
    observation.velocity << 0, 0;

    Vector desired_torque;
    desired_torque << -0.2, 0.2;
    Vector desired_position;
    desired_position << -.1, .1;
    Types::Action action =
        Types::Action::TorqueAndPosition(desired_torque, desired_position);

    Types::Action resulting_action =
        Functions::process_desired_action(action,
                                          observation,
                                          max_torque_Nm,
                                          safety_kd,
                                          default_position_control_kp,
                                          default_position_control_kd);

    // Just testing here if the torque command goes in the right direction

    ASSERT_LT(resulting_action.torque[0], desired_torque[0]);
    ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);

    ASSERT_GT(resulting_action.torque[1], desired_torque[1]);
    ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/**
2			* @file
3			* @brief Test for processing of robot actions
4			* @copyright Copyright (c) 2019, New York University and Max Planck
5			* Gesellschaft.
6			*/
7			#include <gtest/gtest.h>
8			#include "robot_interfaces/n_joint_robot_functions.hpp"
9
10			/**
11			* @brief Fixture for the tests of process_desired_action().
12			*/
13	✗✓✓✗ ✓✗✓✗ ✓✗	18	class TestProcessDesiredAction : public ::testing::Test
14			{
15			protected:
16			using Functions = robot_interfaces::NJointRobotFunctions<2>;
17			using Types = Functions::Types;
18			using Vector = Types::Vector;
19
20			Types::Observation observation;
21			double max_torque_Nm;
22			Vector safety_kd;
23			Vector default_position_control_kp;
24			Vector default_position_control_kd;
25
26		9	void SetUp() override
27			{
28			// set some arbitrary default values. Tests can overwrite specific
29			// values if needed.
30	✓✗✓✗	9	observation.position << 12.34, -0.42;
31	✓✗✓✗	9	observation.velocity << .4, -.2;
32	✓✗✓✗	9	observation.torque << 0.3, -0.25;
33
34		9	max_torque_Nm = 0.3;
35	✓✗✓✗	9	safety_kd << 0.1, 0.1;
36
37	✓✗✓✗	9	default_position_control_kp << 3, 4;
38	✓✗✓✗	9	default_position_control_kd << .3, .4;
39		9	}
40			};
41
42			/**
43			* @brief Test if a valid torque command is returned unchanged (no safety).
44			*/
45	✗✓	5	TEST_F(TestProcessDesiredAction, valid_torque_no_safety)
46			{
47			// disable velocity damping for this test
48	✓✗✓✗	1	safety_kd << 0., 0.;
49
50	✓✗	1	Vector desired_torque;
51	✓✗✓✗	1	desired_torque << 0.1, 0.2;
52	✓✗✓✗	1	Types::Action action = Types::Action::Torque(desired_torque);
53
54			Types::Action resulting_action =
55			Functions::process_desired_action(action,
56			observation,
57			max_torque_Nm,
58			safety_kd,
59			default_position_control_kp,
60	✓✗	1	default_position_control_kd);
61
62	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(desired_torque[0], resulting_action.torque[0]);
63	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(desired_torque[1], resulting_action.torque[1]);
64			}
65
66			/**
67			* @brief Test if clamping to max. torque is working.
68			*/
69	✗✓	5	TEST_F(TestProcessDesiredAction, exceed_max_torque_no_safety)
70			{
71			// disable velocity damping for this test
72	✓✗✓✗	1	safety_kd << 0., 0.;
73
74	✓✗	1	Vector desired_torque;
75	✓✗✓✗	1	desired_torque << 0.5, -1.2;
76	✓✗✓✗	1	Types::Action action = Types::Action::Torque(desired_torque);
77
78			Types::Action resulting_action =
79			Functions::process_desired_action(action,
80			observation,
81			max_torque_Nm,
82			safety_kd,
83			default_position_control_kp,
84	✓✗	1	default_position_control_kd);
85
86	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_EQ(max_torque_Nm, resulting_action.torque[0]);
87	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_EQ(-max_torque_Nm, resulting_action.torque[1]);
88			}
89
90			/**
91			* @brief Test velocity damping with low velocity
92			*/
93	✗✓	5	TEST_F(TestProcessDesiredAction, velocity_damping_low_velocity)
94			{
95	✓✗	1	Vector desired_torque;
96	✓✗✓✗	1	desired_torque << 0.1, 0.2;
97	✓✗✓✗	1	Types::Action action = Types::Action::Torque(desired_torque);
98
99			Types::Action resulting_action =
100			Functions::process_desired_action(action,
101			observation,
102			max_torque_Nm,
103			safety_kd,
104			default_position_control_kp,
105	✓✗	1	default_position_control_kd);
106
107			// joint 0 has positive velocity so expecting a reduced torque.
108			// joint 1 has negative velocity so expecting an increased torque.
109			// both should remain within the max. torque range.
110	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_LT(resulting_action.torque[0], desired_torque[0]);
111	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
112	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_GT(resulting_action.torque[1], desired_torque[1]);
113	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
114			}
115
116			/**
117			* @brief Test velocity damping with very high velocity.
118			*
119			* This test ensures that torques cannot exceed the allowed max. torque due to
120			* the velocity damping.
121			*/
122	✗✓	5	TEST_F(TestProcessDesiredAction, velocity_damping_high_velocity)
123			{
124			// set very high velocity
125	✓✗✓✗	1	observation.velocity << 4000, -2000;
126
127	✓✗	1	Vector desired_torque;
128	✓✗✓✗	1	desired_torque << 0.1, 0.2;
129	✓✗✓✗	1	Types::Action action = Types::Action::Torque(desired_torque);
130
131			Types::Action resulting_action =
132			Functions::process_desired_action(action,
133			observation,
134			max_torque_Nm,
135			safety_kd,
136			default_position_control_kp,
137	✓✗	1	default_position_control_kd);
138
139			// joint 0 has positive velocity so expecting a reduced torque.
140			// joint 1 has negative velocity so expecting an increased torque.
141			// both should remain within the max. torque range.
142	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_LT(resulting_action.torque[0], desired_torque[0]);
143	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
144	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_GT(resulting_action.torque[1], desired_torque[1]);
145	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
146			}
147
148			/**
149			* @brief Test basic position controller (default gains, no velocity)
150			*/
151	✗✓	5	TEST_F(TestProcessDesiredAction, position_controller_basic)
152			{
153			// disable velocity damping for this test
154	✓✗✓✗	1	safety_kd << 0., 0.;
155
156	✓✗✓✗	1	observation.position << 0, 0;
157	✓✗✓✗	1	observation.velocity << 0, 0;
158
159	✓✗	1	Vector desired_position;
160	✓✗✓✗	1	desired_position << -1, 1;
161	✓✗✓✗ ✓✗✓✗	1	Types::Action action = Types::Action::Position(desired_position);
162
163			Types::Action resulting_action =
164			Functions::process_desired_action(action,
165			observation,
166			max_torque_Nm,
167			safety_kd,
168			default_position_control_kp,
169	✓✗	1	default_position_control_kd);
170
171			// Just testing here if the torque command goes in the right direction
172
173	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_LT(resulting_action.torque[0], 0.0);
174	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
175
176	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_GT(resulting_action.torque[1], 0.0);
177	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
178
179			// verify that position and gains are set correctly in the returned action
180	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(desired_position[0], resulting_action.position[0]);
181	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(desired_position[1], resulting_action.position[1]);
182	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(default_position_control_kp[0], resulting_action.position_kp[0]);
183	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(default_position_control_kp[1], resulting_action.position_kp[1]);
184	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(default_position_control_kd[0], resulting_action.position_kd[0]);
185	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_EQ(default_position_control_kd[1], resulting_action.position_kd[1]);
186			}
187
188			/**
189			* @brief Test position controller for only one joint.
190			*/
191	✗✓	5	TEST_F(TestProcessDesiredAction, position_controller_one_joint_only)
192			{
193			// disable velocity damping for this test
194	✓✗✓✗	1	safety_kd << 0., 0.;
195
196	✓✗✓✗	1	observation.position << 0, 0;
197	✓✗✓✗	1	observation.velocity << 0, 0;
198
199	✓✗	1	Vector desired_position;
200	✓✗✓✗	1	desired_position << -1, std::numeric_limits<double>::quiet_NaN();
201	✓✗✓✗ ✓✗✓✗	1	Types::Action action = Types::Action::Position(desired_position);
202
203			Types::Action resulting_action =
204			Functions::process_desired_action(action,
205			observation,
206			max_torque_Nm,
207			safety_kd,
208			default_position_control_kp,
209	✓✗	1	default_position_control_kd);
210
211			// Just testing here if the torque command goes in the right direction
212	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_LT(resulting_action.torque[0], 0.0);
213	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
214
215			// desired position for joint 1 is nan, so no torque should be generated
216	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_EQ(0.0, resulting_action.torque[1]);
217			}
218
219			/**
220			* @brief Test position control with velocity (i.e. verifying D-gain is working)
221			*/
222	✗✓	5	TEST_F(TestProcessDesiredAction, position_controller_with_velocity)
223			{
224			// disable velocity damping for this test
225	✓✗✓✗	1	safety_kd << 0., 0.;
226
227	✓✗✓✗	1	observation.position << 0, 0;
228	✓✗✓✗	1	observation.velocity << 0, 0;
229
230	✓✗	1	Vector desired_position;
231			// use low position change to not saturate torques
232	✓✗✓✗	1	desired_position << -.01, .01;
233
234			Types::Action result_action_without_velocity =
235			Functions::process_desired_action(
236	✓✗✓✗ ✓✗✓✗	2	Types::Action::Position(desired_position),
237			observation,
238			max_torque_Nm,
239			safety_kd,
240			default_position_control_kp,
241	✓✗	2	default_position_control_kd);
242
243	✓✗✓✗	1	observation.velocity << -.01, .01;
244
245			Types::Action result_action_with_velocity =
246			Functions::process_desired_action(
247	✓✗✓✗ ✓✗✓✗	2	Types::Action::Position(desired_position),
248			observation,
249			max_torque_Nm,
250			safety_kd,
251			default_position_control_kp,
252	✓✗	2	default_position_control_kd);
253
254			// Since velocity has same sign as position error, the resulting
255			// (absolute) torques should be lower (since it is
256			// torque = P * position_error - D * velocity )
257	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗	1	EXPECT_LT(result_action_without_velocity.torque[0],
258			result_action_with_velocity.torque[0]);
259	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗	1	EXPECT_GT(result_action_without_velocity.torque[1],
260			result_action_with_velocity.torque[1]);
261		1	}
262
263			/**
264			* @brief Test position control with custom gains.
265			*/
266	✗✓	5	TEST_F(TestProcessDesiredAction, position_controller_custom_gains)
267			{
268			// disable velocity damping for this test
269	✓✗✓✗	1	safety_kd << 0., 0.;
270
271	✓✗✓✗	1	observation.position << 0, 0;
272	✓✗✓✗	1	observation.velocity << 0, 0;
273
274	✓✗✓✗ ✓✗	1	Vector desired_position, kp, kd;
275			// use low position change to not saturate torques with lower gains
276	✓✗✓✗	1	desired_position << -.01, .01;
277			// set gains significantly higher than the default gains above
278	✓✗✓✗	1	kp << 10, 10;
279	✓✗✓✗	1	kd << 5, 5;
280
281			Types::Action resulting_action_default_gains =
282			Functions::process_desired_action(
283	✓✗✓✗ ✓✗✓✗	2	Types::Action::Position(desired_position),
284			observation,
285			max_torque_Nm,
286			safety_kd,
287			default_position_control_kp,
288	✓✗	2	default_position_control_kd);
289
290			Types::Action resulting_action_custom_gains =
291			Functions::process_desired_action(
292	✓✗✓✗ ✓✗✓✗	2	Types::Action::Position(desired_position, kp, kd),
293			observation,
294			max_torque_Nm,
295			safety_kd,
296			default_position_control_kp,
297	✓✗	2	default_position_control_kd);
298
299			// verify that custom gains are set in resulting action
300	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗	1	EXPECT_EQ(kp[0], resulting_action_custom_gains.position_kp[0]);
301	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗	1	EXPECT_EQ(kp[1], resulting_action_custom_gains.position_kp[1]);
302	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗	1	EXPECT_EQ(kd[0], resulting_action_custom_gains.position_kd[0]);
303	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗	1	EXPECT_EQ(kd[1], resulting_action_custom_gains.position_kd[1]);
304
305			// Since custom gains are higher, the resulting (absolute) torques should be
306			// higher
307	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗	1	EXPECT_GT(resulting_action_default_gains.torque[0],
308			resulting_action_custom_gains.torque[0]);
309	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗	1	EXPECT_LT(resulting_action_default_gains.torque[1],
310			resulting_action_custom_gains.torque[1]);
311		1	}
312
313			/**
314			* @brief Test combined torque and position control.
315			*/
316	✗✓	5	TEST_F(TestProcessDesiredAction, position_controller_and_torque)
317			{
318			// disable velocity damping for this test
319	✓✗✓✗	1	safety_kd << 0., 0.;
320
321	✓✗✓✗	1	observation.position << 0, 0;
322	✓✗✓✗	1	observation.velocity << 0, 0;
323
324	✓✗	1	Vector desired_torque;
325	✓✗✓✗	1	desired_torque << -0.2, 0.2;
326	✓✗	1	Vector desired_position;
327	✓✗✓✗	1	desired_position << -.1, .1;
328			Types::Action action =
329	✓✗✓✗ ✓✗✓✗ ✓✗	1	Types::Action::TorqueAndPosition(desired_torque, desired_position);
330
331			Types::Action resulting_action =
332			Functions::process_desired_action(action,
333			observation,
334			max_torque_Nm,
335			safety_kd,
336			default_position_control_kp,
337	✓✗	1	default_position_control_kd);
338
339			// Just testing here if the torque command goes in the right direction
340
341	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_LT(resulting_action.torque[0], desired_torque[0]);
342	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_GE(resulting_action.torque[0], -max_torque_Nm);
343
344	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗	1	ASSERT_GT(resulting_action.torque[1], desired_torque[1]);
345	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✓✗	1	ASSERT_LE(resulting_action.torque[1], max_torque_Nm);
346	✓✗✓✗	3	}