[BUG] DQ_ClassicQPController.compute_setpoint_control_signal() is returning an error about QP parameter f

[marcos-pereira]

Bug description

• The DQ_ClassicQPController.compute_setpoint_control_signal() returns an error about the QP f parameter.

To Reproduce Code

import numpy as np
from dqrobotics import *
from dqrobotics.solvers import DQ_QuadprogSolver
from dqrobotics.robot_control import DQ_ClassicQPController
from dqrobotics.robot_control import ControlObjective
from dqrobotics.robots import KukaLw4Robot
import math

robot = KukaLw4Robot.kinematics()
qp_solver = DQ_QuadprogSolver()
controller = DQ_ClassicQPController(robot, qp_solver)
controller.set_control_objective(ControlObjective.Pose)
controller.set_gain(10.0)
controller.set_damping(0.001)
q = np.array([math.pi, math.pi, math.pi/2, 0, math.pi, math.pi/2, 0])
effector_p = 0.0*k_
effector_t = 1 + E_*0.5*effector_p
effector_phi = -math.pi
effector_n = j_
effector_r = math.cos(effector_phi/2) + effector_n*math.sin(effector_phi/2)
effector_pose = effector_t*effector_r
controller.compute_setpoint_control_signal(q,vec8(effector_pose))

Output

controller.compute_setpoint_control_signal(q,vec8(effector_pose))
  File ".../.local/lib/python3.8/site-packages/dqrobotics/solvers/__init__.py", line 15, in solve_quadratic_program
    (s1, s2) = np.shape(f)
ValueError: not enough values to unpack (expected 2, got 1)

Expected behavior

• The compute_setpoint_control_signal() should return a numpy array with the control signal.

Environment:

• OS: Ubuntu 20.04
• dqrobotics version 20.4.0.32
• Python version 3.8.5

Additional context

• The init.py contains the following below. It seems that the f parameter is somehow not arriving properly there.

  
  from dqrobotics._dqrobotics._solvers import *

try: import quadprog import numpy as np class DQ_QuadprogSolver(DQ_QuadraticProgrammingSolver): def init(self): DQ_QuadraticProgrammingSolver.init(self) pass

    def solve_quadratic_program(self, H, f, A, b, Aeq, beq):
        # Ignoring Aeq and beq because quadprog doesn't handle them directly

        # Saving shape to remove the singleton dimension
        (s1, s2) = np.shape(f)
        (s3, s4) = np.shape(b)

        (x, f, xu, iterations, lagrangian, iact) = quadprog.solve_qp(G=2*H,
                                                                     a=-np.reshape(f, s1),
                                                                     C=-np.transpose(A),
                                                                     b=-np.reshape(b, s3))
        return x

except: pass

...