diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py index d29008e8b02e99e69ae4171ede21d462252ef8c4..f5f8e14ef2be8fe14c4986eff85da3b837c71940 100644 --- a/python/examples/crocoddyl_mpc.py +++ b/python/examples/crocoddyl_mpc.py @@ -35,6 +35,13 @@ if __name__ == "__main__": # TODO: put this back for nicer demos #Mgoal = robot.defineGoalPointCLI() Mgoal = pin.SE3.Random() + if robot.robot_name == "yumi": + Mgoal.rotation = np.eye(3) + Mgoal_transform = pin.SE3.Identity() + Mgoal_transform.translation[1] = 0.1 + Mgoal_left = Mgoal_transform.act(Mgoal) + Mgoal_right = Mgoal_transform.inverse().act(Mgoal) + Mgoal = (Mgoal_left, Mgoal_right) robot.Mgoal = Mgoal.copy() if args.visualize_manipulator: diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/smc_yumi_challenge.py index 83fe5d3b169afb94ca477a6aa6dd7c2f0bd3f4b9..15f442c291004b2a667954252ebbc3fcbb215bc1 100644 --- a/python/examples/smc_yumi_challenge.py +++ b/python/examples/smc_yumi_challenge.py @@ -10,7 +10,7 @@ from abb_python_utilities.names import get_rosified_name import numpy as np import argcomplete, argparse from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager -from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP +from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP, moveLDualArm from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC @@ -44,11 +44,13 @@ class ROSCommHandlerMYumi(Node): self.loop_manager = loop_manager self.args = args + # give me the latest thing even if there wasn't an update qos_prof = rclpy.qos.QoSProfile( reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE, durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, history = rclpy.qos.HistoryPolicy.KEEP_LAST, depth = 1) + # MASSIVE TODO: you want to be subsribed to a proper localization thing, # specifically to output from robot_localization self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, qos_prof) @@ -143,7 +145,15 @@ def main(args=None): args = get_args() robot = RobotManager(args) goal = robot.defineGoalPointCLI() - loop_manager = CrocoIKMPC(args, robot, goal, run=False) + if robot.robot_name == "yumi": + goal.rotation = np.eye(3) + goal_transform = pin.SE3.Identity() +# goal_transform.translation[1] = 0.1 +# goal_left = goal_transform.act(goal) +# goal_left = goal_transform.inverse().act(goal) +# goal = (goal_left, goal_right) + #loop_manager = CrocoIKMPC(args, robot, goal, run=False) + loop_manager = moveLDualArm(args, robot, goal, goal_transform, run=False) executor = MultiThreadedExecutor() ros_comm_handler = ROSCommHandlerMYumi(args, robot, loop_manager) diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc index 7d1daab77b3d534c89f472a45ca60728517b18a6..45e06998fec1cdb20af8d2e092947181e3091db4 100644 Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp deleted file mode 100644 index 43cc7c57dc1da9d88354597a6af9ed841edb7245..0000000000000000000000000000000000000000 Binary files a/python/ur_simple_control/clik/.clik.py.swp and /dev/null differ diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py index 661f28760546631826832dd520b271cab2dd7ea8..2c0434e5bec720deb4fe6e7709e436607ccbe6f2 100644 --- a/python/ur_simple_control/clik/clik.py +++ b/python/ur_simple_control/clik/clik.py @@ -222,6 +222,9 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id) J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id) # compute the joint velocities based on controller you passed + # this works exactly the way you think it does: + # the velocities for the other arm are 0 + # what happens to the base hasn't been investigated but it seems ok qd_left = clik_controller(J_left, err_vector_left) qd_right = clik_controller(J_right, err_vector_right) qd = qd_left + qd_right @@ -234,6 +237,27 @@ def controlLoopClikDualArm(robot : RobotManager, clik_controller, goal_transform # hence the empty dict return breakFlag, {}, log_item +#def clikDualArm(args, robot, goal, goal_transform, run=True): +# """ +# clikDualArm +# ----------- +# """ +# robot.Mgoal = copy.deepcopy(goal) +# clik_controller = getClikController(args) +# controlLoop = partial(controlLoopClikDualArm, robot, clik_controller, goal_transform) +# # we're not using any past data or logging, hence the empty arguments +# log_item = { +# 'qs' : np.zeros(robot.model.nq), +# 'dqs' : np.zeros(robot.model.nv), +# 'dqs_cmd' : np.zeros(robot.model.nv), +# } +# save_past_dict = {} +# loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item) +# if run: +# loop_manager.run() +# else: +# return loop_manager + def controlLoopClikArmOnly(robot, clik_controller, i, past_data): breakFlag = False log_item = {} @@ -338,7 +362,7 @@ def moveL(args, robot : RobotManager, goal_point): loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item) loop_manager.run() -def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform): +def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform, run=True): """ moveL ----- @@ -357,7 +381,10 @@ def moveLDualArm(args, robot : RobotManager, goal_point, goal_transform): 'dqs_cmd' : np.zeros(robot.model.nv), } loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item) - loop_manager.run() + if run: + loop_manager.run() + else: + return loop_manager # TODO: implement def moveLFollowingLine(args, robot, goal_point): diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py index 3b5c8bf58f00147ff3ab8a80a1c1262b7e5e1655..4b01fb3050a74e37f0ac2dc8236ddf538939d0b1 100644 --- a/python/ur_simple_control/managers.py +++ b/python/ur_simple_control/managers.py @@ -1073,7 +1073,16 @@ class RobotManager: def set_publisher_vel_base(self, publisher_vel_base): self.publisher_vel_base = publisher_vel_base - print("set publisher into robotmanager") + print("set vel_base_publisher into robotmanager") + + def set_publisher_vel_left(self, publisher_vel_left): + self.publisher_vel_left = publisher_vel_left + print("set vel_left_publisher into robotmanager") + + def set_publisher_vel_right(self, publisher_vel_right): + self.publisher_vel_right = publisher_vel_right + print("set vel_right_publisher into robotmanager") + class ProcessManager: """ diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp deleted file mode 100644 index 007e01f6afc624b2a6efd9ea7c340e95ac661700..0000000000000000000000000000000000000000 Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp and /dev/null differ diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py index d20301eb404c1f8456b6ef53cca7ce6de5a7b078..ace01fbfdcdec42516970a64b88f99af6ca253f9 100644 --- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py +++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py @@ -29,12 +29,14 @@ if importlib.util.find_spec('shapely'): # and probably better. # i'm pretty sure that's how croco devs & mim_people do mpc -def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data): +def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, goal, i, past_data): breakFlag = False log_item = {} save_past_dict = {} # check for goal + if robot.robot_name == "yumi": + goal_left, goal_right = goal SEerror = robot.getT_w_e().actInv(robot.Mgoal) err_vector = pin.log6(SEerror).vector if np.linalg.norm(err_vector) < robot.args.goal_error: