c  invkinematic2.f
c
c  Art Brooks 01/15/04
c
c Routine to solve inverse kinematics problem
c  Solves for joint states of mechanism 2
c   that produces equivalent motion as joint states of mechanism 1
c
c
c  Allows for joints to have different base locations and orientations
c  and different end locations and orientations
c  That is, mechanisms can attached to common (implied) fixed body at
c  different locations and orientations
c  (prior version invkinematic.f matched end transforms only)
c
c
c*************************************************
c Incorporates routines to simulate kinematics of mechanism/manipulator
c Mechanism is inputted using Denavit-Hartenberg notation
c  where each link has one degree of freedom (rotation or translation)
c  For given state defined by {a,d,alpha,theta} for each link
c  set up transformation matrix A relating Local (ie end) Coordinate System to
c   Base Coordinate System
c
c
c*************************************************
      module mechanism1
      real*8, dimension(:,:,:), allocatable :: alink
      real*8, dimension(4,4) :: atot, ainv, aunit, atot0, atarget, adif
      real*8, dimension(6) :: state
      real*8, dimension(:,:), allocatable :: p_bcs, p_lcs
      real*8, dimension(:), allocatable :: a, d, alpha, theta, d0,theta0
      real*8, dimension(3) :: ex, ey, ez, x0
      real*8 :: xscale
      integer, dimension(:), allocatable :: itype
      integer :: nlinks
      character*80 head
      character*3 angle_units
      end module mechanism1
c*************************************************
      module mechanism2
      real*8, dimension(:,:,:), allocatable :: alink
      real*8, dimension(4,4) :: atot, ainv, aunit, atot0, atarget, adif
      real*8, dimension(6) :: state
      real*8, dimension(:,:), allocatable :: p_bcs, p_lcs
      real*8, dimension(:), allocatable :: a, d, alpha, theta, d0,theta0
      real*8, dimension(3) :: ex, ey, ez, x0
      integer, dimension(:), allocatable :: itype
      integer :: nlinks
      character*80 head
      character*3 angle_units
      end module mechanism2
c*************************************************
      module points
      integer :: npts
      real*8, dimension(:,:), allocatable :: xpts
      end module points
c*************************************************
      module optdata
      parameter (n=6, lwa=n*(3*n+13)/2 )
      parameter(ldfjac=n, lr=n*(n+1)/2)
      real*8, dimension(n) :: x, fvec
      real*8, dimension(lwa) :: wa
      real*8 diag(n),fjac(ldfjac,n),r(lr),qtf(n),w(n,4)
      end module optdata
c*************************************************
      program main
      use mechanism1, ONLY : state, head, xscale
      use optdata
      use points
      external fcn
c
      read(*,"(a80)") head
      write(*,"(a80)") head
c
      call readmech1
      call readmech2
c
      read(*,*) npts, xscale
      allocate(xpts(4,npts))
      xpts(4,:)=1.
      read(*,*) ((xpts(k,i),k=1,3),i=1,npts)
      xpts(1:3,:) = xpts(1:3,:)*xscale
c
c read in state data for mechanism1 and convert to mechanism2
      open(unit=8,file="ipoints",status="replace")
      open(unit=9,file="amat",status="replace")
c
      istep=0
 10   read(*,*,end=99) (state(i), i=1,6)
      call target1(istep)
c
      x(:)=0.
      do k=1,10
      xtol=1.d18
      ifail=1
      call c05nbf(fcn, n, x, fvec, xtol, wa, lwa, ifail)
c
c
C      write(*,*) "ifail=", ifail, ", nfev=", nfev
C      write(*,"(1p6e15.6)") (x(i), i=1,6)
      enddo
c
      call fcn(n, x, fvec, iflag)
c
C      call outp1
C      call outp2
      write(*,"(1p6e15.6)") (x(i), i=1,6)
c
      istep=istep+1
      goto 10
 99   continue
c
c
      stop
      end
c*************************************************
      subroutine build_a(a,d,alpha,theta, alink)
c*************************************************
      real*8 :: a,d,alpha,theta,ca,sa,ct,st
      real*8, dimension(4,4) :: alink
c
      ca=cos(alpha)
      sa=sin(alpha)
      ct=cos(theta)
      st=sin(theta)
c
      alink(1,:)=(/ct,  -st*ca,  st*sa,  a*ct/)
      alink(2,:)=(/st,   ct*ca, -ct*sa,  a*st/)
      alink(3,:)=(/0.d0,      sa,     ca,  d   /)
      alink(4,:)=(/0.d0,      0.d0,     0.d0,  1.d0  /)
c
      return
      end
c*************************************************
      subroutine gen_ainv(atot, ainv)
c*************************************************
      real*8, dimension(4,4) :: atot, ainv ! transform matrix and its inverse
      real*8, dimension(3,3) :: r,rt ! rotation part of transform matrix and its transpose
      real*8, dimension(3) :: x0 ! translation part of transform matrix
