Joint Constraints Not Respected during Trajectory Optimization
Opened this issue · 0 comments
chuazh commented
using the psm_fontanelli notebook, I tried to generate an optimal trajectory while setting the joint constraints for joint 1 with a "psm_simplified" model:
from numpy import deg2rad
optimal_trajectory_folder = 'data/optimal_trajectory/'
trajectory_folder = 'data/' + model_name +'/optimal_trajectory/'
base_freq = 0.18
fourier_order = 6
joint_constraints = []
cartesian_constraints = []
if model_name == 'psm_simplified':
q_dvrk7 = 1.2177*q7 - 1.2177*q6
# q_mod6 = (4871*q6)/4000 - (4153*q5)/5000
# q_mod7 = - (4153*q5)/5000 + (4871*q7)/4000
q_dvrk5 = 1.0186 * q5
q_mod6 = -0.8306 * q5 + 1.2178 * q6
q_mod7 = -0.8306 * q5 + 1.2178 * q7
joint_constraints = [(q1, 0, 1.45, -1.7, 1.7), #old value is -1.45 to 1.45
(q2, -0.75, 0.8, -1.7, 1.7),
(q3,0.07,0.235,-0.35,0.35),
(q4,-1.5,1.5,-2,2),
(q_dvrk5,-1.4,1.4,-2,2),
(q_dvrk7, 0.15, 3,-3,3),
(q_mod7, -1.5, 1.5,-2,2),
(q_mod6, -1.5, 1.5,-2,2)]
else:
raise Exception("No robot name matches " + model_name)
traj_optimizer = TrajOptimizer(robot_model, fourier_order, base_freq,
joint_constraints=joint_constraints,
cartesian_constraints = cartesian_constraints)
traj_optimizer.optimize()
however after running the optimization I find that the max and min joint angle 4.6rad. I believe such a large range causes my dvrk to trip as the joint limits are exceeded during the recording of the training data. Hope someone can verify my findings.