The figure below is a representation of a 2D double pendulum. The longer arm of the pendulum is connected to a fixed support and a joint. The shorter arm is connected to a joint on the longer arm and is free swinging on the other end. The shorter arm additionally has two point markers spaced 1.25m from the joint.
The trajectories of the two dots are assumed to be tracked in a motion capture lab with the coordinates recorded each second.
The trajectories are given in sets m1 and m2 for the respective points. The trajectory location contains:
Setting up the optimization problem
We first start out by setting up the optimization problem. The behavior of the system can be fully described by the two angles as the Degrees of Freedom (DOF) are calculated as:
|Description||Effect on DOF|
|Two bodies with x 3 DOF||+6|
|Two revolute joints x 2 DOF||-4|
Giving us two degrees of freedom as 𝜃1 and 𝜃2.
Now we could easily solve this problem for the angles but that wouldn't be an optimization problem. These types of problem are useful when there is measurement error as the solution will converge to the most likely solution within the defined parameters.
In order to do this, we begin the optimization problem by setting up the system of constraints:
The constraints here relate the angles to the x and y coordinates of the points m1 and m2.
In order to evaluate the system state, an obective function must be defined. For this situation we use the least square method which can be written as:
Or, in an easier to read (but improper) form:
We will be passing this obective function to the matlab function 'fminunc' in order to evaluate the system state for each set of points.
Solving the Optimization Problem in Matlab using fminunc
The code used in matlab to solve for the system state with a set of inputs is as such:
%clear all the previous output and variables clear all clc close all %set the lengths l1=0.5 l2=0.125 %test data input of the x and y coordinates of m1 and m2 m1x=2.011065770951412e-001 m2x=2.728036316390219e-001 m1y=-5.853569186806583e-001 m2y=-6.877509242167823e-001 %definition of the obective function (ugly because of how fminunc wants the input) psi=@(theta)1/2*[ l1*cos(theta(1))+l2*cos(theta(2))-m1x; l1*cos(theta(1))+2*l2*cos(theta(2))-m2x; l1*sin(theta(1))+l2*sin(theta(2))-m1y; l1*sin(theta(1))+2*l2*sin(theta(2))-m2y].'*[l1*cos(theta(1))+l2*cos(theta(2))-m1x; l1*cos(theta(1))+2*l2*cos(theta(2))-m2x; l1*sin(theta(1))+l2*sin(theta(2))-m1y; l1*sin(theta(1))+2*l2*sin(theta(2))-m2y] %solve for the minimum with search start at [2,2] fminunc(psi,[2,2])