c
c calculate inverse transform for combined rotation and translation (homogenous) matrix
c
      r=atot(1:3,1:3)
      rt=transpose(r)
      x0=atot(1:3,4)
c
      ainv(1:3,1:3)=rt
      ainv(1:3,4)=-matmul(rt,x0)
      ainv(4,1:3)=0.d0
      ainv(4,4)=1.d0
c
      return
      end
c*************************************************
      subroutine fcn(n, x, fvec, iflag)
c solve for mechanism state variables
c to match mechanism 1 state variables
c NOTE: mechanisms 1 and 2 have potential different bases and ends
c       and attach to body at different points.
c       Need to match matmul(atarget1(x)*inv(atarget1(0))) to
c                     matmul(atarget2(x)*inv(atarget2(0)))
c   (not just atarget1 to atarget2 as originally thought to match ends)
c
      use mechanism1, ONLY : atarget1 => atarget, ainv1 => ainv
      use mechanism2
      integer n, iflag
      real*8 x(n), fvec(n)
c
C      write(*,*) "iflag=", iflag
      nlinks=n
      do i=1,nlinks
        if(itype(i).eq.1)then
          d(i)=d0(i)+x(i)
        else
          theta(i)=theta0(i)+x(i)
        endif
      enddo
c
      atot=atot0
      do i=1,nlinks
        call build_a(a(i),d(i),alpha(i),theta(i),alink(:,:,i))
        atot=matmul(atot,alink(:,:,i))
      enddo
c      adif=atot-atarget
      adif=matmul(atot,ainv) - matmul(atarget1,ainv1)
c
      fvec(1)=adif(1,4)
      fvec(2)=adif(2,4)
      fvec(3)=adif(3,4)
      fvec(4)=adif(3,2)-adif(2,3)
      fvec(5)=adif(1,3)-adif(3,1)
      fvec(6)=adif(2,1)-adif(1,2)
c
      return
      end
c***********************************************
      subroutine readmech1
      use mechanism1
c
      read(*,*) nlinks, angle_units
      write(*,"(i5,2x,a3)") nlinks, angle_units
      allocate(a(nlinks),d(nlinks),alpha(nlinks),theta(nlinks))
      allocate(         d0(nlinks),             theta0(nlinks))
      allocate(itype(nlinks))
      allocate(alink(4,4,nlinks))
c
c read in initial parameters for links
c type 1-> prismatic, d varies (ie translation)
c type 2-> revolute, theta varies (ie rotation)
c
c
      pi2=asin(1.d0)*4.d0
      dtor=pi2/360.d0
c
      do i=1,nlinks
      read(*,*) itype(i), a(i), d(i), alpha(i), theta(i)
      write(*,"(i5,4f10.5)") itype(i), a(i), d(i), alpha(i), theta(i)
      if(angle_units.eq."deg")then
        alpha(i)=alpha(i)*dtor
        theta(i)=theta(i)*dtor
      endif
      d0(i)=d(i)
      theta0(i)=theta(i)
      enddo
c
c initialize transform with unit matrix or fixed base link data
c
      read(*,*) iflag
      write(*,"(i5)") iflag
      if(iflag.eq.0)then
        atot(1,:)=(/1.d0,0.d0,0.d0,0.d0/)
        atot(2,:)=(/0.d0,1.d0,0.d0,0.d0/)
        atot(3,:)=(/0.d0,0.d0,1.d0,0.d0/)
        atot(4,:)=(/0.d0,0.d0,0.d0,1.d0/)
      else
        read(*,*) ex, ey, ez, x0
        write(*,"(3f10.5)") ex, ey, ez, x0
        atot(1,:)=(/ex(1),ey(1),ez(1),x0(1)/)
        atot(2,:)=(/ex(2),ey(2),ez(2),x0(2)/)
        atot(3,:)=(/ex(3),ey(3),ez(3),x0(3)/)
        atot(4,:)=(/0.d0   ,0.d0   ,0.d0   ,1.d0   /)
      endif
      atot0=atot
c
      atarget=atot0
      do i=1,nlinks
        call build_a(a(i),d(i),alpha(i),theta(i),alink(:,:,i))
        atarget=matmul(atarget,alink(:,:,i))
      enddo
      call gen_ainv(atarget,ainv)
C      write(*,*) " atarget"
C      write(*,"(4f12.6)") (( atarget(i,j), j=1,4), i=1,4)
c
      return
      end
c***********************************************
      subroutine readmech2
      use mechanism2
c
      read(*,*) nlinks, angle_units
      write(*,"(i5,2x,a3)") nlinks, angle_units
      allocate(a(nlinks),d(nlinks),alpha(nlinks),theta(nlinks))
      allocate(         d0(nlinks),             theta0(nlinks))
      allocate(itype(nlinks))
      allocate(alink(4,4,nlinks))
c
c read in initial parameters for links
c type 1-> prismatic, d varies (ie translation)
c type 2-> revolute, theta varies (ie rotation)
c
c
      pi2=asin(1.d0)*4.d0
      dtor=pi2/360.d0
