# -*- coding: utf-8 -*-
"""
Copyright ©2017. The Regents of the University of California (Regents). All Rights Reserved.
Permission to use, copy, modify, and distribute this software and its documentation for educational,
research, and not-for-profit purposes, without fee and without a signed licensing agreement, is
hereby granted, provided that the above copyright notice, this paragraph and the following two
paragraphs appear in all copies, modifications, and distributions. Contact The Office of Technology
Licensing, UC Berkeley, 2150 Shattuck Avenue, Suite 510, Berkeley, CA 94720-1620, (510) 643-
7201, otl@berkeley.edu, http://ipira.berkeley.edu/industry-info for commercial licensing opportunities.
IN NO EVENT SHALL REGENTS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL,
INCIDENTAL, OR CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF
THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF REGENTS HAS BEEN
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE. THE SOFTWARE AND ACCOMPANYING DOCUMENTATION, IF ANY, PROVIDED
HEREUNDER IS PROVIDED "AS IS". REGENTS HAS NO OBLIGATION TO PROVIDE
MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
"""
"""
Encapsulates data and operations on a 2D or 3D object that can be grasped
Author: Jeff Mahler
"""
from abc import ABCMeta, abstractmethod
import copy
import logging
import numpy as np
from .meshpy import mesh as m
from .meshpy import sdf as s
import IPython
import matplotlib.pyplot as plt
from autolab_core import RigidTransform, SimilarityTransform
# class GraspableObject(metaclass=ABCMeta):
[docs]class GraspableObject:
""" Encapsulates geometric structures for computing contact in grasping.
Attributes
----------
sdf : :obj:`Sdf3D`
signed distance field, for quickly computing contact points
mesh : :obj:`Mesh3D`
3D triangular mesh to specify object geometry, should match SDF
key : :obj:`str`
object identifier, usually given from the database
model_name : :obj:`str`
name of the object mesh as a .obj file, for use in collision checking
mass : float
mass of the object
convex_pieces : :obj:`list` of :obj:`Mesh3D`
convex decomposition of the object geom for collision checking
"""
__metaclass__ = ABCMeta
def __init__(self, sdf, mesh, key='', model_name='', mass=1.0, convex_pieces=None):
self.sdf_ = sdf
self.mesh_ = mesh
self.key_ = key
self.model_name_ = model_name # for OpenRave usage, gross!
self.mass_ = mass
self.convex_pieces_ = convex_pieces
@property
def sdf(self):
return self.sdf_
@property
def mesh(self):
return self.mesh_
@property
def mass(self):
return self.mass_
@property
def key(self):
return self.key_
@property
def model_name(self):
return self.model_name_
@property
def convex_pieces(self):
return self.convex_pieces_
[docs]class GraspableObject3D(GraspableObject):
""" 3D Graspable object for computing contact in grasping.
Attributes
----------
sdf : :obj:`Sdf3D`
signed distance field, for quickly computing contact points
mesh : :obj:`Mesh3D`
3D triangular mesh to specify object geometry, should match SDF
key : :obj:`str`
object identifier, usually given from the database
model_name : :obj:`str`
name of the object mesh as a .obj file, for use in collision checking
mass : float
mass of the object
convex_pieces : :obj:`list` of :obj:`Mesh3D`
convex decomposition of the object geom for collision checking
"""
def __init__(self, sdf, mesh, key='',
model_name='', mass=1.0,
convex_pieces=None):
if not isinstance(sdf, s.Sdf3D):
raise ValueError('Must initialize 3D graspable object with 3D sdf')
if not isinstance(mesh, m.Mesh3D):
raise ValueError('Must initialize 3D graspable object with 3D mesh')
GraspableObject.__init__(self, sdf, mesh, key=key,
model_name=model_name, mass=mass,
convex_pieces=convex_pieces)
[docs] def moment_arm(self, x):
""" Computes the moment arm to a point x.
Parameters
----------
x : 3x1 :obj:`numpy.ndarray`
point to get moment arm for
Returns
-------
3x1 :obj:`numpy.ndarray`
"""
return x - self.mesh.center_of_mass
[docs] def rescale(self, scale):
""" Rescales uniformly by a given factor.
Parameters
----------
scale : float
the amount to scale the object
Returns
-------
:obj:`GraspableObject3D`
the graspable object rescaled by the given factor
"""
stf = SimilarityTransform(scale=scale)
sdf_rescaled = self.sdf_.rescale(scale)
mesh_rescaled = self.mesh_.transform(stf)
convex_pieces_rescaled = None
if self.convex_pieces_ is not None:
convex_pieces_rescaled = []
for convex_piece in self.convex_pieces_:
convex_piece_rescaled = convex_piece.transform(stf)
convex_pieces_rescaled.append(convex_piece_rescaled)
return GraspableObject3D(sdf_rescaled, mesh_rescaled, key=self.key,
model_name=self.model_name, mass=self.mass,
convex_pieces=convex_pieces_rescaled)