Drake | Zend Framework 1.x and 2.x extension library

I am trying to get the Coriolis matrix for my robot (need the matrix explicitly for the controller) based on the following approach which I have found online:

Basically there are rectangles (buildings) and circles (people).

The task I need to do is basically, when a function "fg" is called, every circle that is inside a given radius needs to run to the closest rectangle, and after all the circles inside the radius finds a rectangle, I need to report on a .txt file the names of the circles that run to each rectangle sorted alphabetically.

Such as:

Rectangle A: c1 c2 c3

Rectangle B: c7 c11 c20

...

And so on...

I need to store the addresses of the circles that run, on a vector of each rectangle. I tried to use qsort from stdlib.h, but maybe the function that i use to compare is wrong

(EDIT - full code to better understand):

I am working on recovering from collision. I have the names of bodies in collision and the frames associated with them and now I want to move the body/frame that is closer to the end effector to get out of collision, but I couldn't find a straightforward way to get this information from a MultiBodyPlant. I could construct another representation of the graph and search through it, but I was wondering if it is possible to maybe get this from drake instead?

I see the current chapter of Underactuated: System Identification and the corresponding notebook, and it currently does it through symbolics.

I'd like to try out stuff like system identification using forward-mode automatic differentiation ("autodiff" via AutoDiffXd, etc.), just to check things like scalability, get a better feel for symbolics and autodiff options in Drake, etc.

As a first steps towards system identification with autodiff, how do I take gradients of MultibodyPlant quantities (e.g. generalized forces, forward dynamics, etc.) with respect to inertial parameters (say mass)?

• Note: Permalinks of Underactuated chapter + notebook at time of writing: sysid.html, sysid.ipynb

I have been trying to get a linearized version of a MultiBodyPlant without gravity to experiment with LQR for systems without gravity. However, the linearization leads to some interesting phenomena. A simplified example can be found in this google colab notebook.

When I linearize and check the rank of the controllability matrix for a single free-floating rigid body, I get a rank of 6. This is expected as I use the get_applied_generalized_force_input_port() as the input port, hence making sure that all possible forces can be applied to the system. The system of a single rigid body has 6 degrees of freedom (DoF) and the rank of the controllability matrix is 6, hence it is controllable.

However, when I use Drake's in-built function IsControllable() to check the controllability, it results False meaning that it thinks the system is not controllable. In the source of the IsControllable() function, the rank of the matrix is checked against the number of rows in the A matrix. I think that this might be causing an issue as the linearization involves the use of quaternions during the AutoDiff (thus adding one more row to the A matrix due to 4 numbers being used for quaternions to represent the state). The linearization process does not know about the unit-quaternion constraint, and hence the A matrix for a system using quaternions will have 1 more row than the DoF of the system.

I wonder if this is the correct intuition for the controllability mismatch?

And could this cause issues as other functions within Drake that maybe use the IsControllable() function for verifying controllability?

Consider the following data.frame:

How do I visualize different coordinate frames in drake when using the LCM visualizer, such as the body frame attached to the end effector of the Kuka?

I am learning how to use Drake to solving optimization problems. This problem was to find the optimal length and width of a fence, the fence must have a perimeter less than or equal to 40. The code below only works when the perimeter constraint is an equality constraint. It should work as an inequality constraint, but my optimal solution results in x=[nan nan]. Does anyone know why this is the case?

I'm new to MIT Drake, I was using CasADi (ipopt) for a mecanum wheeled model for MPC.

In CasADi, I can create a symbolic variable for the optimization parameters (i.e., initial state and target state).

Then use it to compute the error in state, how can I get an equivalent functionality in Drake?