def appendUrdfModel(self, urdfFilename, frame_name, fMm, prefix=None): if not isinstance(fMm, pinocchio.SE3): fMm = pinocchio.XYZQUATToSE3(fMm) id_parent_frame = self.model.getFrameId(frame_name) model, gmodel = pinocchio.buildModelsFromUrdf( hpp.rostools.retrieve_resource(urdfFilename), geometry_types=pinocchio.GeometryType.COLLISION) if prefix is not None: for i in range(1, len(model.names)): model.names[i] = prefix + model.names[i] for f in model.frames: f.name = prefix + f.name for go in gmodel.geometryObjects: go.name = prefix + go.name igeom = self.gmodel.ngeoms nmodel, ngmodel = pinocchio.appendModel(self.model, model, self.gmodel, gmodel, id_parent_frame, fMm) self.gid_field_of_view = ngmodel.getGeometryId("field_of_view") for go in gmodel.geometryObjects: ngmodel.addCollisionPair( pinocchio.CollisionPair(self.gid_field_of_view, ngmodel.getGeometryId(go.name))) self.model, self.gmodel = nmodel, ngmodel self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
def loadModelFromUrdf(self, urdfPath, urdfDir=None, rootJointType=pinocchio.JointModelFreeFlyer, removeMimicJoints=True): """ Load a model using the pinocchio urdf parser. This parser looks for urdf files in which kinematics and dynamics information have been added. - param urdfPath: a path to the URDF file. - param urdfDir: A list of directories. If None, will use ROS_PACKAGE_PATH. """ if urdfPath.startswith("package://"): from os import path n1 = 10 # len("package://") n2 = urdfPath.index(path.sep, n1) pkg = urdfPath[n1:n2] relpath = urdfPath[n2 + 1:] import rospkg rospack = rospkg.RosPack() abspkg = rospack.get_path(pkg) urdfFile = path.join(abspkg, relpath) else: urdfFile = urdfPath if urdfDir is None: import os urdfDir = os.environ["ROS_PACKAGE_PATH"].split(':') if rootJointType is None: self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile) else: self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile, rootJointType()) self.pinocchioData = pinocchio.Data(self.pinocchioModel) if removeMimicJoints: self._removeMimicJoints(urdfFile=urdfFile)
def reduceModel(self, jointsToRemove, config, len_prefix=0): jointIds = [ self.model.getJointId(jn[len_prefix:]) for jn in jointsToRemove ] self.model, self.gmodel = pinocchio.buildReducedModel( self.model, self.gmodel, jointIds, np.array(config)) self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__( self, urdfString=None, urdfFilename=None, fov=np.radians((60., 90.)), # fov = np.radians((49.5, 60)), geoms=["arm_3_link_0"], ): if isinstance(fov, str): self.fov = fov fov_fcl = hppfcl.MeshLoader().load( hpp.rostools.retrieve_resource(fov)) else: # build FOV tetahedron dx, dy = np.sin(fov) pts = hppfcl.StdVec_Vec3f() self.fov = [ (0, 0, 0), (dx, dy, 1), (-dx, dy, 1), (-dx, -dy, 1), (dx, -dy, 1), ] pts.extend([np.array(e) for e in self.fov]) self.fov.append(self.fov[1]) fov_fcl = hppfcl.BVHModelOBBRSS() fov_fcl.beginModel(4, 5) fov_fcl.addSubModel(pts, _tetahedron_tris) fov_fcl.endModel() self.cos_angle_thr = np.cos(np.radians(70)) if urdfFilename is None: assert urdfString is not None # Pinocchio does not allow to build a GeometryModel from a XML string. urdfFilename = '/tmp/tmp.urdf' with open(urdfFilename, 'w') as f: f.write(urdfString) self.model, self.gmodel = pinocchio.buildModelsFromUrdf( hpp.rostools.retrieve_resource(urdfFilename), root_joint=pinocchio.JointModelPlanar(), geometry_types=pinocchio.GeometryType.COLLISION) id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame") parent_frame = self.model.frames[id_parent_frame] go = pinocchio.GeometryObject("field_of_view", id_parent_frame, parent_frame.parent, fov_fcl, parent_frame.placement) self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model) for n in geoms: assert self.gmodel.existGeometryName(n) self.gmodel.addCollisionPair( pinocchio.CollisionPair(self.gmodel.getGeometryId(n), self.gid_field_of_view)) self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(self, state, actuationModel, costModel): crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuationModel.nu, costModel.nr) self.actuation = actuationModel self.costs = costModel self.enable_force = True self.armature = np.matrix(np.zeros(0)) # We cannot abstract data in Python bindings, let's create this internal data inside model self.pinocchioData = pinocchio.Data(self.state.pinocchio) self.actuationData = self.actuation.createData() self.costsData = self.costs.createData(self.pinocchioData) self.Minv = None
def loadModelFromString(self, urdfString, rootJointType=pinocchio.JointModelFreeFlyer, removeMimicJoints=True): """ Load a URDF model contained in a string - param rootJointType: the root joint type. None for no root joint. - param removeMimicJoints: if True, all the mimic joints found in the model are removed. """ if rootJointType is None: self.pinocchioModel = pinocchio.buildModelFromXML(urdfString) else: self.pinocchioModel = pinocchio.buildModelFromXML(urdfString, rootJointType()) self.pinocchioData = pinocchio.Data(self.pinocchioModel) if removeMimicJoints: self._removeMimicJoints(urdfString=urdfString)
def removeJoints(self, joints): """ - param joints: a list of joint names to be removed from the self.pinocchioModel """ jointIds = list() for j in joints: if self.pinocchioModel.existJointName(j): jointIds.append(self.pinocchioModel.getJointId(j)) if len(jointIds) > 0: q = pinocchio.neutral(self.pinocchioModel) self.pinocchioModel = pinocchio.buildReducedModel(self.pinocchioModel, jointIds, q) self.pinocchioData = pinocchio.Data(self.pinocchioModel)
def createData(self): data = crocoddyl.DifferentialActionModelAbstract.createData(self) data.pinocchio = pinocchio.Data(self.state.pinocchio) data.costs = self.costs.createData(data.pinocchio) data.shareCostMemory(data.costs) return data
import consim from example_robot_data.robots_loader import loadSolo print("Test solo cpp sim started") dt = 1e-3 ndt = 10 T = 3.0 kp = 10 kd = 0.05 N = int(T / dt) # se3.RobotWrapper.BuildFromURDF(conf.urdf, [conf.path, ], se3.JointModelFreeFlyer()) robot = loadSolo() robot.data = se3.Data(robot.model) if not robot.model.check(robot.data): print("Python data not consistent with model") sim = consim.build_simple_simulator(dt, ndt, robot.model, robot.data, conf.K[0, 0], conf.B[0, 0], conf.K[1, 1], conf.B[1, 1], conf.mu, conf.mu) cpts = [] for cf in conf.contact_frames: if not robot.model.existFrame(cf): print("ERROR: Frame", cf, "does not exist") cpts += [sim.add_contact_point(robot.model.getFrameId(cf))] q = conf.q0 conf.q0[2] += 0.1 dq = zero(robot.nv)