wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update and fix few parts (nothing serious)

Browse Source
This commit is contained in:
Brian Delhaisse
2019-05-03 03:58:39 +02:00
parent 49c8c1ffd1
commit a7be1285e8
6 changed files with 105 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/robots/minitaur.py
+3 -2
View File
@@ -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,
pyrobolearn/robots/urdfs/franka/franka.urdf
+43
View File
@@ -4,6 +4,37 @@
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="black">
<color rgba="0.0 0.0 0.0 1"/>
</material>
<material name="white">
<color rgba="0.9 0.9 0.9 1"/>
</material>
<material name="dark_gray">
<color rgba="0.3 0.3 0.3 1"/>
</material>
<material name="gray">
<color rgba="0.5 0.5 0.5 1"/>
</material>
<material name="light_gray">
<color rgba="0.7 0.7 0.7 1"/>
</material>
<material name="pure_red">
<color rgba="1.0 0.0 0.0 1"/>
</material>
<material name="red">
<color rgba="0.5 0.1 0.2 1"/>
</material>
<material name="pure_green">
<color rgba="0.0 1.0 0.0 1"/>
</material>
<material name="pure_blue">
<color rgba="0.0 0.0 1.0 1"/>
</material>
<material name="blue">
<color rgba="0.0 0.2 0.3 1"/>
</material>
<link name="panda_link0">
<inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
<mass value="1"/> <!-- volume = 0.002442 -->
@@ -12,6 +43,7 @@
<inertia ixx="0.00286650" ixy="0.0" ixz="-0.0004095" iyy="0.00368550" iyz="0.0" izz="0.00368550"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link0.stl"/>
</geometry>
@@ -30,6 +62,7 @@
<inertia ixx="0.00654165" ixy="0.0" ixz="0.0" iyy="0.00566943" iyz="0.00174444" izz="0.00218055"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link1.stl"/>
</geometry>
@@ -56,6 +89,7 @@
<inertia ixx="0.00648789" ixy="0.0" ixz="0.0" iyy="0.00216263" iyz="0.0017301" izz="0.00605536"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link2.stl"/>
</geometry>
@@ -82,6 +116,8 @@
<inertia ixx="0.00346363" ixy="-0.0009896" ixz="-0.00148441" iyy="0.00395844" iyz="-0.0009896" izz="0.00346363"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<!-- <material name="light_gray"/> -->
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link3.stl"/>
</geometry>
@@ -108,6 +144,7 @@
<inertia ixx="0.00349301" ixy="0.00149701" ixz="-0.00099800" iyy="0.00349301" iyz="0.00099800" izz="0.00449102"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link4.stl"/>
</geometry>
@@ -134,6 +171,7 @@
<inertia ixx="0.01098901" ixy="0.000000" ixz="0.000000" iyy="0.01054945" iyz="-0.00263736" izz="0.00175824"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link5.stl"/>
</geometry>
@@ -160,6 +198,7 @@
<inertia ixx="0.00139373" ixy="0.000000" ixz="0.000000" iyy="0.00278745" iyz="0.000000" izz="0.00278745"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link6.stl"/>
</geometry>
@@ -186,6 +225,7 @@
<inertia ixx="0.00220264" ixy="0.000000" ixz="0.000000" iyy="0.00220264" iyz="0.000000" izz="0.00220264"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/link7.stl"/>
</geometry>
@@ -230,6 +270,7 @@
<inertia ixx="0.00409836" ixy="0.000000" ixz="0.000000" iyy="0.00061475" iyz="0.000000" izz="0.00409836"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="white"/>
<geometry>
<mesh filename="meshes/visual/hand.stl"/>
</geometry>
@@ -248,6 +289,7 @@
<inertia ixx="0.00030727" ixy="0.000000" ixz="0.000000" iyy="0.00003018" iyz="0.00005455" izz="0.00007091"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="light_gray"/>
<geometry>
<mesh filename="meshes/visual/finger.stl"/>
</geometry>
@@ -266,6 +308,7 @@
<inertia ixx="0.00030727" ixy="0.000000" ixz="0.000000" iyy="0.00003018" iyz="0.00005455" izz="0.00007091"/> <!-- divided by the volume, just need to be multiplied by the mass -->
</inertial>
<visual>
<material name="light_gray"/>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/visual/finger.stl"/>
pyrobolearn/trajectory_optimization/README.md
+6
View File
@@ -0,0 +1,6 @@
## Trajectory optimization
This folder provides trajectory optimization schemes.
TODO:
- [ ] implement CIO
pyrobolearn/trajectory_optimization/__init__.py
View File
pyrobolearn/optimizers/cio.py → pyrobolearn/trajectory_optimization/cio.py
+50 -6
View File
@@ -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
pyrobolearn/worlds/world.py
+3 -2
View File
@@ -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