diff --git a/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py b/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py
new file mode 100644
index 0000000000000000000000000000000000000000..a76255a9edeb29a710572192ed5f74a0e8b0cf80
--- /dev/null
+++ b/examples/regrasping/crocoddyl/pendulum/fwd_sim_pendulum_scipy.py
@@ -0,0 +1,159 @@
+import crocoddyl
+from pendulum_friction_computation import pendulum_coulomb, pendulum_lugre
+
+import numpy as np
+import pinocchio as pin
+from argparse import Namespace
+from scipy.integrate import solve_ivp
+
+
+def stateEquation(
+ t: float, # required by solver API
+ x: np.ndarray, # required by solver API
+ # our arguments from here on
+ args: Namespace,
+ i,
+ mu_tors: float,
+ model_pin: pin.Model,
+ data_pin: pin.Data,
+ data_actuation: crocoddyl.ActuationDataAbstract,
+ u: np.ndarray,
+ t_from_ocp: np.ndarray,
+ history: dict[str, np.ndarray],
+):
+ """
+ stateEquation
+ ----------------
+ the ode solver calls stateEquation to integrate x_dot = f(t, x).
+ hence the problem needs to be written in state-space from.
+ inputs are:
+ - t - passed by the solver because it does non-homogeneous steps
+ - x - system state (everything that changes with time)
+ outputs are:
+ - x_dot - first derivative
+ Because the solver will call stateEquation many times to estimate derivatives,
+ we can't store results as it runs.
+ Instead, we re-run the equations with the solved states
+ and re-calculate the non-state quantities we want to save.
+ the hacky history arguments lets us do this by calling stateEquation,
+ which is much better than copy-pasting and slightly changing stateEquation.
+
+ state -> TODO: investigate how storing theta as ctheta, stheta can be forced to work to clean up the mess
+ -----
+ 0 : 3 - pendulum rotatational joint angles
+ 4 - lugre state variable
+
+
+ controller - list of grasping forces, provided by ocp solver.
+ there should be an option between holding them constant,
+ or linearly interpolating between them.
+
+ TODO: pass other arguments as a tuple args=(...),
+ TODO: fix evil pythonic memory accessing of variables outside of the function scope
+ """
+ x_dot = np.zeros(5)
+ q = x[:2]
+ v = x[2:4]
+
+ u_tau_1 = np.interp(t, t_from_ocp, u[:, 0])
+ u_fn = np.interp(t, t_from_ocp, u[:, 1])
+ u_t = np.array([u_tau_1, u_fn])
+
+ if args.friction_model == "coulomb":
+ F_f, _ = pendulum_coulomb(
+ args,
+ mu_tors,
+ x,
+ u_t,
+ model_pin,
+ data_pin,
+ data_actuation,
+ )
+
+ if args.friction_model == "lugre":
+ F_f = pendulum_lugre(
+ args,
+ x,
+ u_t,
+ data_actuation,
+ )
+
+ # Forward dynamics
+ tau = np.zeros(2)
+ tau[0] = u_tau_1
+ tau[1] = F_f
+ q_ddot = pin.aba(model_pin, data_pin, q, v, tau)
+ x_dot[:4] = q_ddot
+ x_dot[4] = data_actuation.z_dot
+
+ if history != None:
+ history["q"][i] = q.copy()
+ history["q_ddot"][i] = q_ddot.copy()
+ history["tau"][i] = tau.copy()
+ history["F_f"][i] = F_f.copy()
+
+ return x_dot
+
+
+def fwdSimPendulum(
+ args: Namespace,
+ dt: float,
+ T: int,
+ x0: np.ndarray,
+ t_final: float,
+ mu_tors: float,
+ model_pin: pin.Model,
+ data_pin: pin.Data,
+ data_actuation: crocoddyl.ActuationDataAbstract,
+ u: np.ndarray,
+ t_from_ocp: np.ndarray,
+):
+ t_init = 0
+ t_final = T * dt
+ history = None
+
+ result = solve_ivp(
+ stateEquation,
+ [t_init, t_final],
+ x0,
+ method="LSODA",
+ max_step=args.dt,
+ args=(
+ args,
+ 0,
+ mu_tors,
+ model_pin,
+ data_pin,
+ data_actuation,
+ u,
+ t_from_ocp,
+ history,
+ ),
+ )
+ N = result.y.shape[1]
+ history = {
+ "t": np.zeros((N,)),
+ "q": np.zeros((N, 2)),
+ "v": np.zeros((N, 2)),
+ "z": np.zeros((N,)),
+ "F_f": np.zeros((N,)),
+ "q_ddot": np.zeros((N, 2)),
+ }
+ history["q"] = result.y[0 : model_pin.nv].T
+ history["v"] = result.y[model_pin.nv : model_pin.nv * 2].T
+ history["z"] = result.y[model_pin.nv * 2 : model_pin.nv * 2 + 1].T
+ history["t"] = result.t
+ for i in range(N):
+ stateEquation(
+ result.t[i],
+ result.y[:, i],
+ args,
+ i,
+ mu_tors,
+ model_pin,
+ data_pin,
+ data_actuation,
+ u,
+ t_from_ocp,
+ history,
+ )
diff --git a/examples/regrasping/crocoddyl/pendulum/multibody_with_lugre_state.py b/examples/regrasping/crocoddyl/pendulum/multibody_with_lugre_state.py
new file mode 120000
index 0000000000000000000000000000000000000000..117ddc71fa9f038ded76eebc7fe3cd111b45b356
--- /dev/null
+++ b/examples/regrasping/crocoddyl/pendulum/multibody_with_lugre_state.py
@@ -0,0 +1 @@
+../multibody_with_lugre_state.py
\ No newline at end of file
diff --git a/examples/regrasping/crocoddyl/pendulum_croco_models.py b/examples/regrasping/crocoddyl/pendulum/pendulum_croco_models.py
similarity index 63%
rename from examples/regrasping/crocoddyl/pendulum_croco_models.py
rename to examples/regrasping/crocoddyl/pendulum/pendulum_croco_models.py
index d09af4230955c812232c5d0ffc40a95ce21d42ad..994736d106f9847e0878ff75f059db7429efe2ed 100644
--- a/examples/regrasping/crocoddyl/pendulum_croco_models.py
+++ b/examples/regrasping/crocoddyl/pendulum/pendulum_croco_models.py
@@ -1,4 +1,5 @@
import numpy as np
+from pendulum_friction_computation import pendulum_coulomb, pendulum_lugre
import crocoddyl
import pinocchio as pin
@@ -66,54 +67,29 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
def calc(self, data, x, u):
data.tau[0] = u[0]
- # now we need to calculate the sticking condition.
- # it's whether tau[1] is bigger than mu_tors * fn.
- # 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.
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
+ F_f, self.sticking_condition = pendulum_coulomb(
+ self.args,
+ self.mu_tors,
+ x,
+ u,
+ self.model_pin,
+ self.data_pin,
+ data,
+ )
- 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
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)
+ F_f = pendulum_lugre(
+ self.args,
+ self.mu_tors,
+ x,
+ u,
+ self.model_pin,
+ self.data_pin,
+ data,
)
- 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
+
+ data.tau[1] = F_f
# calc is always called first
def calcDiff(self, data, x, u):
diff --git a/examples/regrasping/crocoddyl/pendulum/pendulum_friction_computation.py b/examples/regrasping/crocoddyl/pendulum/pendulum_friction_computation.py
new file mode 100644
index 0000000000000000000000000000000000000000..86755b73350c7a9d55b047864f6f128e1d886c26
--- /dev/null
+++ b/examples/regrasping/crocoddyl/pendulum/pendulum_friction_computation.py
@@ -0,0 +1,99 @@
+import pinocchio as pin
+import numpy as np
+from argparse import Namespace
+import crocoddyl
+
+
+def computeStickingCondition(mu_tors, model_pin, data_pin, x, u):
+ sticking_condition = True
+ if np.abs(x[3]) > 0.0:
+ 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 sticking_condition:
+ M = pin.crba(model_pin, data_pin, x[:2])
+ M_inv = np.linalg.inv(M)
+ nle_tau = pin.nle(model_pin, data_pin, x[:2], x[2:])
+ tau2 = -1 * (M_inv[1, 0] * u[0] - nle_tau[1]) / M_inv[1, 1]
+ print(x[3], tau2, -1 * np.sign(x[3]) * mu_tors * u[1])
+ if np.abs(tau2) > mu_tors * u[1]:
+ sticking_condition = False
+ else:
+ sticking_condition = True
+ return sticking_condition
+
+
+def pendulum_coulomb(
+ args: Namespace,
+ mu_tors: float,
+ x: np.ndarray,
+ u: np.ndarray,
+ model_pin: pin.Model,
+ data_pin: pin.Data,
+ data_actuation: crocoddyl.ActuationDataAbstract,
+) -> tuple[np.ndarray, bool]:
+ """
+ x : [q1, q2, v1, v2]
+ u : [tau1, fn]
+ """
+ # now we need to calculate the sticking condition.
+ # it's whether tau[1] is bigger than mu_tors * fn.
+ # 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.
+ sticking_condition = True
+ F_f = 0.0
+ tau2 = 0.0
+ if np.abs(x[3]) > 0.0:
+ sticking_condition = False
+ # NOTE:if there was sticking before, then a[1] = 0.
+ # now if this is the onset of sliping, it won't be.
+ # but for the purposes of this calculation it is, right?
+ if (np.abs(x[3]) < args.min_nonsticking_velocity) or sticking_condition:
+ M = pin.crba(model_pin, data_pin, x[:2])
+ M_inv = np.linalg.inv(M)
+ data_actuation.nle_tau = pin.nle(model_pin, data_pin, x[:2], x[2:])
+ tau2 = -1 * (M_inv[1, 0] * u[0] - data_actuation.nle_tau[1]) / M_inv[1, 1]
+ if np.abs(tau2) > mu_tors * u[1]:
+ sticking_condition = False
+ else:
+ sticking_condition = True
+
+ if sticking_condition:
+ F_f = tau2
+ else:
+ F_f = -1 * np.sign(x[3]) * 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 args.control_gripping_force:
+ F_f = 0.0
+
+ return F_f, sticking_condition
+
+
+def pendulum_lugre(
+ args: Namespace,
+ x: np.ndarray,
+ u: np.ndarray,
+ data_actuation: crocoddyl.ActuationDataAbstract,
+) -> np.ndarray:
+ """
+ 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
+ """
+ data_actuation.g_v = (
+ u[-1]
+ * (args.mu_c + (args.mu_s - args.mu_c))
+ * np.exp(-1 * (x[3] / args.v_s) ** 2)
+ )
+ data_actuation.z_dot = (
+ x[3] - ((args.sigma_0 * np.abs(x[3])) / data_actuation.g_v) * x[-1]
+ )
+ F_f = (
+ args.sigma_0 * x[-1]
+ + args.sigma_1 * data_actuation.z_dot
+ + args.sigma_2 * x[-1]
+ )
+ return F_f
diff --git a/examples/regrasping/crocoddyl/pendulum_regrasp.py b/examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py
similarity index 93%
rename from examples/regrasping/crocoddyl/pendulum_regrasp.py
rename to examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py
index d9c61a1a20fe06633b8fd1eec29f771de0f7a67f..9dae3590ba0c25d19e290434cbbdf01ee73acc37 100644
--- a/examples/regrasping/crocoddyl/pendulum_regrasp.py
+++ b/examples/regrasping/crocoddyl/pendulum/pendulum_regrasp.py
@@ -2,7 +2,8 @@ from pendulum_croco_models import (
ActuationModelDoublePendulum,
CostModelDoublePendulum,
)
-from utils import get_args, computeStickingCondition
+from utils import get_args
+from pendulum_friction_computation import computeStickingCondition
from multibody_with_lugre_state import MultiBodyWithLugreState
import matplotlib.pyplot as plt
@@ -96,9 +97,16 @@ else:
solver.solve()
# Plotting the entire motion
log = solver.getCallbacks()[1]
-print(x0)
xs = np.array(log.xs)
us = np.array(log.us)
+t = np.linspace(0, T * dt, T)
+
+xs_good_simulation = fwdSimPendulum()
+
+print(
+ "both of these should be the same x0, but they're sometimes not, so please check things make sense :)"
+)
+print(x0)
print(xs[0])
if WITHPLOT:
crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)
diff --git a/examples/regrasping/crocoddyl/pendulum/utils.py b/examples/regrasping/crocoddyl/pendulum/utils.py
new file mode 120000
index 0000000000000000000000000000000000000000..50fbc6d8f5567dcbb19d623b7c39e80ad017e79e
--- /dev/null
+++ b/examples/regrasping/crocoddyl/pendulum/utils.py
@@ -0,0 +1 @@
+../utils.py
\ No newline at end of file
diff --git a/examples/regrasping/crocoddyl/friction/friction.py b/examples/regrasping/crocoddyl/planar_object/friction/friction.py
similarity index 100%
rename from examples/regrasping/crocoddyl/friction/friction.py
rename to examples/regrasping/crocoddyl/planar_object/friction/friction.py
diff --git a/examples/regrasping/crocoddyl/friction/friction_utils.py b/examples/regrasping/crocoddyl/planar_object/friction/friction_utils.py
similarity index 100%
rename from examples/regrasping/crocoddyl/friction/friction_utils.py
rename to examples/regrasping/crocoddyl/planar_object/friction/friction_utils.py
diff --git a/examples/regrasping/crocoddyl/planar_object/fwd_sim_scipy.py b/examples/regrasping/crocoddyl/planar_object/fwd_sim_scipy.py
new file mode 100644
index 0000000000000000000000000000000000000000..9249156848f09d0b558d67b2bed8106200caa88e
--- /dev/null
+++ b/examples/regrasping/crocoddyl/planar_object/fwd_sim_scipy.py
@@ -0,0 +1,295 @@
+import numpy as np
+import pinocchio as pin
+
+# from pin_to_vedo_plot_util import *
+from dynamics_utils import *
+from controllers import *
+
+"""
+#######################################################################
+# physical quantities and their coordinates #
+#######################################################################
+{J} - planar joint frame
+{b} - body CoM frame
+{c1}, {c2} - contact frames
+q_J_b - SE(2) pose in of body CoM {J} (x,y,theta)
+a_b_se2 - body se(2) acceleration given in {b} frame, aba output
+v_b_se2 - body se(2) twist given in {b} frame
+tau_f_b - joint-space torques and forces due to the object moving and friction
+tau - all joint-space torques and forces
+aba(q_J_b4, v_b_se2, tau) --> a_b_se2
+"""
+
+
+# def stateEquation(t, x, history=None, history_controller=None):
+def stateEquation(
+ t,
+ x,
+ args,
+ i,
+ model,
+ data,
+ controller,
+ fic_c1,
+ fic_c2,
+ box_dimensions,
+ OBJECT_JOINT_ID,
+ history,
+ history_controller,
+):
+ """
+ stateEquation
+ ----------------
+ the ode solver calls stateEquation to integrate x_dot = f(t, x).
+ hence the problem needs to be written in state-space from.
+ inputs are:
+ - t - passed by the solver because it does non-homogeneous steps
+ - x - system state (everything that changes with time)
+ outputs are:
+ - x_dot - first derivative
+ Because the solver will call stateEquation many times to estimate derivatives,
+ we can't store results as it runs.
+ Instead, we re-run the equations with the solved states
+ and re-calculate the non-state quantities we want to save.
+ the hacky history arguments lets us do this by calling stateEquation,
+ which is much better than copy-pasting and slightly changing stateEquation.
+
+ state -> TODO: investigate how storing theta as ctheta, stheta can be forced to work to clean up the mess
+ -----
+ 0 : 5 - arm rotatational joint angles
+ 6 : 7 - p1, p2 - jaw gripper left and right finger
+ 8 : 10 - x,y,theta of the "planar joint", i.e. held object
+ 11 : 16 - qdot of rotational joints
+ 17 : 18 - qdot of jaw gripper left and right finger
+ 19 : 21 - v_b_se2 of the "planar joint", i.e. held object
+ 22 : 24 - z1, z of contact 1
+ 25 : 27 - z2, z of contact 2
+ if AdaptiveController:
+ 28 : 29 - adaptive parameters estimates
+
+ TODO: pass other arguments as a tuple args=(...),
+ TODO: fix evil pythonic memory accessing of variables outside of the function scope
+
+ and put this function in a different file or at least
+ don't have it as main.
+ if necessary, shove all parameters etc in a class or dictionary
+ to make it less messy.
+ class option is probably better so that different things happen
+ depending on the controller etc (ex. do you need gravity compensation,
+ or are you running a controller for the arm as well)
+ """
+ x[10] = x[10] % (2 * np.pi)
+ q_J_b3 = np.array([x[8], x[9], x[10]])
+ q_J_b4 = np.array([x[8], x[9], np.cos(x[10]), np.sin(x[10])]) # , x[3]])
+
+ q = np.zeros(model.nq)
+ q[0:8] = x[0:8]
+ q[8:12] = q_J_b4
+ # do T_b_J on q_dot
+ # got to here
+ v_b_se2 = np.array([x[19], x[20], x[21]])
+ v_b_se3 = pin.Motion(
+ np.array([v_b_se2[0], v_b_se2[1], 0.0]), np.array([0.0, 0.0, v_b_se2[2]])
+ )
+
+ v = x[11:22]
+
+ z_c1 = np.array([x[22], x[23], x[24]]).reshape((3, 1))
+ z_c2 = np.array([x[25], x[26], x[27]]).reshape((3, 1))
+
+ if controller.__class__.__name__ == "AdaptiveController":
+ controller.setEstimates(x[28], x[29])
+
+ fic_c1.lugre["z"] = z_c1 # set friction model state
+ fic_c2.lugre["z"] = z_c2 # set friction model state
+
+ v_of_c1 = pin.Motion()
+ v_of_c1.setZero()
+ v_of_c2 = pin.Motion()
+ v_of_c2.setZero()
+
+ f_f_c1_se3 = pin.Force()
+ f_f_c1_se3.setZero()
+ f_f_c2_se3 = pin.Force()
+ f_f_c2_se3.setZero()
+ # f_f_b_se3 = pin.Force()
+ # f_f_b_se3.setZero()
+ f_f_b_se3 = np.zeros(6)
+
+ # get T_w_b - needed to map to friction contacts
+ pin.forwardKinematics(model, data, q)
+
+ # need to add length to get to the end of the fingertip
+ # to fingertip
+ T_w_b = data.oMi[OBJECT_JOINT_ID].copy()
+ # need to add length to get to the end of the fingertip
+ # to fingertip
+ # TODO: move this to a side function
+ to_finger_tip_offset = pin.SE3.Identity()
+ to_finger_tip_offset.translation[2] = 0.045
+ T_w_c1 = data.oMi[-1].act(to_finger_tip_offset)
+ T_w_c2 = data.oMi[-2].act(to_finger_tip_offset)
+ # this should be correct
+ contact_flag = isGripperInContact(T_w_c1, T_w_b, box_dimensions)
+ # realign T_w_b to make have contact and box axis aligned so that the
+ # se2 to se3 math checks out
+ align_T_w_b_matrix = pin.SE3.Identity()
+ align_T_w_b_matrix.rotation = pin.rpy.rpyToMatrix(0, np.pi / 2, 0)
+ # we also need the contact frames z axis to be pointing into the object
+ align_c1_matrix = pin.SE3.Identity()
+ align_c1_matrix.rotation = pin.rpy.rpyToMatrix(0.0, np.pi / 2, 0.0)
+ T_w_c1 = T_w_c1.act(align_c1_matrix)
+ align_c2_matrix = pin.SE3.Identity()
+ align_c2_matrix.rotation = pin.rpy.rpyToMatrix(0.0, np.pi / 2, 0.0)
+ T_w_c2 = T_w_c2.act(align_c2_matrix)
+
+ # let's calculate gravity compensation - without it, the robot just falls down lel.
+ # we want to do the compensation only for the arm and the gripper
+ tau_gravity = pin.computeGeneralizedGravity(model, data, q)
+ # we need tau_g (as in torque only) in controllers that assume only rotational motion
+ tau_g_rotation_object = getObjectTauG(tau_gravity[8:11], T_w_b, T_w_c1, T_w_c2)
+ # we don't want magical access to object forces
+ tau_gravity[8:11] = 0
+
+ # fic_c1.set_fn(5 - t * 2.5)
+ # fic_c2.set_fn(5 - t * 2.5)
+
+ if contact_flag:
+ u, controller_log_item = controller.computeControl(
+ x[10], x[21], t, tau_g_rotation_object
+ )
+ fic_c1.set_fn(u)
+ fic_c2.set_fn(u)
+ # fic_c1.set_fn(0)
+ # fic_c2.set_fn(0)
+ # we want v_c1 in body coordinates.
+ # we have v_b (body twist) and need to compute v_c1 twist from it.
+ # SE3.act(twist) is:
+ # | R [p]R |
+ # | 0 R | which calcs the angular -> linear effect (and inv is the correct inv ofc).
+ # also, the angular velocity of a rigid body is the same for every point, so that doesn't change.
+ T_c1_b = T_w_c1.actInv(T_w_b)
+ T_c2_b = T_w_c2.actInv(T_w_b)
+
+ # distance from contact to center of mass
+ # z-axes of c1 and b are parallel by construction,
+ # and we don't care about z offset
+ # due to contact symmetry
+ l_com = np.linalg.norm(T_c1_b.translation[:2])
+
+ v_body_c1 = T_c1_b.act(v_b_se3)
+ v_body_c1 = np.array(
+ [v_body_c1.linear[0], v_body_c1.linear[1], v_body_c1.angular[2]]
+ )
+ v_body_c2 = T_c2_b.act(v_b_se3)
+ v_body_c2 = np.array(
+ [v_body_c2.linear[0], v_body_c2.linear[1], v_body_c2.angular[2]]
+ )
+
+ dy_c1, f_f_c1_dict = fic_c1.ode_step(t, z_c1, v_body_c1)
+ dy_c2, f_f_c2_dict = fic_c2.ode_step(t, z_c2, v_body_c2)
+ f_f_c1_se3.linear = np.array([f_f_c1_dict["x"], f_f_c1_dict["y"], 0])
+ f_f_c1_se3.angular = np.array([0, 0, f_f_c1_dict["tau"]])
+ f_f_c2_se3.linear = np.array([f_f_c2_dict["x"], f_f_c2_dict["y"], 0])
+ f_f_c2_se3.angular = np.array([0, 0, f_f_c2_dict["tau"]])
+
+ f_f_b_c1 = T_c1_b.action.T @ f_f_c1_se3.vector
+ f_f_b_c2 = T_c2_b.action.T @ f_f_c2_se3.vector
+ f_f_b_se3 = f_f_b_c1 + f_f_b_c2
+ J = pin.computeJointJacobian(model, data, q, OBJECT_JOINT_ID)
+ tau_f_b = J.T @ f_f_b_se3
+ # TODO compensate negative friction
+ # this is a hot-fix
+ tau_f_b[0:6] = 0.0
+ # there are two taus because contact forces will be added at some later point
+ else:
+ u = 0.0
+ controller_log_item = None
+ fic_c1.set_fn(0.0)
+ fic_c2.set_fn(0.0)
+ dy_c1, f_f_c1_dict = fic_c1.ode_step(t, z_c1, np.zeros(3))
+ dy_c2, f_f_c2_dict = fic_c2.ode_step(t, z_c2, np.zeros(3))
+ tau_f_b = np.zeros(model.nv)
+
+ # adding up all of the torques (with forces being mapped to joint space)
+ # TODO: remove comment!!!
+ # tau = tau_f_b + tau_gravity
+
+ # TODO: remove this
+ tau_something = np.zeros(model.nv)
+ tau_something[0] = 1.1
+ tau_something[1] = -0.1
+ tau_something[8:11] = 0
+ tau = tau_f_b + tau_gravity + tau_something
+
+ # Forward dynamics
+ q_ddot = pin.aba(model, data, q, v, tau)
+ # estimate position derivative numerically rather than compute.
+ # it works fine and makes the code way simpler.
+ # our dts aren't that small either.
+ q_next4 = (pin.integrate(model, q, v * args.dt) - q) / args.dt
+ x_dot = np.zeros(model.nv)
+ x_dot[:10] = q_next4[:10]
+ x_dot[10] = v_b_se2[2]
+ # no gripping
+ x_dot[6:8] = np.zeros(2)
+
+ if history != None:
+ # history["x_dot"][i] = x_dot.copy()
+ # TODO: change to all joints and extract only the object one
+ a_g = pin.aba(model, data, q, np.zeros(model.nv), np.zeros(model.nv))
+ history["nle"][i] = a_g.copy()
+ history["q"][i] = q.copy()
+ history["q_ddot"][i] = q_ddot.copy()
+ history["tau"][i] = tau.copy()
+ history["tau_f"][i] = tau_f_b.copy()
+ # history["f_f_b"][i] = np.array([f_f_b_se3.linear[0], f_f_b_se3.linear[1], f_f_b_se3.angular[2]])
+ history["f_f_b"][i] = np.array([f_f_b_se3[0], f_f_b_se3[1], f_f_b_se3[5]])
+ history["v_of_c1"][i] = v_of_c1.vector.copy()
+ history["v_b_se2"][i] = v_b_se2.copy()
+ history["T_w_b"].append(T_w_b.copy())
+ history["T_w_c1"].append(T_w_c1.copy())
+ history["T_w_c2"].append(T_w_c2.copy())
+ history["contact_flag"][i] = contact_flag
+ history["fn"][i] = u
+
+ if history_controller != None and controller_log_item != None:
+ history_controller["theta_t"][i] = controller_log_item["theta_t"]
+ history_controller["theta_d_t"][i] = controller_log_item["theta_d_t"]
+ history_controller["theta_dot_t"][i] = controller_log_item["theta_dot_t"]
+ history_controller["theta_dot_d_t"][i] = controller_log_item["theta_dot_d_t"]
+ history_controller["theta_ddot_r_t"][i] = controller_log_item["theta_ddot_r_t"]
+ history_controller["u"][i] = controller_log_item["u"]
+ history_controller["s"][i] = controller_log_item["s"]
+ history_controller["h_hat"][i] = controller_log_item["h_hat"]
+ history_controller["b_hat"][i] = controller_log_item["b_hat"]
+
+ x_dot = np.hstack((x_dot, q_ddot))
+ # evil python array concatenation syntax
+ # adding lugre parameters
+ if not controller.__class__.__name__ == "AdaptiveController":
+ return np.array(
+ x_dot.tolist()
+ + [
+ fic_c1.lugre["dz"][0][0],
+ fic_c1.lugre["dz"][1][0],
+ fic_c1.lugre["dz"][2][0],
+ fic_c2.lugre["dz"][0][0],
+ fic_c2.lugre["dz"][1][0],
+ fic_c2.lugre["dz"][2][0],
+ ]
+ )
+ # adding adaptive control numbers
+ else:
+ return np.array(
+ x_dot.tolist()
+ + [
+ fic_c1.lugre["dz"][0][0],
+ fic_c1.lugre["dz"][1][0],
+ fic_c1.lugre["dz"][2][0],
+ fic_c2.lugre["dz"][0][0],
+ fic_c2.lugre["dz"][1][0],
+ fic_c2.lugre["dz"][2][0],
+ ]
+ + controller.computeEstimateDerivative()
+ )
diff --git a/examples/regrasping/crocoddyl/gripper_and_object_croco_model.py b/examples/regrasping/crocoddyl/planar_object/gripper_and_object_croco_model.py
similarity index 100%
rename from examples/regrasping/crocoddyl/gripper_and_object_croco_model.py
rename to examples/regrasping/crocoddyl/planar_object/gripper_and_object_croco_model.py
diff --git a/examples/regrasping/crocoddyl/pin_models.py b/examples/regrasping/crocoddyl/planar_object/pin_models.py
similarity index 71%
rename from examples/regrasping/crocoddyl/pin_models.py
rename to examples/regrasping/crocoddyl/planar_object/pin_models.py
index bf7d9dfc492c58436fd507625b7623c56d2804c4..608fca03308117f3a04405766c04c06dae1367ed 100644
--- a/examples/regrasping/crocoddyl/pin_models.py
+++ b/examples/regrasping/crocoddyl/planar_object/pin_models.py
@@ -3,55 +3,6 @@ import numpy as np
import hppfcl as fcl
-def make_cartpole(ub=True):
- model = pin.Model()
-
- m1 = 1.0
- m2 = 0.1
- length = 0.5
- base_sizes = (0.4, 0.2, 0.05)
-
- base = pin.JointModelPX()
- base_id = model.addJoint(0, base, pin.SE3.Identity(), "base")
-
- if ub:
- pole = pin.JointModelRUBY()
- else:
- pole = pin.JointModelRY()
- pole_id = model.addJoint(1, pole, pin.SE3.Identity(), "pole")
-
- base_inertia = pin.Inertia.FromBox(m1, *base_sizes)
- pole_inertia = pin.Inertia(
- m2,
- np.array([0.0, 0.0, length / 2]),
- m2 / 5 * np.diagflat([1e-2, length**2, 1e-2]),
- )
-
- base_body_pl = pin.SE3.Identity()
- pole_body_pl = pin.SE3.Identity()
- pole_body_pl.translation = np.array([0.0, 0.0, length / 2])
-
- model.appendBodyToJoint(base_id, base_inertia, base_body_pl)
- model.appendBodyToJoint(pole_id, pole_inertia, pole_body_pl)
-
- # make visual/collision models
- collision_model = pin.GeometryModel()
- shape_base = fcl.Box(*base_sizes)
- radius = 0.01
- shape_pole = fcl.Capsule(radius, length)
- RED_COLOR = np.array([1, 0.0, 0.0, 1.0])
- WHITE_COLOR = np.array([1, 1.0, 1.0, 1.0])
- geom_base = pin.GeometryObject("link_base", base_id, shape_base, base_body_pl)
- geom_base.meshColor = WHITE_COLOR
- geom_pole = pin.GeometryObject("link_pole", pole_id, shape_pole, pole_body_pl)
- geom_pole.meshColor = RED_COLOR
-
- collision_model.addGeometryObject(geom_base)
- collision_model.addGeometryObject(geom_pole)
- visual_model = collision_model
- return model, collision_model, visual_model
-
-
def gripper_and_object(
object_dimensions: np.ndarray,
) -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]:
diff --git a/examples/regrasping/crocoddyl/regrasp.py b/examples/regrasping/crocoddyl/planar_object/regrasp.py
similarity index 100%
rename from examples/regrasping/crocoddyl/regrasp.py
rename to examples/regrasping/crocoddyl/planar_object/regrasp.py
diff --git a/examples/regrasping/crocoddyl/utils.py b/examples/regrasping/crocoddyl/utils.py
index 37fa7b49839fe6869af717f967440ff9d6ca88d8..15da30f1fe77bc50e3ef9783f4df2026bdae7bff 100644
--- a/examples/regrasping/crocoddyl/utils.py
+++ b/examples/regrasping/crocoddyl/utils.py
@@ -43,6 +43,12 @@ def get_args():
choices=["coulomb", "lugre"],
help="select friction model from list of available ones",
)
+ parser.add_argument(
+ "--min-nonsticking-velocity",
+ type=float,
+ default=1e-2,
+ help="if the sliding velocity is below this, enter sticking mode",
+ )
# NOTE: most are taken from gabriel
parser.add_argument(
"--mu-s", type=float, default=1.2, help="static coulomb friction coefficient"
@@ -125,23 +131,3 @@ def get_args():
args = parser.parse_args()
return args
-
-
-def computeStickingCondition(mu_tors, model_pin, data_pin, x, u):
- sticking_condition = True
- if np.abs(x[3]) > 0.0:
- 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 sticking_condition:
- M = pin.crba(model_pin, data_pin, x[:2])
- M_inv = np.linalg.inv(M)
- nle_tau = pin.nle(model_pin, data_pin, x[:2], x[2:])
- tau2 = -1 * (M_inv[1, 0] * u[0] - nle_tau[1]) / M_inv[1, 1]
- print(x[3], tau2, -1 * np.sign(x[3]) * mu_tors * u[1])
- if np.abs(tau2) > mu_tors * u[1]:
- sticking_condition = False
- else:
- sticking_condition = True
- return sticking_condition