diff --git a/pyrobolearn/robots/minitaur.py b/pyrobolearn/robots/minitaur.py index 655395e..d63bd72 100644 --- a/pyrobolearn/robots/minitaur.py +++ b/pyrobolearn/robots/minitaur.py @@ -23,8 +23,9 @@ class Minitaur(QuadrupedRobot): Minitaur robot from Ghost Robotics (https://www.ghostrobotics.io/) References: - [1] pybullet_envs/bullet/minitaur.py - [2] https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py + [1] "Design Principles for a Family of Direct-Drive Legged Robots", Kenneally et al., 2016 + [2] pybullet/gym/pybullet_envs/bullet/minitaur.py + [3] https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur.py """ def __init__(self, diff --git a/pyrobolearn/robots/urdfs/franka/franka.urdf b/pyrobolearn/robots/urdfs/franka/franka.urdf index 9e02429..31e860b 100644 --- a/pyrobolearn/robots/urdfs/franka/franka.urdf +++ b/pyrobolearn/robots/urdfs/franka/franka.urdf @@ -4,6 +4,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -12,6 +43,7 @@ + @@ -30,6 +62,7 @@ + @@ -56,6 +89,7 @@ + @@ -82,6 +116,8 @@ + + @@ -108,6 +144,7 @@ + @@ -134,6 +171,7 @@ + @@ -160,6 +198,7 @@ + @@ -186,6 +225,7 @@ + @@ -230,6 +270,7 @@ + @@ -248,6 +289,7 @@ + @@ -266,6 +308,7 @@ + diff --git a/pyrobolearn/trajectory_optimization/README.md b/pyrobolearn/trajectory_optimization/README.md new file mode 100644 index 0000000..8cf72a5 --- /dev/null +++ b/pyrobolearn/trajectory_optimization/README.md @@ -0,0 +1,6 @@ +## Trajectory optimization + +This folder provides trajectory optimization schemes. + +TODO: +- [ ] implement CIO diff --git a/pyrobolearn/trajectory_optimization/__init__.py b/pyrobolearn/trajectory_optimization/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/optimizers/cio.py b/pyrobolearn/trajectory_optimization/cio.py similarity index 53% rename from pyrobolearn/optimizers/cio.py rename to pyrobolearn/trajectory_optimization/cio.py index ab28880..8f90333 100644 --- a/pyrobolearn/optimizers/cio.py +++ b/pyrobolearn/trajectory_optimization/cio.py @@ -5,11 +5,9 @@ References: [1] "Automated Discovery and Learning of Complex Movement Behaviors" (PhD thesis), Mordatch, 2015 [2] Mordatch's presentation given in CS294 """ -# TODO: this is not an optimizer, but more an optimization process. It should be in another directory, maybe in -# `trajectory_optimization`?? import numpy as np -from scipy.interpolate as interp1d +from scipy.interpolate import interp1d class CIO(object): @@ -31,6 +29,52 @@ class CIO(object): - :math:`L_{task}` describes the task objectives (i.e. high-level goals of the movement) - :math:`L_{hint}` provides hints to accelerate the optimization. This term is optional. + We now describe each cost more specifically. + + + * The contact invariant cost is given by: + + .. math:: L_{CI}(s) = \sum_i^N \sum_t^T c_{i, \phi(t)} (||e_{i,t}(s)||^2 + ||\dot{e}_{i,t}(s)||^2) + + where :math:`e_{i,t} = [p_i(q_t) - n'(p), 0]` is the 4D contact-violation vector. + + * The physics violation cost is formulated as: + + .. math:: L_{physics}(s) = \sum_t^T || J_t(s)^T f_t(s) + B u_t(s) - \tau_t(s) ||^2 + + where the external contact forcing terms :math:`f_t = [f_1, ..., f_N]^\top \in \mathbb{R}^{6N}` (with each forcing + term (for each end-effector) is given by :math:`f_i = [f_c, \tau_c]` where :math:`f_c` is the translational + contact force, and :math:`\tau_c` is the torsion around the surface normal) and joint actuation + :math:`u_t \in \mathbb{R}^{D_a}` (where :math:`D_a` are the number of actuated joints and are computed according to: + + .. math:: + + f_t, u_t =& \arg \min_{f, u} || J_t(q_t)^\top f - Bu - \tau_t(q_t, \dot{q}_t, \ddot{q}_t)||^2 + f^\top W_t f + + u^\top R u \\ + \mbox{subject to } \quad A f \leq b + + which is solved using quadratic programming (QP). The linear constraint is the linear approximation to the friction + cone (i.e. the friction pyramid). The torques :math:`\tau_t(q_t, \dot{q}_t, \ddot{q}_t)` are given by the whole + body dynamic equation: + + .. math:: \tau_t(q_t, \dot{q}_t, \ddot{q}_t) = H(q_t) \ddot{q}_t + C(q_t, \dot{q}_t) \dot{q}_t + g(q_t) + + + * The task cost is expressed as: + + .. math:: L_{task}(s) = \sum_b l_b(q_T(s)) + \sum_t^T ||f_t(s)||^2 + ||u_t(s)||^2 + ||\ddot{q}_t(s)||^2 + + where :math:`b` is an index over different tasks, :math:`l_b` are task specific terms which only depends on the + final pose :math:`q_T(s)`. + + + * The optional hint cost is given by: + + .. math:: L_{hint}(s) = \sum_t \max(||z_t(s) - n(z_t(s))|| - \epsilon, 0)^2 + + where :math:`z_t(s) = z_t(q, \ddot{q})` is the zero-moment point (ZMP). + + The CIO consists of 3 phases: 1. only :math:`L_{task}` is enabled 2. All 4 terms (:math:`L_{task}`, :math:`L_{physics}`, :math:`L_{CI}`, :math:`L_{hint}`) are enabled but with @@ -40,10 +84,10 @@ class CIO(object): Note that the solution obtained at the end of each phase is perturbed with small zero-mean Gaussian noise to break any symmetries, and used to initialize the next phase. - From the optimized state :math:`s^*`, the optimal joints :math:`q^*` at each time step can be computed (using IK). - Then, a PD controller can be used to move the joints to their desired configuration. + From the optimized state :math:`s^*`, the optimal joints :math:`q^*` at each time step can be computed (using IK + and cubic spline interpolation). A PD controller can then be used to move the joints to their desired configuration. - Note that the framework do not take into account any sensory feedbacks. + Note that the framework does not take into account any sensory feedback. References: [1] "Automated Discovery and Learning of Complex Movement Behaviors" (PhD thesis), Mordatch, 2015 diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 1a38c69..e0fbb87 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -1102,18 +1102,19 @@ class World(object): def create_city(self): pass - def load_table(self, position, scaling=1.): + def load_table(self, position, orientation=(0, 0, 0, 1), scaling=1.): """ Load a table in the world. Args: position (float[3]): position of the table + orientation (float[4]): orientation of the table (quaternion [x,y,z,w]) scaling (float): scaling for the table Returns: int: unique id of the table """ - table = self.sim.load_urdf('table/table.urdf', position=position, scale=scaling) + table = self.sim.load_urdf('table/table.urdf', position=position, orientation=orientation, scale=scaling) self.movable_bodies[table] = 'table' return table