From c5eea5ccb43f71434272f9c4fe91dc0cc248ef8b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 12 May 2025 18:31:36 +0200
Subject: [PATCH] lugre on pendulum progress
---
.../gripper_and_object_croco_model.py | 3 +
.../crocoddyl/multibody_with_lugre_state.py | 64 ++++++++++++++++
.../crocoddyl/pendulum_croco_models.py | 74 ++++++++++++-------
.../regrasping/crocoddyl/pendulum_regrasp.py | 9 ++-
examples/regrasping/crocoddyl/regrasp.py | 1 +
5 files changed, 124 insertions(+), 27 deletions(-)
create mode 100644 examples/regrasping/crocoddyl/multibody_with_lugre_state.py
diff --git a/examples/regrasping/crocoddyl/gripper_and_object_croco_model.py b/examples/regrasping/crocoddyl/gripper_and_object_croco_model.py
index 19b118d..5f2cb68 100644
--- a/examples/regrasping/crocoddyl/gripper_and_object_croco_model.py
+++ b/examples/regrasping/crocoddyl/gripper_and_object_croco_model.py
@@ -6,6 +6,8 @@ import crocoddyl
from friction.friction import ReducedFrictionModel
from pin_models import isGripperInContact, findInHandContactPoints
+#class
+
# TODO: make it so that you switch between having an arm or just the end-effector
# in the model with an argument.
# TODO: make friction models switchable with an argument
@@ -291,6 +293,7 @@ class ActuationModelRegrasp(crocoddyl.ActuationModelAbstract):
# TODO: possibly not necessary to do this computation here, but let's worry about that later
mu_tors = self.args.beta * self.args.contact_radius * self.args.mu_s
# fx, fy, tauz
+ # NOTE: this is missing the force due to other joints' acceleration
f_d_and_g = pin.nle(
self.model, self.joint_data, x[: self.nq], x[self.nq :]
)[-3:]
diff --git a/examples/regrasping/crocoddyl/multibody_with_lugre_state.py b/examples/regrasping/crocoddyl/multibody_with_lugre_state.py
new file mode 100644
index 0000000..702de0c
--- /dev/null
+++ b/examples/regrasping/crocoddyl/multibody_with_lugre_state.py
@@ -0,0 +1,64 @@
+import crocoddyl
+import numpy as np
+
+
+# NOTE: could be i'm missing some member functions (or even attributes)
+# here, but i'll only add them if stuff doesn't work out
+class MultiBodyWithLugreState(crocoddyl.StateAbstract):
+ # we're assuming a singular lugre contact
+ def __init__(self, state_multibody):
+ self.state_multibody = state_multibody
+ # NOTE: all these throw an error for some reason
+ # self.nx = state_multibody.nx + 1 # manifold embedding dim
+ # self.ndx = state_multibody.ndx + 1 # tangent space dim
+ # self.nq = state_multibody.nq
+ # self.nv = state_multibody.nv
+
+ super().__init__(state_multibody.nx + 1, state_multibody.ndx + 1)
+
+ def zero(self):
+ return np.append(self.state_multibody.zero(), 0.0)
+
+ def rand(self):
+ return np.concatenate([self.state_multibody.rand(), np.random.random() - 0.5])
+
+ def diff(self, x0, x1):
+ rez = np.zeros(self.ndx)
+ rez[:-1] = self.state_multibody.diff(x0[:-1], x1[:-1])
+ rez[-1] = x1[-1] - x0[-1] # this is just euclidean
+ return rez
+
+ # there's an optional argument firstsecond,
+ # which let's you specify whether you're calculating the jacobian
+ # from x0 to x1, or the other direction.
+ # by default both are computed
+ def Jdiff(self, x0, x1):
+ J1 = np.zeros((self.ndx, self.nxd))
+ J2 = np.zeros((self.ndx, self.nxd))
+ (J1[:-1, :-1], J2[:-1, :-1]) = self.state_multibody.Jdiff(
+ x0[: self.nx - 1], x1[: self.nx - 1]
+ )
+ # only changes itself, and is euclidean, hence only 1 on the diagonal
+ J1[-1, -1] = 1.0
+ J2[-1, -1] = 1.0
+ return (J1, J2)
+
+ # same comment as on Jdiff
+ def Jintegrate(self, x, dx):
+ J1 = np.zeros((self.ndx, self.nxd))
+ J2 = np.zeros((self.ndx, self.nxd))
+ (J1[:-1, :-1], J2[:-1, :-1]) = self.state_multibody.Jintegrate(
+ x[: self.nx - 1], dx[: self.ndx - 1]
+ )
+ # only changes itself, and is euclidean, hence only 1 on the diagonal
+ J1[-1, -1] = 1.0
+ J2[-1, -1] = 1.0
+ return (J1, J2)
+
+
+def main():
+ import example_robot_data
+
+ pendulum = example_robot_data.load("double_pendulum")
+ state = crocoddyl.StateMultibody(pendulum.model)
+ state_l = MultiBodyWithLugreState(state)
diff --git a/examples/regrasping/crocoddyl/pendulum_croco_models.py b/examples/regrasping/crocoddyl/pendulum_croco_models.py
index ed1360f..d09af42 100644
--- a/examples/regrasping/crocoddyl/pendulum_croco_models.py
+++ b/examples/regrasping/crocoddyl/pendulum_croco_models.py
@@ -59,6 +59,8 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
self.data_pin = data_pin
self.mu_tors = self.args.beta * self.args.contact_radius * self.args.mu_s
self.sticking_condition = True
+ self.z_dot = 0.0
+ self.g_v = 0.0
# u = [tau_1, fn]
@@ -69,39 +71,61 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
# but what is tau[1]?
# well, if there's sticking, then v[1] = 0.
# if v[1] > 0, then we're already sliding so there's nothing to check.
- self.sticking_condition = True
- if np.abs(x[3]) > 0.0:
- self.sticking_condition = False
- # if there was sticking before, then a[1] = 0.
- # now if this is the onset of sliping, it won't be.
- # NOTE: but for the purposes of this calculation it is, right?
- if (np.abs(x[3]) < 1e-1) or self.sticking_condition:
- M = pin.crba(self.model_pin, self.data_pin, x[:2])
- M_inv = np.linalg.inv(M)
- data.nle_tau = pin.nle(self.model_pin, self.data_pin, x[:2], x[2:])
- tau2 = -1 * (M_inv[1, 0] * u[0] - data.nle_tau[1]) / M_inv[1, 1]
- if np.abs(tau2) > self.mu_tors * u[1]:
+ if self.args.friction_model == "coulomb":
+ self.sticking_condition = True
+ if np.abs(x[3]) > 0.0:
self.sticking_condition = False
+ # if there was sticking before, then a[1] = 0.
+ # now if this is the onset of sliping, it won't be.
+ # NOTE: but for the purposes of this calculation it is, right?
+ if (np.abs(x[3]) < 1e-1) or self.sticking_condition:
+ M = pin.crba(self.model_pin, self.data_pin, x[:2])
+ M_inv = np.linalg.inv(M)
+ data.nle_tau = pin.nle(self.model_pin, self.data_pin, x[:2], x[2:])
+ tau2 = -1 * (M_inv[1, 0] * u[0] - data.nle_tau[1]) / M_inv[1, 1]
+ if np.abs(tau2) > self.mu_tors * u[1]:
+ self.sticking_condition = False
+ else:
+ self.sticking_condition = True
+
+ if self.sticking_condition:
+ data.tau[1] = tau2
else:
- self.sticking_condition = True
-
- if self.sticking_condition:
- data.tau[1] = tau2
- else:
- data.tau[1] = -1 * np.sign(x[3]) * self.mu_tors * u[1]
- # data.tau[1] = np.clip(data.tau[1], -10, 10)
- # print(data.tau[1], x[3], self.sticking_condition)
- if not self.args.control_gripping_force:
- data.tau[1] = 0.0
+ data.tau[1] = -1 * np.sign(x[3]) * self.mu_tors * u[1]
+ # data.tau[1] = np.clip(data.tau[1], -10, 10)
+ # print(data.tau[1], x[3], self.sticking_condition)
+ if not self.args.control_gripping_force:
+ data.tau[1] = 0.0
+ if self.args.friction_model == "lugre":
+ """
+ s(v) = F_C + (F_S - F_C) exp(-(v/v_s)**delta_vs)
+ z_dot = v - ((sigma_0 * abs(v)) / (s(v))) * z
+ F = sigma_0 * z + sigma_1 * z_dot + sigma_2 * v
+ """
+ self.g_v = (
+ u[-1]
+ * (self.args.mu_c + (self.args.mu_s - self.args.mu_c))
+ * np.exp(-1 * (x[3] / self.args.v_s) ** 2)
+ )
+ self.z_dot = x[3] - ((self.args.sigma_0 * np.abs(x[3])) / self.g_v) * x[-1]
+ F_f = (
+ self.args.sigma_0 * x[-1]
+ + self.args.sigma_1 * self.z_dot
+ + self.args.sigma_2 * x[-1]
+ )
+ data.tau[1] = F_f
# calc is always called first
def calcDiff(self, data, x, u):
if not self.args.control_gripping_force:
return
- if self.sticking_condition:
+ if self.args.friction_model == "coulomb":
+ if self.sticking_condition:
+ data.dtau_du[1, 1] = 0.0
+ else:
+ data.dtau_du[1, 1] = -1 * np.sign(x[3]) * self.mu_tors
+ if self.args.friction_model == "lugre":
data.dtau_du[1, 1] = 0.0
- else:
- data.dtau_du[1, 1] = -1 * np.sign(x[3]) * self.mu_tors
pin.aba(self.model_pin, self.data_pin, x[:2], x[2:], data.tau)
pin.computeRNEADerivatives(
diff --git a/examples/regrasping/crocoddyl/pendulum_regrasp.py b/examples/regrasping/crocoddyl/pendulum_regrasp.py
index 0fa773c..d9c61a1 100644
--- a/examples/regrasping/crocoddyl/pendulum_regrasp.py
+++ b/examples/regrasping/crocoddyl/pendulum_regrasp.py
@@ -3,12 +3,11 @@ from pendulum_croco_models import (
CostModelDoublePendulum,
)
from utils import get_args, computeStickingCondition
+from multibody_with_lugre_state import MultiBodyWithLugreState
import matplotlib.pyplot as plt
import crocoddyl
-import os
import signal
-import sys
import time
import example_robot_data
import numpy as np
@@ -28,6 +27,8 @@ model = pendulum.model
data = pendulum.data
state = crocoddyl.StateMultibody(model)
+if args.friction_model == "lugre":
+ state = MultiBodyWithLugreState(state)
# this is the first link
actuation = ActuationModelDoublePendulum(state, args, pendulum.model, pendulum.data)
@@ -50,6 +51,10 @@ runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
terminalCostModel.addCost("xGoal", xPendCost, 100.0)
+# NOTE: this can't take anything but multibodystate
+# TODO: just start implementing all this shit in c++ already,
+# we've seen enough python and source code to know what's going on
+# (or at least it won't be any better with more prep...)
runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actuation, runningCostModel
diff --git a/examples/regrasping/crocoddyl/regrasp.py b/examples/regrasping/crocoddyl/regrasp.py
index 57a3371..dd2c866 100644
--- a/examples/regrasping/crocoddyl/regrasp.py
+++ b/examples/regrasping/crocoddyl/regrasp.py
@@ -13,6 +13,7 @@ import argparse
import time
# TODO: 1) inherit from numdiff everywhere
+# --> can't, they not exported!
# TODO: 2) put in gravity compensation into the actuation model
--
GitLab