From a7b9c049141dcfa9d2830e54f784651a5b65ef39 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 29 Nov 2024 21:44:51 +0100
Subject: [PATCH] fixed underactuation in the base - simple torque constraint
in the y direction which i can't believe i missed before. i didn't even
change the constraint, but simply changed the maximu effort of mobile base y
direction to 0, i.e. did it in the mode. the same will be done for the QP
defining clik
---
python/ur_simple_control/clik/clik.py | 1 +
.../optimal_control/crocoddyl_mpc.py | 2 +-
.../__pycache__/get_model.cpython-312.pyc | Bin 13613 -> 13613 bytes
python/ur_simple_control/util/get_model.py | 4 ++--
.../__pycache__/visualize.cpython-312.pyc | Bin 16632 -> 16632 bytes
5 files changed, 4 insertions(+), 3 deletions(-)
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 3f39f81..40511b8 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -194,6 +194,7 @@ def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controll
if np.linalg.norm(wrench[mask]) > args.contact_detecting_force:
print("hit with", np.linalg.norm(wrench[mask]))
breakFlag = True
+ robot.sendQd(np.zeros(robot.model.nq))
if (args.pinocchio_only) and (i > 500):
print("let's say you hit something lule")
breakFlag = True
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 940acd6..9431fbe 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -180,7 +180,7 @@ def CrocoPathFollowingMPC(args, robot, x0, path):
controlLoop = partial(CrocoPathFollowingMPCControlLoop, args, robot, solver, path)
log_item = {
'qs' : np.zeros(robot.model.nq),
- 'dqs' : np.zeros(robot.model.nq)
+ 'dqs' : np.zeros(robot.model.nv)
}
save_past_dict = {}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index 361d94f551ed5213c11d66ae7a950361e4d7f57f..2e7416863b44cbd0856cb8c0534e2a51249208a6 100644
GIT binary patch
delta 54
zcmZ3RwKj|QG%qg~0|NuYE>*AeiyL`Q%Cj=puw;o$4wMm|{7&A9k$JPB!X|b`jmbSm
K3Y%FpG<gBU9S@xV
delta 56
zcmZ3RwKj|QG%qg~0|Ns?aEp8T<&C^2<tKlUmzexc-f6Os0*GeY?58l5eexV5{>ih9
Lgf_EiXz~I8EcO!r
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 0a2f075..c37f000 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -194,11 +194,11 @@ def heron_approximation():
# there are no position limit by default and that is what we want.
# TODO: put in heron's values
model_mobile_base.velocityLimit[0] = 2
- model_mobile_base.velocityLimit[1] = 2
+ model_mobile_base.velocityLimit[1] = 0
model_mobile_base.velocityLimit[2] = 2
# TODO: i have literally no idea what reasonable numbers are here
model_mobile_base.effortLimit[0] = 200
- model_mobile_base.effortLimit[1] = 200
+ model_mobile_base.effortLimit[1] = 0
model_mobile_base.effortLimit[2] = 200
#print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
#body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index d892f4bb1b092ff28f489a8358275af98997021d..1def7147bf3b5c4ed1bfb7ca66479b3a5142b94a 100644
GIT binary patch
delta 22
dcmey-$oQj?k^3|+FBby?14E^n*GBHQ4ggfD2EhOT
delta 22
dcmey-$oQj?k^3|+FBby?1B0o9%SP_E4ggb329W>&
--
GitLab