diff --git a/python/examples/.cart_pulling.py.swp b/python/examples/.cart_pulling.py.swp
index e874c004798a168923f4377f425b22573deb4629..46cded145a8cbc25199c3764296de5e366310042 100644
Binary files a/python/examples/.cart_pulling.py.swp and b/python/examples/.cart_pulling.py.swp differ
diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 8115932811e2caa1790f4d9a86a9fec630330b50..3c17d05b33ad9d0e758abdd11de238826ea05dda 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -10,7 +10,7 @@ from ur_simple_control.optimal_control.crocoddyl_mpc import *
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly, controlLoopClikDualArmsOnly
import pinocchio as pin
import crocoddyl
from functools import partial
@@ -88,7 +88,11 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
"""
q = robot.getQ()
- T_w_e = robot.getT_w_e()
+ if robot.robot_name != "yumi"
+ T_w_e = robot.getT_w_e()
+ else:
+ T_w_e_l, T_w_e_right = robot.getT_w_e()
+
# we use binary as string representation (i don't want to deal with python's binary representation).
# the reason for this is that then we don't have disgusting nested ifs
# TODO: this has to have named entries for code to be readable
@@ -134,7 +138,12 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
#controlLoopClik(robot, invKinmQP, i, past_data)
clikController = partial(dampedPseudoinverse, 1e-3)
#controlLoopClik(robot, clikController, i, past_data)
- controlLoopClikArmOnly(robot, clikController, i, past_data)
+ if robot.robot_name != "yumi":
+ controlLoopClikArmOnly(robot, clikController, i, past_data)
+ else:
+ # TODO: DEFINE SENSIBLE TRANSFOR
+ goal_transform = pin.SE3.Identity()
+ controlLoopClikDualArmsOnly(robot, clikController, goal_transform, i, past_data)
# case 2)
# MASSIVE TODO:
@@ -207,10 +216,16 @@ def cartPulling(args, robot : RobotManager, goal, path_planner):
log_item = {}
q = robot.getQ()
- T_w_e = robot.getT_w_e()
+ if robot.robot_name != "yumi":
+ T_w_e = robot.getT_w_e()
+ else:
+ T_w_e_l, T_w_e_right = robot.getT_w_e()
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
- save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+ #T_base = self.robot_manager.data.oMi[1]
+ # NOTE: why the fuck was the past path defined from the end-effector?????
+ #save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+ save_past_item = {'path2D_untimed' : q[:2],
'priority_register' : ['0','1','1']}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_item, log_item)
loop_manager.run()
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
deleted file mode 100644
index f08ae83e3367ae7983ea04cac5959c3c261c03e9..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/.managers.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 5e5ec88f0d4c288ad7a7c164efadf74fe092bd31..6f831d73c09b597dd6d5e50e7302edd43fd2bafb 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -590,6 +590,45 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=',', fmt='%.5f')
return joint_trajectory
+def controlLoopClikDualArmsOnly(robot : RobotManager, clik_controller, goal_transform, i, past_data)
+ """
+ controlLoopClikDualArmsOnly
+ ---------------
+ """
+ breakFlag = False
+ log_item = {}
+ q = robot.getQ()
+ T_w_e_left, T_w_e_right = robot.getT_w_e()
+ #
+ Mgoal_left = robot.Mgoal.act(goal_transform)
+ Mgoal_right = robot.Mgoal.act(goal_transform.inverse())
+
+ SEerror_left = T_w_e_left.actInv(Mgoal_left)
+ SEerror_right = T_w_e_right.actInv(Mgoal_right)
+
+ err_vector_left = pin.log6(SEerror_left).vector
+ err_vector_right = pin.log6(SEerror_right).vector
+
+ if (np.linalg.norm(err_vector_left) < robot.args.goal_error) and (np.linalg.norm(err_vector_right) < robot.args.goal_error):
+ breakFlag = True
+ J_left = pin.computeFrameJacobian(robot.model, robot.data, q, robot.l_ee_frame_id)
+ J_left = J_left[:,3:]
+ J_right = pin.computeFrameJacobian(robot.model, robot.data, q, robot.r_ee_frame_id)
+ J_right = J_right[:,3:]
+
+
+ # compute the joint velocities based on controller you passed
+ qd_left = clik_controller(J_left, err_vector_left)
+ qd_right = clik_controller(J_right, err_vector_right)
+ qd = qd_left + qd_right
+ robot.sendQd(qd)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item['dqs_cmd'] = qd.reshape((robot.model.nv,))
+ # we're not saving here, but need to respect the API,
+ # hence the empty dict
+ return breakFlag, {}, log_item
if __name__ == "__main__":
args = get_args()
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
deleted file mode 100644
index c17dbc853afbb73baf10fc8e0ad325f99980e80e..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp and /dev/null differ
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 d6e5ce7afad91b54068d7d5c3f91495151cf07b2..0000000000000000000000000000000000000000
Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp and /dev/null differ