Open flbauer opened 1 year ago
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)
`
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)
`