diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py index 1cabaa0e29c10c4b58049881bc1ae2c6f2c829ff..8414bc09ac264f10dbb95dcb77af4a0daa80ba0e 100644 --- a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling.py +++ b/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) ################################################### diff --git a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py b/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py index 41a1f7c909d53b905a2fdd695ad8dd2355431779..8379bcb0c958158968c16b491241f80757c8daea 100644 --- a/examples/cart_pulling/disjoint_control/mpc_base_clik_single_arm.py +++ b/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]}) diff --git a/python/smc/path_generation/path_math/cart_pulling_path_math.py b/python/smc/path_generation/path_math/cart_pulling_path_math.py index 1a1e54da2a446bc1abb2ac77fe8b294304087859..a8b89773e9096b23878878cd9fc65d49f163983b 100644 --- a/python/smc/path_generation/path_math/cart_pulling_path_math.py +++ b/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