Skip to content

Commit

Permalink
Fixed structure for pose traj generation, works with fatrop too; all …
Browse files Browse the repository at this point in the history 
…opti results now match rockit!
  • Loading branch information
@ricburli
ricburli committed Jan 7, 2025
1 parent 658f5a0 commit 0bffc5e
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 30 deletions.
6 changes: 3 additions & 3 deletions examples/trajectory_generation/COMPARING trajectory_generation_fullpose.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ def interpolate_model_invariants(demo_invariants, progress_values):
FSt_start = orthonormalize(optim_calc_results.FSt_frames[current_index])
FSr_start = orthonormalize(optim_calc_results.FSr_frames[current_index])
p_obj_end = optim_calc_results.Obj_pos[-1] + np.array([0.1,0.1,0.1])
rotate = R.from_euler('z', 0, degrees=True)
rotate = R.from_euler('z', 30, degrees=True)
R_obj_end = orthonormalize(rotate.apply(optim_calc_results.Obj_frames[-1]))
FSt_end = orthonormalize(optim_calc_results.FSt_frames[-1])
FSr_end = orthonormalize(optim_calc_results.FSr_frames[-1])
Expand Down Expand Up @@ -162,8 +162,8 @@ def interpolate_model_invariants(demo_invariants, progress_values):
# specify optimization problem symbolically
FS_online_generation_problem_pos = OCP_gen_pos(N=number_samples,w_invars = weights_params['w_invars'][3:])
FS_online_generation_problem_rot = OCP_gen_rot(window_len=number_samples,w_invars = weights_params['w_invars'][:3])
fatrop_online_generation_problem = OCP_gen_pose_fatrop(boundary_constraints, number_samples, solver = 'ipopt')
rockit_online_generation_problem = OCP_gen_pose_rockit(boundary_constraints, number_samples,False)
fatrop_online_generation_problem = OCP_gen_pose_fatrop(boundary_constraints, number_samples, solver = 'fatrop')
rockit_online_generation_problem = OCP_gen_pose_rockit(boundary_constraints, number_samples,True)

initial_values = {
"trajectory": {
Expand Down
88 changes: 61 additions & 27 deletions invariants_py/generate_trajectory/opti_generate_pose_traj_from_vector_invars_fatrop.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,27 @@
from invariants_py.ocp_initialization import generate_initvals_from_constraints_opti
from invariants_py.kinematics.orientation_kinematics import rotate_x
from invariants_py import spline_handler as sh
# import yourdfpy as urdf
# import invariants_py.data_handler as dh

class OCP_gen_pose:

def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = False, solver = 'ipopt'):
def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = False, solver = 'ipopt', robot_params = {}):

# fatrop_solver = check_solver(fatrop_solver)

# # Robot urdf location
# urdf_file_name = robot_params.get('urdf_file_name', None)
# path_to_urdf = dh.find_robot_path(urdf_file_name)
# include_robot_model = True if path_to_urdf is not None else False
# if include_robot_model:
# robot = urdf.URDF.load(path_to_urdf)
# nb_joints = robot_params.get('joint_number', robot.num_actuated_joints)
# q_limits = robot_params.get('q_lim', np.array([robot._actuated_joints[i].limit.upper for i in range(robot.num_actuated_joints)]))
# root = robot_params.get('root', robot.base_link)
# tip = robot_params.get('tip', 'tool0')
# q_init = robot_params.get('q_init', np.zeros(nb_joints))

''' Create decision variables and parameters for the optimization problem '''
opti = cas.Opti() # use OptiStack package from Casadi for easy bookkeeping of variables

Expand All @@ -22,14 +36,23 @@ def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = Fals
R_r = []
X = []
invars = []
# q = []
# qdot = []
for k in range(N):
R_r.append(opti.variable(3,3)) # rotational Frenet-Serret frame
R_obj.append(opti.variable(3,3)) # object orientation
R_t.append(opti.variable(3,3)) # translational Frenet-Serret frame
p_obj.append(opti.variable(3,1)) # object position
R_obj.append(opti.variable(3,3)) # object orientation
R_r.append(opti.variable(3,3)) # rotational Frenet-Serret frame
# if include_robot_model:
# q.append(opti.variable(nb_joints,1))
# X.append(cas.vertcat(cas.vec(R_r[k]), cas.vec(R_obj[k]),cas.vec(R_t[k]), cas.vec(p_obj[k]), cas.vec(q[k])))
# else:
X.append(cas.vertcat(cas.vec(R_r[k]), cas.vec(R_obj[k]),cas.vec(R_t[k]), cas.vec(p_obj[k])))
for k in range(N-1):
invars.append(opti.variable(6,1)) # invariants

if k < N-1:
invars.append(opti.variable(6,1)) # invariants
# if include_robot_model:
# qdot.append(opti.variable(nb_joints,1))

