iCub 2 Shoulder Constraints degrees <--> radians?

[Woolfrey]

The iCub wiki specifies the shoulder constraints as Aq + b > 0 where q is in degrees, if I'm not mistaken.

I'm working in radians for all the other calculations. How should I convert the above constraint to radians? I tried multiplying the A matrix by 180/pi, but when I print the result of Aq+b the result seems very large (i.e. very far from the constraints). I'm not sure if this is correct or not!

Thanks in advance!

[pattacini]

Hi @Woolfrey

Judging from https://github.com/robotology/icub-main/blob/master/src/libraries/iKin/src/iKinIpOpt.cpp#L155-L174, it seems that only the vector $b$ gets converted in radians, having the shoulders' joints already expressed in radians.

Therefore, the documentation is not really clear in this aspect. I'll amend it.

[Woolfrey]

EDIT: Sorry, I misread: I only need to convert the vector b to radians? (That would have been my second guess).

Just double-checking so I don't break the robot!

[pattacini]

EDIT: Sorry, I misread: I only need to convert the vector b to radians? (That would have been my second guess).

Yep 👍🏻

[pattacini]

Opened up this PR:

• Fix shoulders' constraints equation icub-tech-iit/documentation#233

[pattacini]

I'll merge the PR straight away as the documentation is flawed now.

[Woolfrey]

Great, thanks! I will check it out in Gazebo tomorrow morning before moving to the real robot.

[Woolfrey]

My Cartesian controller and QP solver appear to be working fine with the added constraint. I'm getting different behaviour for the left and right arms when moving up and down:

I'm assuming this is because the left and right arms start in slightly different configurations which causes the QP solver to find different joint motion. I'm also minimizing instantaneous kinetic energy, so this might explain some of it (1 arm moving fast + 1 moving slow < 2 arms moving fast). I checked that A*q + b which is all non-zero as well (5 constraints for each arm, so output is 10x1 vector):

Is this kind of shoulder motion to be expected, in your experience?

[pattacini]

The different behaviors you spotted are perfectly normal (I saw them too) as long as the primary task is deemed attained correctly.

The solver can find different configurations of the shoulders due to the different starting points and the redundancy of the manipulators unless some regularization term is explicitly enforced, for example on the height of the elbow, which in turn will constrain the search space of the solver.