I understand you need to calculate variables “jAngsL”, “jAngR”, “siminL”, “siminR”.
These indicate the joint angles (“jAngsL”, “jAngR”) and are calculated using inverse kinematics (IK). The calculations are made by setting the base of each leg as an end-effector and foot as the base.
You can find the code which calculates these joint angles in the file “invKinBody2Foot” in the following file exchange: https://www.mathworks.com/matlabcentral/fileexchange/64227-matlab-and-simulink-robotics-arena-walking-robot
In the file, the function “invKinBody2Foot” calculates the joint angles required to position the foot at a specified transformation relative to the body.
Here's a brief summary about the approach followed in the "invKinBody2Foot" function:
- The input “tform” is a transformation matrix describing the foot’s position and orientation relative to the body.
The function first applies some offsets to convert this transformation from the body to the foot into a form that relates the base of each leg to the foot.
2. The function “invKinBody2Foot” computes the inverse of the transformation matrix. This is done by taking the transpose of rotation matrix R and adjusting the position vector “p”. This effectively transforms the problem into finding the joint angles that position the foot relative to the base of the leg.
This function uses an analytical inverse kinematics solution derived from the paper which the above file mentions at its beginning. This involves solving for the joint angles in reverse order, starting from the foot and moving towards the hip.
3. Calculations for knee angle(“th4”), ankle pitch angle(“th5”), ankle yaw angle(“th6”) are calculated using the properties of triangles and vectors. You can read more about it in the example file: https://viewer.mathworks.com/?viewer=live_code&url=https%3A%2F%2Fwww.mathworks.com%2Fmatlabcentral%2Fmlc-downloads%2Fdownloads%2F3412b526-987b-4ab2-876d-bce7a62a2e5e%2F4697ade5-08c0-4e35-a8e3-78058d7486a0%2Ffiles%2FControlDesign%2FInverseKinematics%2FcalculateInvKin.mlx&embed=web
So, the computed joint angles “th1” through “th6” are packed into a vector “th” representing the complete set of joint angles needed for foot to achieve the desired position and orientation.
You can read more about the approach and calculation in the example section of the following file exchange: https://www.mathworks.com/matlabcentral/fileexchange/64227-matlab-and-simulink-robotics-arena-walking-robot
Hope this helps!
