From b5abc084709f8da7c282c4166b1a2a45cf0bfe05 Mon Sep 17 00:00:00 2001
From: maxwell <maxwell.mzy@cainiao.com>
Date: Sat, 15 Jun 2024 23:13:31 +0800
Subject: [PATCH] add dJdqi in to dJdqi

---
 matlab-diff/+redmax/Scene.m            |  3 ++-
 matlab-diff/+redmax/TaskBDF1PointPos.m |  7 +++++++
 matlab-diff/driverRedMaxAdjointBDF1.m  | 16 +++++++++-------
 3 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/matlab-diff/+redmax/Scene.m b/matlab-diff/+redmax/Scene.m
index 9b03eeb..e9b37b6 100644
--- a/matlab-diff/+redmax/Scene.m
+++ b/matlab-diff/+redmax/Scene.m
@@ -131,7 +131,7 @@ function reset(this)
 		end
 		
 		%%
-		function saveHistory(this,Hl,Hu,Hp,M,f,K,D,J)
+		function saveHistory(this,Hl,Hu,Hp,M,f,K,D,J,dJdq)
 			[q,qdot] = this.joints{1}.getQ();
 			this.history(this.k).q = q;
 			this.history(this.k).qdot = qdot;
@@ -146,6 +146,7 @@ function saveHistory(this,Hl,Hu,Hp,M,f,K,D,J)
 				this.history(this.k).K = K;
 				this.history(this.k).D = D;
 				this.history(this.k).J = J;
+				this.history(this.k).dJdq = dJdq;
 			end
 			if ~isempty(this.task)
 				% Used by the adjoint method
diff --git a/matlab-diff/+redmax/TaskBDF1PointPos.m b/matlab-diff/+redmax/TaskBDF1PointPos.m
index 8ed9bf1..4a8f2ba 100644
--- a/matlab-diff/+redmax/TaskBDF1PointPos.m
+++ b/matlab-diff/+redmax/TaskBDF1PointPos.m
@@ -90,6 +90,13 @@ function calcStep(this)
 				R = E(1:3,1:3);
 				dxdqm(:,this.body.idxM) = R*se3.Gamma(this.xlocal);
 				J = this.scene.history(k).J;
+
+                nr = redmax.Scene.countR();
+	            for i = 1 : nr
+		            dJdqi = this.scene.history(k).dJdq(:,:,i);
+                    J(:,i) =  J(:,i) + dJdqi*q;
+                end
+
 				this.dPdq{k} = J'*dxdqm'*dx*wp;
 			else
 				this.dPdq{k} = zeros(nr,1);
diff --git a/matlab-diff/driverRedMaxAdjointBDF1.m b/matlab-diff/driverRedMaxAdjointBDF1.m
index 2ce4b03..f594550 100644
--- a/matlab-diff/driverRedMaxAdjointBDF1.m
+++ b/matlab-diff/driverRedMaxAdjointBDF1.m
@@ -79,7 +79,7 @@ function simLoop(scene)
 	
 	% Compute new state
 	q1 = q0 + h*qdot0; % initial guess
-	[q1,GL,GU,Gp,M,f,K,D,J] = newton(@(q1)evalBDF1(q1,scene),q1);
+	[q1,GL,GU,Gp,M,f,K,D,J,dJdq] = newton(@(q1)evalBDF1(q1,scene),q1);
 	qdot1 = (q1 - q0)/h;
 	
 	% Save new state
@@ -94,7 +94,7 @@ function simLoop(scene)
 	scene.k = k + 1;
 	
 	% End of step
-	scene.saveHistory(GL,GU,Gp,M,f,K,D,J);
+	scene.saveHistory(GL,GU,Gp,M,f,K,D,J,dJdq);
 	scene.draw();
 end
 %fprintf('%d steps\n',nsteps);
@@ -102,7 +102,7 @@ function simLoop(scene)
 end
 
 %%
-function [x,Hl,Hu,Hp,M,f,K,D,J] = newton(evalFcn,xInit)
+function [x,Hl,Hu,Hp,M,f,K,D,J,dJdq] = newton(evalFcn,xInit)
 tol = 1e-9;
 dxMax = 1e3;
 iterMax = 5*length(xInit);
@@ -110,7 +110,7 @@ function simLoop(scene)
 x = xInit;
 iter = 1;
 while true
-	[g,H,M,f,K,D,J] = evalFcn(x);
+	[g,H,M,f,K,D,J,dJdq] = evalFcn(x);
 	if testGrad
 		% Finite difference test
 		sqrteps = sqrt(eps); %#ok<UNRCH>
@@ -146,7 +146,7 @@ function simLoop(scene)
 end
 
 %%
-function [g,H,M,f,K,D,J] = evalBDF1(q1,scene)
+function [g,H,M,f,K,D,J,dJdq] = evalBDF1(q1,scene)
 h = scene.h;
 h2 = h*h;
 nr = redmax.Scene.countR();
@@ -165,7 +165,7 @@ function simLoop(scene)
 	g = M*dqtmp - h2*f;
 else
 	jroot.update();
-	[M,f,dMdq,K,D,J] = computeValues(scene);
+	[M,f,dMdq,K,D,J,dJdq] = computeValues(scene);
 	g = M*dqtmp - h2*f;
 	H = M - h*D - h2*K;
 	for i = 1 : nr
@@ -176,13 +176,15 @@ function simLoop(scene)
 end
 
 %%
-function [M,f,dMdq,K,D,J] = computeValues(scene)
+function [M,f,dMdq,K,D,J,dJdq] = computeValues(scene)
 nr = redmax.Scene.countR();
 broot = scene.bodies{1};
 jroot = scene.joints{1};
 froot = scene.forces{1};
 
 qdot = jroot.getQdot();
+nm = redmax.Scene.countM();
+dJdq = zeros(nm,nr,nr);
 if nargout == 2
 	[J,Jdot] = jroot.computeJacobian();
 	[Mm,fm] = broot.computeMassGrav(scene.grav);