# Define system parameters P (known values in optimization that need to be set right before solving)
h = opti.parameter(1,1) # step size for integration of dynamic model
Expand All @@ -38,6 +61,8 @@ def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = Fals
for k in range(N-1):
invars_demo.append(opti.parameter(6,1)) # model invariants
w_invars.append(opti.parameter(6,1)) # weights for invariants
# if include_robot_model:
# q_lim = opti.parameter(nb_joints,1)

# Boundary values
if "position" in boundary_constraints and "initial" in boundary_constraints["position"]:
Expand All @@ -59,37 +84,46 @@ def __init__(self, boundary_constraints, N = 40, bool_unsigned_invariants = Fals

''' Specifying the constraints '''

# Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator)
opti.subject_to(tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0)
opti.subject_to(tril_vec(R_obj[0].T @ R_obj[0] - np.eye(3)) == 0)
opti.subject_to(tril_vec(R_r[0].T @ R_r[0] - np.eye(3)) == 0)

# if include_robot_model:
# for i in range(nb_joints):
# # ocp.subject_to(-q_lim[i] <= (q[i] <= q_lim[i])) # This constraint definition does not work with fatrop, yet
# opti.subject_to(-q_lim[i] - q[i] <= 0 )
# opti.subject_to(q[i] - q_lim[i] <= 0)

# Dynamic constraints
integrator = dynamics.define_integrator_invariants_pose(h)
# integrator = dynamics.define_integrator_invariants_pose(h,include_robot_model)
for k in range(N-1):
# Integrate current state to obtain next state (next rotation and position)
Xk_end = integrator(X[k],invars[k],h)

# Gap closing constraint
opti.subject_to(X[k+1]==Xk_end)

if k == 0:
# Constrain rotation matrices to be orthogonal (only needed for one timestep, property is propagated by integrator)
opti.subject_to(tril_vec(R_t[0].T @ R_t[0] - np.eye(3)) == 0)
opti.subject_to(tril_vec(R_obj[0].T @ R_obj[0] - np.eye(3)) == 0)
opti.subject_to(tril_vec(R_r[0].T @ R_r[0] - np.eye(3)) == 0)
# Boundary constraints
if "position" in boundary_constraints and "initial" in boundary_constraints["position"]:
opti.subject_to(p_obj[0] == p_obj_start)
if "position" in boundary_constraints and "initial" in boundary_constraints["position"]:
opti.subject_to(p_obj[0] == p_obj_start)
if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]:
opti.subject_to(tril_vec_no_diag(R_obj[0].T @ R_obj_start - np.eye(3)) == 0.)
if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]:
opti.subject_to(tril_vec_no_diag(R_t[0].T @ R_t_start - np.eye(3)) == 0.)
if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]:
opti.subject_to(tril_vec_no_diag(R_r[0].T @ R_r_start - np.eye(3)) == 0.)

if "position" in boundary_constraints and "final" in boundary_constraints["position"]:
opti.subject_to(p_obj[-1] == p_obj_end)
if "orientation" in boundary_constraints and "initial" in boundary_constraints["orientation"]:
opti.subject_to(tril_vec_no_diag(R_obj[0].T @ R_obj_start - np.eye(3)) == 0.)
if "orientation" in boundary_constraints and "final" in boundary_constraints["orientation"]:
opti.subject_to(tril_vec_no_diag(R_obj[-1].T @ R_obj_end - np.eye(3)) == 0.)
if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["translational"]:
opti.subject_to(tril_vec_no_diag(R_t[0].T @ R_t_start - np.eye(3)) == 0.)
if "moving-frame" in boundary_constraints and "translational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["translational"]:
opti.subject_to(tril_vec_no_diag(R_t[-1].T @ R_t_end - np.eye(3)) == 0.)
if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "initial" in boundary_constraints["moving-frame"]["rotational"]:
opti.subject_to(tril_vec_no_diag(R_r[0].T @ R_r_start - np.eye(3)) == 0.)
if "moving-frame" in boundary_constraints and "rotational" in boundary_constraints["moving-frame"] and "final" in boundary_constraints["moving-frame"]["rotational"]:
opti.subject_to(tril_vec_no_diag(R_r[-1].T @ R_r_end - np.eye(3)) == 0.)

# Dynamic constraints
integrator = dynamics.define_integrator_invariants_pose(h)
for k in range(N-1):
# Integrate current state to obtain next state (next rotation and position)
Xk_end = integrator(X[k],invars[k],h)

# Gap closing constraint
opti.subject_to(Xk_end==X[k+1])

''' Specifying the objective '''

Expand Down Expand Up @@ -298,7 +332,7 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,
step_size = 0.1

# Create an instance of OCP_gen_pos
ocp = OCP_gen_pose(boundary_constraints,solver='ipopt', N=N)
ocp = OCP_gen_pose(boundary_constraints,solver='fatrop', N=N)

# Call the generate_trajectory function
invariants, calculated_trajectory_pos, calculated_trajectory_rot, calculated_movingframe_pos, calculated_movingframe_rot = ocp.generate_trajectory(invariant_model, boundary_constraints, step_size)
Expand Down

0 comments on commit 0bffc5e

Please sign in to comment.