done 4 today

examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py

@@ -72,7 +72,10 @@ if __name__ == "__main__":
     ################################################
     # 2. construct initial T_w_abs (starting middle of handlebar pose)
     ################################################
-    pathSE3 = path2D_to_SE3(path_base, args.handlebar_height)
+    pathSE3 = path2D_to_SE3(path_base, args.handlebar_height, np.pi)
+    init_rot = pathSE3[0].rotation @ pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0)
+    init_rot = init_rot @ pin.rpy.rpyToMatrix(0.0, 0.0, np.pi)
+    # T_w_absgoal = constructInitialT_w_abs(args, path_base, init_rot)
     T_w_absgoal = constructInitialT_w_abs(args, path_base, pathSE3[0].rotation)
 
     if args.visualizer:
@@ -105,9 +108,10 @@ if __name__ == "__main__":
     robot.mode = robot.control_mode.base_only
     # NOTE: T_w_absgoal's x-axis points toward the path.
     # z-axis points down by an undocumented choice
-    rotation_init_rpy = pin.rpy.matrixToRpy(T_w_absgoal.rotation)
-    rotation_init = pin.rpy.rpyToMatrix(0.0, 0.0, rotation_init_rpy[2] + np.pi / 2)
-    T_w_bgoal = pin.SE3(rotation_init, p_basegoal)
+    # rotation_init_rpy = pin.rpy.matrixToRpy(T_w_absgoal.rotation)
+    # rotation_init = pin.rpy.rpyToMatrix(0.0, 0.0, rotation_init_rpy[2] - np.pi / 2)
+    # T_w_bgoal = pin.SE3(rotation_init, p_basegoal)
+    T_w_bgoal = pin.SE3(init_rot, p_basegoal)
     moveL(args, robot, T_w_bgoal)
 
     ###################################################

examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py

@@ -74,7 +74,7 @@ if __name__ == "__main__":
 
     _, path2D = data
     path2D = np.array(path2D).reshape((-1, 2))
-    pathSE3 = path2D_to_SE3(path2D, args.handlebar_height)
+    pathSE3 = path2D_to_SE3(path2D, args.handlebar_height, np.pi)
     if args.visualizer:
         # TODO: document this somewhere
         robot.visualizer_manager.sendCommand({"Mgoal": pathSE3[0]})

python/smc/path_generation/path_math/cart_pulling_path_math.py

@@ -123,5 +123,5 @@ def construct_EE_path(
     )
 
     # step (4)
-    ee_SE3trajectory = path2D_to_SE3(ee_2Dtrajectory, args.handlebar_height)
+    ee_SE3trajectory = path2D_to_SE3(ee_2Dtrajectory, args.handlebar_height, np.pi)
     return ee_SE3trajectory