From 9379492bead3f1dd7eef8295ee087d2af599bd79 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 15:55:57 +0100
Subject: [PATCH] cm
---
.../optimal_control/.crocoddyl_mpc.py.swp | Bin 32768 -> 32768 bytes
.../.crocoddyl_optimal_control.py.swp | Bin 53248 -> 53248 bytes
.../optimal_control/crocoddyl_mpc.py | 6 +++++-
.../crocoddyl_optimal_control.py | 14 +++++++++-----
4 files changed, 14 insertions(+), 6 deletions(-)
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
index 7afae0d2bef6f7304be8900bbdbf553c2ca61646..6cd696f6d4eab96ea05e9f6248c28ce6b8b91ccf 100644
GIT binary patch
delta 202
zcmZo@U}|V!5=$}&^Ym4)&@*CSU|<knU?>fXNM9<nQLIXUQFil0fnt5$bWR3_@0<`>
z*~x+q>OsZ43=A)L7#QyHFfau2Ffbh9W?(qX&A`yb&A^b$&A<@A&A_0=&A_0^&A`CN
z&A`CP&A@P-i-BPV7Xw2k7Xw2E7XyPH7XyPD7Xt$$7s&L@j4ZjFo1MHbGEV0AwVqty
w$2B>~*8{|6%88%s<r~12np2!=#h{>|pfH)=w|nxO5ar3sf>&>L_1kR-0Glr><p2Nx
delta 145
zcmZo@U}|V!5=$}&^Ym4)&@*CSU|<knU^pBSk$!@Aqga&yqvYm^0>%2ghqxFRoVX#f
zl9L4;)SdWw85sC@85pkeFfi=kVPKfe!@y9=!@y9&!@v;9!@v;2!@!`#!@%&En}J~q
sHv_|FZU%-*ZU%-zZUzP)Zjfo48CgznZT{|gk#Vzpuo2^CSHInc04mHKXaE2J
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
index 0ee405b879f06e30223bd6cc93ebbd3579b14418..66611dacb8ead0c62400d66d3a0c15fbbddc7fd1 100644
GIT binary patch
delta 371
zcmZozz}&EaSv1KY%+puFLeGeSfq_AQfgv_5B7OcwQE5R&x6Ouvm-|@tI2jlYP3Bw>
zuKSmpfnhE;14B4B1A`4W0|Ns$1H)D>28JnI3=Cyl3=Aq<3=Bs&85j<ObZqWjV97c8
z%UqSo>sAV|Yh-HbC=|s{HkhN$rl6nzVg{}&+q`C#0V7imLO3UWa^SkM$!k_|Z|+%B
z!OUyV%)n600&$qzWWf#F(^hjZFt~FtFxYZ1FkEG4V8~@>VED<#!0>~Of#C)l14ADh
z149)X14AVn14A4e1A{di1A`<R1H&&?28JW73=B(I85kN_85lBH85j&$85sVuFfhDe
zVPLq-!oYBVg@IuS3&_UJj4ZCqo2M-N$22*}MIPjX$-9>bFhM*pIm|_7@`=>~llLs)
xhd5rHH8rO=)oQY@i^^pFb^Md}EHRp_8!0ilb}sj3gVtrNoAYP&FfKYE007siV|V}n
delta 257
zcmZozz}&EaSv1KY%+puFLeGeSfq_AQf#FeTMEZ=4qSAtlPMZw{FZZ#UaWXKRn#{Q%
zT=zdW1H(dY28L*E1_lRi1_l;x28Nwn3=A{47#OO!7#Os;7#PlQGBBJ5>Db)6z>;(F
zzLk!X4d!T1_FY%CdBrLNMmVQ&O*1oZ01E>{2`j{2r^$jFwuj|$FfjaLXJGir&cJY!
zoq?gBoq?g6oq?f>oq-{qoq@rIoq<7$oq^#u8w0~pHU@@eYzz!dYzz#UYzz#BYzz$l
vSQ!{zvNAASVP#-A$jZR5loe#dW=57Umd#TZ{$rb5&=tJ-zzl2VMF#``7)w9p
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 47aa20a..d20301e 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -338,7 +338,11 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
# idk if that's necessary
solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
- solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+ if robot.robot_name != "yumi":
+ solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+ else:
+ solver.problem.terminalModel.differential.costs.costs['l_ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+ solver.problem.terminalModel.differential.costs.costs['r_ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
solver.solve(xs_init, us_init, args.max_solver_iter)
xs = np.array(solver.xs)
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 4f1ce3c..942dae2 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -516,16 +516,16 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
# MASSIVE TODO: actually put in reference for left arm
path_ee[i],
state.nv)
- eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
- runningCostModel.addCost("l_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+ l_eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("l_ee_pose" + str(i), l_eeTrackingCost, args.ee_pose_cost)
r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
state,
robot.model.getFrameId("robr_joint_7"),
# MASSIVE TODO: actually put in reference for left arm
path_ee[i],
state.nv)
- eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
- runningCostModel.addCost("r_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+ r_eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+ runningCostModel.addCost("r_ee_pose" + str(i), r_eeTrackingCost, args.ee_pose_cost)
baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
@@ -572,7 +572,11 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
0.0,
)
- terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, args.ee_pose_cost)
+ if robot.robot_name != "yumi":
+ terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, args.ee_pose_cost)
+ else:
+ terminalCostModel.addCost("l_ee_pose" + str(args.n_knots), l_eeTrackingCost, args.ee_pose_cost)
+ terminalCostModel.addCost("r_ee_pose" + str(args.n_knots), r_eeTrackingCost, args.ee_pose_cost)
terminalCostModel.addCost("base_translation" + str(args.n_knots), baseTrackingCost, args.base_translation_cost)
# now we define the problem
--
GitLab