Skip to content
Snippets Groups Projects
Commit c5eea5cc authored by m-guberina's avatar m-guberina
Browse files

lugre on pendulum progress

parent 17849161
No related branches found
No related tags found
No related merge requests found
...@@ -6,6 +6,8 @@ import crocoddyl ...@@ -6,6 +6,8 @@ import crocoddyl
from friction.friction import ReducedFrictionModel from friction.friction import ReducedFrictionModel
from pin_models import isGripperInContact, findInHandContactPoints from pin_models import isGripperInContact, findInHandContactPoints
#class
# TODO: make it so that you switch between having an arm or just the end-effector # TODO: make it so that you switch between having an arm or just the end-effector
# in the model with an argument. # in the model with an argument.
# TODO: make friction models switchable with an argument # TODO: make friction models switchable with an argument
...@@ -291,6 +293,7 @@ class ActuationModelRegrasp(crocoddyl.ActuationModelAbstract): ...@@ -291,6 +293,7 @@ class ActuationModelRegrasp(crocoddyl.ActuationModelAbstract):
# TODO: possibly not necessary to do this computation here, but let's worry about that later # 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 mu_tors = self.args.beta * self.args.contact_radius * self.args.mu_s
# fx, fy, tauz # fx, fy, tauz
# NOTE: this is missing the force due to other joints' acceleration
f_d_and_g = pin.nle( f_d_and_g = pin.nle(
self.model, self.joint_data, x[: self.nq], x[self.nq :] self.model, self.joint_data, x[: self.nq], x[self.nq :]
)[-3:] )[-3:]
......
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)
...@@ -59,6 +59,8 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract): ...@@ -59,6 +59,8 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
self.data_pin = data_pin self.data_pin = data_pin
self.mu_tors = self.args.beta * self.args.contact_radius * self.args.mu_s self.mu_tors = self.args.beta * self.args.contact_radius * self.args.mu_s
self.sticking_condition = True self.sticking_condition = True
self.z_dot = 0.0
self.g_v = 0.0
# u = [tau_1, fn] # u = [tau_1, fn]
...@@ -69,6 +71,7 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract): ...@@ -69,6 +71,7 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
# but what is tau[1]? # but what is tau[1]?
# well, if there's sticking, then v[1] = 0. # 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 v[1] > 0, then we're already sliding so there's nothing to check.
if self.args.friction_model == "coulomb":
self.sticking_condition = True self.sticking_condition = True
if np.abs(x[3]) > 0.0: if np.abs(x[3]) > 0.0:
self.sticking_condition = False self.sticking_condition = False
...@@ -93,15 +96,36 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract): ...@@ -93,15 +96,36 @@ class ActuationModelDoublePendulum(crocoddyl.ActuationModelAbstract):
# print(data.tau[1], x[3], self.sticking_condition) # print(data.tau[1], x[3], self.sticking_condition)
if not self.args.control_gripping_force: if not self.args.control_gripping_force:
data.tau[1] = 0.0 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 # calc is always called first
def calcDiff(self, data, x, u): def calcDiff(self, data, x, u):
if not self.args.control_gripping_force: if not self.args.control_gripping_force:
return return
if self.args.friction_model == "coulomb":
if self.sticking_condition: if self.sticking_condition:
data.dtau_du[1, 1] = 0.0 data.dtau_du[1, 1] = 0.0
else: else:
data.dtau_du[1, 1] = -1 * np.sign(x[3]) * self.mu_tors 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
pin.aba(self.model_pin, self.data_pin, x[:2], x[2:], data.tau) pin.aba(self.model_pin, self.data_pin, x[:2], x[2:], data.tau)
pin.computeRNEADerivatives( pin.computeRNEADerivatives(
......
...@@ -3,12 +3,11 @@ from pendulum_croco_models import ( ...@@ -3,12 +3,11 @@ from pendulum_croco_models import (
CostModelDoublePendulum, CostModelDoublePendulum,
) )
from utils import get_args, computeStickingCondition from utils import get_args, computeStickingCondition
from multibody_with_lugre_state import MultiBodyWithLugreState
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import crocoddyl import crocoddyl
import os
import signal import signal
import sys
import time import time
import example_robot_data import example_robot_data
import numpy as np import numpy as np
...@@ -28,6 +27,8 @@ model = pendulum.model ...@@ -28,6 +27,8 @@ model = pendulum.model
data = pendulum.data data = pendulum.data
state = crocoddyl.StateMultibody(model) state = crocoddyl.StateMultibody(model)
if args.friction_model == "lugre":
state = MultiBodyWithLugreState(state)
# this is the first link # this is the first link
actuation = ActuationModelDoublePendulum(state, args, pendulum.model, pendulum.data) actuation = ActuationModelDoublePendulum(state, args, pendulum.model, pendulum.data)
...@@ -50,6 +51,10 @@ runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt) ...@@ -50,6 +51,10 @@ runningCostModel.addCost("uReg", uRegCost, 1e-4 / dt)
runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt) runningCostModel.addCost("xGoal", xPendCost, 1e-5 / dt)
terminalCostModel.addCost("xGoal", xPendCost, 100.0) 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( runningModel = crocoddyl.IntegratedActionModelEuler(
crocoddyl.DifferentialActionModelFreeFwdDynamics( crocoddyl.DifferentialActionModelFreeFwdDynamics(
state, actuation, runningCostModel state, actuation, runningCostModel
......
...@@ -13,6 +13,7 @@ import argparse ...@@ -13,6 +13,7 @@ import argparse
import time import time
# TODO: 1) inherit from numdiff everywhere # TODO: 1) inherit from numdiff everywhere
# --> can't, they not exported!
# TODO: 2) put in gravity compensation into the actuation model # TODO: 2) put in gravity compensation into the actuation model
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment