ContinuumRobotExamples
ContinuumRobotExamples copied to clipboard
about the fsolve() function in TendonRobotStatics.m
The fsolve() function in TendonRobotStatics.m can easily diverge when you give a little larger tendon forces. After tunning, this can be fixed by a more accurate initial guess, for example:
n0_guess = [0;0;-sum(tau)];
v0_guess = inv(Kse)*inv(R0)*n0_guess + [0;0;1];
m0_guess = zeros(3,1);
for i = 1:length(tau)
m0_guess = m0_guess + cross( r{i}, tau(i)*[0;0;-1] );
end
u0_guess = inv(Kbt)*inv(R0)*m0_guess + [0;0;0];
init_guess = [n0_guess; u0_guess];
global Y; % Forward declaration for future scoping rule changes
fsolve(@shootingFunction, init_guess, options);
Good point- the initial guess makes a big difference in the performance of the solver, and can be the difference between success and failure. One approach is iteratively solving the problem for increasing tendon forces, i.e. parameter stepping. For an initial guess, using the equilibrium should be a huge step up from guessing all zeroes.
The git process isn't too hard if you want to make this change. You just create a new branch named something like "better-tendon-guess", create a commit on your branch with your changes, then create a pull request to merge your branch into the main one.
Just two things to change: v0_guess isn't necessary, and u0_guess = Kbt\mb0_guess is fine. And n0_guess can be nb0_guess.