c
      do i=1,nlinks
      read(*,*) itype(i), a(i), d(i), alpha(i), theta(i)
      write(*,"(i5,4f10.5)") itype(i), a(i), d(i), alpha(i), theta(i)
      if(angle_units.eq."deg")then
        alpha(i)=alpha(i)*dtor
        theta(i)=theta(i)*dtor
      endif
      d0(i)=d(i)
      theta0(i)=theta(i)
      enddo
c
c initialize transform with unit matrix or fixed base link data
c
      read(*,*) iflag
      write(*,"(i5)") iflag
      if(iflag.eq.0)then
        atot(1,:)=(/1.d0,0.d0,0.d0,0.d0/)
        atot(2,:)=(/0.d0,1.d0,0.d0,0.d0/)
        atot(3,:)=(/0.d0,0.d0,1.d0,0.d0/)
        atot(4,:)=(/0.d0,0.d0,0.d0,1.d0/)
      else
        read(*,*) ex, ey, ez, x0
        write(*,"(3f10.5)") ex, ey, ez, x0
        atot(1,:)=(/ex(1),ey(1),ez(1),x0(1)/)
        atot(2,:)=(/ex(2),ey(2),ez(2),x0(2)/)
        atot(3,:)=(/ex(3),ey(3),ez(3),x0(3)/)
        atot(4,:)=(/0.d0   ,0.d0   ,0.d0   ,1.d0   /)
      endif
      atot0=atot
c
      atarget=atot0
      do i=1,nlinks
        call build_a(a(i),d(i),alpha(i),theta(i),alink(:,:,i))
        atarget=matmul(atarget,alink(:,:,i))
      enddo
      call gen_ainv(atarget,ainv)
C      write(*,*) " atarget"
C      write(*,"(4f12.6)") (( atarget(i,j), j=1,4), i=1,4)
c
      return
      end
c***********************************************
      subroutine target1(istep)
      use mechanism1
      use optdata
      use points
      real*8, dimension(4) :: pts
      real*8, dimension(4,4) :: amat
c
      do i=1,nlinks
        if(itype(i).eq.1)then
          d(i)=d0(i)+state(i)
        else
          theta(i)=theta0(i)+state(i)
        endif
      enddo
c
      atarget=atot0
      do i=1,nlinks
        call build_a(a(i),d(i),alpha(i),theta(i),alink(:,:,i))
        atarget=matmul(atarget,alink(:,:,i))
      enddo
c      call gen_ainv(atarget,ainv)
C      write(*,*) " atarget"
C      write(*,"(4f12.6)") (( atarget(i,j), j=1,4), i=1,4)
c
Cc dump out transform of sample point [1,0,0]
c dump out transform of inputted points
c
      do i=1,npts
c      pts= matmul(matmul(atarget,ainv),(/1.,1.,1.,1./))
      pts= matmul(matmul(atarget,ainv),xpts(:,i))
      pts(1:3)=pts(1:3)/xscale
      write(8,"(1p3e15.7)") (pts(k),k=1,3)
      enddo
c
c dump out transform
c
      amat=matmul(atarget,ainv)
      write(9,"('Step ', i5)") istep
      write(9,"(4f21.12)") (( amat(i,j), j=1,4), i=1,4)
      return
      end
c***********************************************
      subroutine target2
      use mechanism2
      use optdata
c
      do i=1,nlinks
        if(itype(i).eq.1)then
          d(i)=d0(i)+state(i)
        else
          theta(i)=theta0(i)+state(i)
        endif
      enddo
c
      atarget=atot0
      do i=1,nlinks
        call build_a(a(i),d(i),alpha(i),theta(i),alink(:,:,i))
        atarget=matmul(atarget,alink(:,:,i))
      enddo
c      call gen_ainv(atarget,ainv)
C      write(*,*) " atarget"
C      write(*,"(4f12.6)") (( atarget(i,j), j=1,4), i=1,4)
c
      return
      end
c*************************************************
      subroutine outp2
      use mechanism2
c
      write(*,*) " outp2 "
      write(*,*) " atot"
      write(*,"(4f12.6)") (( atot(i,j), j=1,4), i=1,4)
      write(*,*) " atot0"
      write(*,"(4f12.6)") (( atot0(i,j), j=1,4), i=1,4)
      write(*,*) " atarget"
      write(*,"(4f12.6)") (( atarget(i,j), j=1,4), i=1,4)
c
      return
      end
c*************************************************
      subroutine outp1
      use mechanism1
c
      write(*,*) " outp1 "
      write(*,*) " atot"
      write(*,"(4f12.6)") (( atot(i,j), j=1,4), i=1,4)
      write(*,*) " atot0"
      write(*,"(4f12.6)") (( atot0(i,j), j=1,4), i=1,4)
      write(*,*) " atarget"
      write(*,"(4f12.6)") (( atarget(i,j), j=1,4), i=1,4)
c
      return
      end