diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 3faa03fd887c0f00fbb5baa9c001cc45b08dc145..8115932811e2caa1790f4d9a86a9fec630330b50 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
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse, controlLoopClikArmOnly
import pinocchio as pin
import crocoddyl
from functools import partial
@@ -56,6 +56,26 @@ def isGraspOK(args, robot, grasp_pose):
isOK = True
return not isOK
+
+def isGripperRelativeToBaseOK(args, robot):
+ isOK = False
+ # we want to be in the back of the base (x-axis) and on handlebar height
+ T_w_base = robot.data.oMi[1]
+ # rotation for the gripper is base with z flipped to point into the ground
+ rotate = pin.SE3(pin.rpy.rpyToMatrix(np.pi, 0.0, 0.0), np.zeros(3))
+ # translation is prefered distance from base
+ translate = pin.SE3(np.eye(3), np.array([args.base_to_handlebar_preferred_distance, 0.0, args.handlebar_height]))
+ #grasp_pose = T_w_base.act(rotate.act(translate))
+ grasp_pose = T_w_base.act(translate.act(rotate))
+ SEerror = robot.getT_w_e().actInv(grasp_pose)
+ err_vector = pin.log6(SEerror).vector
+ # TODO: figure this out
+ # it seems you have to use just the arm to get to finish with this precision
+ #if np.linalg.norm(err_vector) < robot.args.goal_error:
+ if np.linalg.norm(err_vector) < 2*1e-1:
+ isOK = True
+ return isOK, grasp_pose
+
def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solver_pulling,
path_planner : ProcessManager, i : int, past_data):
"""
@@ -75,17 +95,14 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
priority_register = ['0','1','1']
# TODO implement this based on laser scanning or whatever
#priority_register[0] = str(int(areObstaclesTooClose()))
+ graspOK, grasp_pose = isGripperRelativeToBaseOK(args, robot)
+ # NOTE: this keeps getting reset after initial grasp has been completed.
+ # and we want to let mpc cook
+ priority_register[1] = str(int(not graspOK)) # set if not ok
# TODO: get grasp pose from vision, this is initial hack to see movement
- if i < 300:
- rotation = np.eye(3)
- rotation[1][1] = -1.0
- rotation[2][2] = -1.0
- translation = np.array([10.0 + args.base_to_handlebar_preferred_distance, 4.0, args.handlebar_height])
- grasp_pose = pin.SE3(rotation, translation)
- priority_register[1] = str(int(isGraspOK(args, robot, grasp_pose)))
- else:
- grasp_pose = robot.getT_w_e()
- priority_register[1] = '0'
+# if i > 1000:
+# priority_register[1] = '0'
+
# interpret string as base 2 number, return int in base 10
priority_int = ""
for prio in priority_register:
@@ -116,7 +133,8 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
else:
#controlLoopClik(robot, invKinmQP, i, past_data)
clikController = partial(dampedPseudoinverse, 1e-3)
- controlLoopClik(robot, clikController, i, past_data)
+ #controlLoopClik(robot, clikController, i, past_data)
+ controlLoopClikArmOnly(robot, clikController, i, past_data)
# case 2)
# MASSIVE TODO:
@@ -126,18 +144,23 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
# whether we're transitioning from cliking to pulling
# this is the time and place to populate the past path from the pulling to make sense
if past_data['priority_register'][-1][1] == '1' and priority_register[1] == '0':
- #if True:
- # this is not the right spacing
- # TODO: this ain't right
+ # create straight line path from ee to base
p_cart = q[:2]
p_ee = T_w_e.translation[:2]
straigh_line_path = np.linspace(p_ee, p_cart, args.past_window_size)
+ # time it the same way the base path is timed
+ time_past = np.linspace(0.0, args.past_window_size * robot.dt, args.past_window_size)
+ s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.past_window_size)
+ path2D_handlebar = np.hstack((
+ np.interp(s, time_past, straigh_line_path[:,0]).reshape((-1,1)),
+ np.interp(s, time_past, straigh_line_path[:,1]).reshape((-1,1))))
+
past_data['path2D_untimed'].clear()
- past_data['path2D_untimed'].extend(straigh_line_path[i] for i in range(args.past_window_size))
+ past_data['path2D_untimed'].extend(path2D_handlebar[i] for i in range(args.past_window_size))
if priority_int < 2:
# TODO make this one work
- breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
+ breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, grasp_pose, i, past_data)
#BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
p = q[:2]
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 5fc227209086cfad64e7d419604fa8ab43afd51a..a9eb3f1f591c6d984b1594473aff140ff612f947 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -187,6 +187,31 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
# hence the empty dict
return breakFlag, {}, log_item
+def controlLoopClikArmOnly(robot, clik_controller, i, past_data):
+ breakFlag = False
+ log_item = {}
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ # first check whether we're at the goal
+ SEerror = T_w_e.actInv(robot.Mgoal)
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < robot.args.goal_error:
+ breakFlag = True
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ # cut off base from jacobian
+ J = J[:,3:]
+ # compute the joint velocities based on controller you passed
+ qd = clik_controller(J, err_vector)
+ # add the missing velocities back
+ qd = np.hstack((np.zeros(3), qd))
+ 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
def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controller, i, past_data):
"""
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 26571a05dfcbcc577633cc6e28e9acc5fedbaaa2..65d3858af6cf57d6cd82e4117b1373ae180cc5e5 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -213,7 +213,8 @@ def CrocoEndEffectorPathFollowingMPC(args, robot, x0, path_planner):
loop_manager.run()
-def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, iter_n, past_data):
+def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager,
+ grasp_pose, iter_n, past_data):
"""
CrocoPathFollowingMPCControlLoop
-----------------------------
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 3060ceb54148de68f2629836e7769c1fb7d51163..c7d2828b857f6c62dbc4e3fcb8ca9a3e3e497834 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -498,14 +498,14 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
# NOTE: this should be done outside of this function to clarity
state.nv)
eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
- runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, 1e2)
+ runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
state,
robot.model.getFrameId("mobile_base"),
path_base[i],
state.nv)
baseTrackingCost = crocoddyl.CostModelResidual(state, baseTranslationResidual)
- runningCostModel.addCost("base_translation" + str(i), baseTrackingCost, 1e2)
+ runningCostModel.addCost("base_translation" + str(i), baseTrackingCost, args.base_translation_cost)
if args.solver == "boxfddp":
runningModel = crocoddyl.IntegratedActionModelEuler(
@@ -543,8 +543,8 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
0.0,
)
- terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, 1e2)
- terminalCostModel.addCost("base_translation" + str(args.n_knots), baseTrackingCost, 1e2)
+ terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, args.ee_pose_cost)
+ terminalCostModel.addCost("base_translation" + str(args.n_knots), baseTrackingCost, args.base_translation_cost)
# now we define the problem
problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
diff --git a/python/ur_simple_control/optimal_control/get_ocp_args.py b/python/ur_simple_control/optimal_control/get_ocp_args.py
index 97d87a1f98817979448d3c57ec6fe1304ffd13c7..177b2cf490781465d61cf999a40efd06aaa6235c 100644
--- a/python/ur_simple_control/optimal_control/get_ocp_args.py
+++ b/python/ur_simple_control/optimal_control/get_ocp_args.py
@@ -12,4 +12,8 @@ def get_OCP_args(parser : argparse.ArgumentParser):
help="optimal control problem solver you want", choices=["boxfddp", "csqp"])
parser.add_argument("--max-solver-iter", type=int, default=200, \
help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc")
+ parser.add_argument("--ee-pose-cost", type=float, default=1e2, \
+ help="ee pose cost in pulling mpc")
+ parser.add_argument("--base-translation-cost", type=float, default=1e2, \
+ help="base translation cost in pulling mpc")
return parser
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 0fc4397867432a4ab28e1eaa7818cda03dcf7f9c..9d63181379b28f43c03be7bf490f0e9d9361937d 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -578,6 +578,7 @@ def path2D_to_SE3(path2D, path_height):
pathSE3 = []
for i in range(len(path2D) - 1):
# first set the x axis to be in the theta direction
+ # TODO: make sure this one makes sense
rotation = np.array([
[-np.cos(thetas[i]), np.sin(thetas[i]), 0.0],
[-np.sin(thetas[i]), -np.cos(thetas[i]), 0.0],