Closure Excavator Example not using closures

[flbauer]

In the example excavator_rr.py closure groups are referring to geometric_XXX functions and not closure_XXX functions

Following modified functions will make the closure example work: `def closure_q_to_a_group_1(state: Dict[str, float]): nlp = {'x': closure_1_state[1:], 'f': c_1, 'p': closure_1_state[0]} nlp_solver = casadi.nlpsol('q_to_a', 'ipopt', nlp, opts) solution = nlp_solver(x0=[0, 0], p=[state['q_1']['ry']]) sol_vector = np.array(solution['x']) return {'a_1': sol_vector[1]}

def closure_a_to_q_group_1(state: Dict[str, float]): nlp = {'x': closure_1_state[:2], 'f': c_1, 'p': closure_1_state[2]} nlp_solver = casadi.nlpsol('a_to_q', 'ipopt', nlp, opts) solution = nlp_solver(x0=[0, 0], p=[state['a_1']]) sol_vector = np.array(solution['x']) return {'q_1': {'ry': sol_vector[0]}} ... def closure_q_to_a_group_2(state: Dict[str, float]): nlp = {'x': closure_2_state[1:], 'f': c_2, 'p': closure_2_state[0]} nlp_solver = casadi.nlpsol('q_to_a', 'ipopt', nlp, opts) solution = nlp_solver(x0=[0, 0], p=[state['q_2']['ry']]) sol_vector = np.array(solution['x']) return {'a_2': sol_vector[1]}

def closure_a_to_q_group_2(state: Dict[str, float]): nlp = {'x': closure_2_state[:2], 'f': c_2, 'p': closure_2_state[2]} nlp_solver = casadi.nlpsol('a_to_q', 'ipopt', nlp, opts) solution = nlp_solver(x0=[0, 0], p=[state['a_2']]) sol_vector = np.array(solution['x']) return {'q_2': {'ry': sol_vector[0]}}

closure_group_1 = KinematicGroup(name="geometric group 1", virtual_chain=[virtual_joint_1, link_1], actuated_state={'a_1': 0}, actuated_to_virtual=closure_a_to_q_group_1, virtual_to_actuated=closure_q_to_a_group_1)

closure_group_2 = KinematicGroup(name="geometric group 2", virtual_chain=[virtual_joint_2, link_2], actuated_state={'a_2': 0}, actuated_to_virtual=closure_a_to_q_group_2, virtual_to_actuated=closure_q_to_a_group_2, parent=geometric_group_1)

`