From 0ba3ab8976090a21da498cfc390dd5d9ae665801 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Tue, 10 Dec 2024 16:08:11 +0100
Subject: [PATCH] cm
---
.../.crocoddyl_optimal_control.py.swp | Bin 69632 -> 69632 bytes
.../crocoddyl_optimal_control.py | 4 +++-
2 files changed, 3 insertions(+), 1 deletion(-)
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 d737650ae8977398706a53b075ca34bd36a1186a..d6e5ce7afad91b54068d7d5c3f91495151cf07b2 100644
GIT binary patch
delta 237
zcmZozz|ydQMKsAE%+puFLeGeSfq_AQfx#~_B7OZvQE352v&n`6_Kf123k4?c=XGRe
zU@&EYC^4HXc;Pmq>E^c=0+=Vid?P(Msz!ZsLov(bmv7W2f4HJFS*}}c^8Oat&3sqC
zaq)I>F)%Q3Lv)Hy7JOi>Hl2ro!Iy`D!H0)|L7s<!;T1On!&Ytvh9YhT1`BQm24-%M
zj?IiL6S*dbzLuXnUrBiK`kI!>zHhd3m8IsmW#%R3#1~C2P!!>aFVaykOf}Y={NRlW
VCtPlFVvQ(Ad=5lv^Mf}E$^aitMnV7p
delta 185
zcmZozz|ydQMKsAE%+puFLeGeSfq_AQf#G^YMEd-VqS6A4rjrc?>>0&27Ya<?&)dz+
zz);NsQDQn-@WO4z6`S8)2w<LEFk5Q!_kSvrAHLC>{NakyWVvp&$@^PmH}hTn#>M-9
zi-9488=^~Wvfu-2#RohL44Ziv80vT!7(96x7}$6i7|w7rFihuWU<l=AU<lb<_+S#(
jB*w7G%&+Aq7rfz^T%Z`T+2*wg<K%$1JevjHeoz7cFXlPo
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 3be6cc0..16315d1 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -67,6 +67,7 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+ terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
else:
framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
state,
@@ -100,6 +101,8 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
runningCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
terminalCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
terminalCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+ terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
+ terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
# put these costs into the running costs
runningCostModel.addCost("xReg", xRegCost, 1e-3)
@@ -107,7 +110,6 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# and add the terminal cost, which is the distance to the goal
# NOTE: shouldn't there be a final velocity = 0 here?
terminalCostModel.addCost("uReg", uRegCost, 1e3)
- terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
######################################################################
# state constraints #
--
GitLab