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: