<jupyter_start><jupyter_text>AREBO SimulationNotebook that simulates AREBO's open kinematic chain, and the AREBO-Human closed kinematic chain. The simulation is visualized using iPython's interactive function.<jupyter_code>%pylab inline
import itertools
import functools
import pandas as pd
import sys
import json
import os

sys.path.append("scripts")
from ipywidgets import widgets
from ipywidgets import interactive
from mpl_toolkits.mplot3d import Axes3D

# From my libraries
from myrobotics import rotx, roty, rotz, HTMat, HTMat4DH
from myrobotics import forward_kinematics
from arebo import arebo_inv_kinematics
from arebo import a1, a2, a3, b1, b2 , b3
from arebo import sincos
from arebo import show_arebo
from arebo import show_arebo_human
def arebo_inv_kinematics(Xh, Rh, dh):
    # Inverse Kinematics
    # Step 1. Find the orientation of the plane in the robot.
    ta1 = np.arctan2(Xh[1] , Xh[0])
    
    # Step 2. Find new position for the 3-link planar arm.
    _s1, _c1 = sincos(ta1)
    if np.isclose(_s1, 0.0):
        xd = Xh[0] / _c1
    elif np.isclose(_c1, 0.0):
        xd = Xh[1] / _s1
    else:
        xd = 0.5 * ((Xh[0] / _c1) + (Xh[1] / _s1))
    yd = Xh[2]
    
    # Step 3.
    _R1 = np.array([[_c1, 0, _s1],
                    [_s1, 0, -_c1],
                    [0, 1, 0]])
    _Rh = np.matmul(_R1.T, Rh)

    # Determine t2 + t3 + t4
    _ta4 = np.arctan2(_Rh[0, 2], -_Rh[1, 2])
    
    # Determine t6
    ta6 = np.arctan2(_Rh[2, 0], _Rh[2, 1]) 

    # Determine t5
    _s4t, _c4t = sincos(_ta4)
    _s6, _c6 = sincos(ta6)
    
    # Find _r1 and _r2
    # _r1
    if np.isclose(_c4t, 0.0):
        _r1 = _Rh[0, 2] / _s4t
    elif np.isclose(_s4t, 0.0):
        _r1 = - _Rh[1, 2] / _c4t
    else:
        _r1 = 0.5 * (_Rh[0, 2] / _s4t - _Rh[1, 2] / _c4t)
        
    # _r2
    if np.isclose(_c6, 0.0):
        _r2 = _Rh[2, 0] / _s6
    elif np.isclose(_s6, 0.0):
        _r2 = _Rh[2, 1] / _c6
    else:
        _r2 = 0.5 * ((_Rh[2, 0] / _s6) + (_Rh[2, 1] / _c6))
    
    ta5 = np.arctan2(-_Rh[2, 2], 0.5 * (_r1 + _r2))
    _s5, _c5 = sincos(ta5)
    
    # Convert to 2-link planar arm problem
    a, b, c = dh[1]['a'], dh[2]['a'], dh[4]['d']
    _xd = xd - c * _c4t
    _yd = yd - c * _s4t    
    _n = _xd**2 + _yd**2 - a**2 - b**2
    _d = 2 * a * b
    ta3 = np.arccos(_n / _d)

    alpha = np.arctan2(_yd, _xd)

    _n = _xd**2 + _yd**2 + a**2 - b**2
    _d = 2 * a * np.sqrt(_xd**2 + _yd**2)
    beta = np.arccos(_n / _d)

    ta2 = alpha - beta
    ta4 = _ta4 - ta2 - ta3

    return np.array([ta1, ta2, ta3, ta4, ta5, ta6])
def interactive_closedchain_skin(ph1, ph2, ph3):
    p_h = [np.pi * _t / 180. for _t in [ph1, ph2, ph3]]
    
    # Human forward kinematics
    # Human HT matices 
    # Find Homogenrous transformation matrices.
    _Hh = [HTMat4DH(_t + _dh['t'], _dh['d'], _dh['a'], _dh['al'])
           for _t, _dh in zip(p_h, human_dh)]
    _Hhuman = functools.reduce(np.dot, _Hh)
    _Xhuman = _Hhuman[:3, 3] + sh_pos
    _Rhuman = _Hhuman[:3, :3]

    # AREBO position inverse kinematics
    ta = arebo_inv_kinematics(Xh=_Xhuman, Rh=_Rhuman, dh=arebo_dh)
    show_arebo_human(arebo_dh, ta, human_dh, p_h, sh_pos)


def interactive_arebo_fkin(th1, th2, th3, th4, th5, th6):
    ta = [np.pi * _t / 180. for _t in [th1, th2, th3, th4, th5, th6]]
    show_arebo(arebo_dh, ta)<jupyter_output><empty_output><jupyter_text>AREBO New Design<jupyter_code># AREBO parameters
a = 8.0
b = 6.0
c = 2.0

# a = 40.0
# b = 40.0
# c = 15.0

arebo_dh = [{'a': 0, 'al': +np.pi/2, 'd': 0, 't': 0},
            {'a': a, 'al': 0,        'd': 0, 't': 0},
            {'a': b, 'al': 0,        'd': 0, 't': 0},
            {'a': 0, 'al': -np.pi/2, 'd': 0, 't': -np.pi/2},
            {'a': 0, 'al': +np.pi/2, 'd': c, 't': +np.pi/2},
            {'a': 0, 'al': 0,        'd': 0, 't': +np.pi/2}]
# Human Arm parameters
uh = 8.0
human_dh = [{'a': 0, 'al': +np.pi/2, 'd': 0,  't': 0},
            {'a': 0, 'al': -np.pi/2, 'd': 0,  't': -np.pi/2},
            {'a': 0, 'al': 0,        'd': uh, 't': -np.pi}]

sh_pos = [2, 2, 6]
# sh_pos = [-15.0, -15.0, 10.0]<jupyter_output><empty_output><jupyter_text>Visualize AREBO Forward Kinematics<jupyter_code>interactive_plot = interactive(interactive_arebo_fkin,
                               th1=(-180, 180, 5),
                               th2=(-180, 180, 5),
                               th3=(-180, 180, 5),
                               th4=(-180, 180, 5),
                               th5=(-180, 180, 5),
                               th6=(-180, 180, 5)
                              )
output = interactive_plot.children[-1]
output.layout.height = '600px'
interactive_plot<jupyter_output><empty_output><jupyter_text>Visualize AREBO-Human Closed Kinematic Chain<jupyter_code>interactive_plot = interactive(interactive_closedchain_skin,
                               ph1=(-180, 180, 5),
                               ph2=(-180, 180, 5),
                               ph3=(-180, 180, 5)
                              )
output = interactive_plot.children[-1]
output.layout.height = '600px'
interactive_plot<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1591486037883226, 'fraction_numerical': 0.06483108767818785, 'mean_word_length': 2.076276276276276, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '4', 'dir': 'jupyter-structured-clean-dedup', 'id': '392300', 'license': 'MIT', 'n_tokens_mistral': 2339, 'n_tokens_neox': 2142, 'n_words': 536, 'path': 'AREBO_Simulation.ipynb', 'repo_name': 'siva82kb/AREBO', 'size': '5121'}