Issues with Initializing and Using Straight Line Cost in Arm Reacher for Trajectory Optimization

[robertkim1221]

I'm currently working on optimizing trajectories using the Arm Reacher module in CuRobo and I've encountered an issue with initializing and using the straight line cost in the optimization process. Here's a detailed breakdown of the problem and the steps I've taken so far:

Problem:
The straight line cost does not seem to be initialized or taken into account during the optimization process. I have tried adding debug statements like print(st_cost) in arm_reacher.py but do not see any output being printed. However, for the pose_cost, I can see the debug prints at every iteration, indicating that it is being used correctly.

Despite the efforts in trying to debug and to understand how to add custom costs, the straight line cost still does not appear to be used during the optimization process. Additionally, adding the straight_line_cfg in the YAML configuration leads to tensor shape mismatch errors.

Goal:
My main goal is to add a custom cost to the cost_fn method. However, with the current issue, I'm unable to proceed effectively as there's no clear documentation or information available online.

Any guidance or suggestions on how to properly initialize and use the straight line cost, or how to add custom costs to the cost_fn method, would be greatly appreciated.

EDIT

I simply changed the vec_weight to [1.0, 1.0, 1.0] from [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] and I was able to use the straight line cost in the optimization. However, I would still appreciate it if someone could guide me with adding new costs to the arm_reacher cost_fn.