def __init__(self, filename): # Parsing the dwl control suite configuration and initialization self.parseConfigFile(filename) self.loop_sim = False self.fixed_base = False self.t = 0. # Creating the bullet clien and configure it if self.enable_gui: self.client = pb.connect(pb.GUI, options='--opengl2') else: self.client = pb.connect(pb.DIRECT) # Creating and configurating the bullet interface self.biface = BulletInterface(self.client) self.biface.setGravity(self.gravity[dwl.X], self.gravity[dwl.Y], self.gravity[dwl.Z]) self.biface.setTimeStep(self.time_step) self.biface.resetDebugVisualizerCamera(self.cam_dist, self.cam_yaw, self.cam_pitch, self.cam_target.tolist()) # Loading the ground model pb.setAdditionalSearchPath(pybullet_data.getDataPath()) self.plane_id = pb.loadURDF('plane.urdf') # Loading the robot model to dwl and simulator self.fbs = dwl.FloatingBaseSystem() self.wkin = dwl.WholeBodyKinematics() self.wdyn = dwl.WholeBodyDynamics() self.fbs.resetFromURDFFile(self.root_path + self.urdf_path, self.root_path + self.yarf_path) self.wkin.reset(self.fbs) self.wdyn.reset(self.fbs, self.wkin) pb.setAdditionalSearchPath(self.root_path) self.robot_id = pb.loadURDF(self.urdf_path, self.robot_pos0.tolist(), pb.getQuaternionFromEuler(self.robot_rpy0), flags=pb.URDF_USE_INERTIA_FROM_FILE) # Creating the actual whole-body state self.ws_states = [dwl.WholeBodyState(self.fbs.getJointDoF()), # actual state dwl.WholeBodyState(self.fbs.getJointDoF())] # desired state self.parseRobot() self.resetRobotPosition(self.robot_pos0) self.readControllerPlugins() # Creating the controller if not self.changeController(self.ctrl_name, self.ctrl_config): print bcolors.FAIL + 'Error: the default controller is not defined' + bcolors.ENDC sys.exit() if self.enable_ros: self.enableROS()
def __init__(self, urdf, yarf): # Four-bar linkage mechanical constants self.akh = 0.522 self.fkb = 0.694 self.cj = 0.005 self.jk = 0.360 + 0.041 # The linkage bars self.ak = 0.04614 self.bk = 0.03406 self.ar = 0.056 self.br = 0.056 self.ak2 = self.ak**2 self.bk2 = self.bk**2 self.ar2 = self.ar**2 self.br2 = self.br**2 self.cyl_retracted = 0.3251 # The triangle CJK; it is invariant regardless the linkage status self.ck2 = self.cj**2 + self.jk**2 self.ck = math.sqrt(self.ck2) self.ckh = math.acos(self.jk / self.ck) self.LF_HFE_JOINT_MOTOR_OFF = -2.356 self.LF_HAA_JOINT_MOTOR_OFF = -0.698 self.fbs = dwl.FloatingBaseSystem() self.fbs.resetFromURDFFile(urdf, yarf) self.LF_HAA = self.fbs.getJointId('lf_haa_joint') self.LF_HFE = self.fbs.getJointId('lf_hfe_joint') self.LF_KFE = self.fbs.getJointId('lf_kfe_joint') self.LH_HAA = self.fbs.getJointId('lh_haa_joint') self.LH_HFE = self.fbs.getJointId('lh_hfe_joint') self.LH_KFE = self.fbs.getJointId('lh_kfe_joint') self.RF_HAA = self.fbs.getJointId('rf_haa_joint') self.RF_HFE = self.fbs.getJointId('rf_hfe_joint') self.RF_KFE = self.fbs.getJointId('rf_kfe_joint') self.RH_HAA = self.fbs.getJointId('rh_haa_joint') self.RH_HFE = self.fbs.getJointId('rh_hfe_joint') self.RH_KFE = self.fbs.getJointId('rh_kfe_joint') self.FBLStatus = {'theta': 0.0, 'cylinder': 0.0, 'lever': 0.0}
def __init__(self, urdf, yarf): self.a1 = 0.3219 self.b1 = 0.045 self.a2 = 0.3218 self.b2 = 0.045 self.eps11 = 0.1089 self.eps21 = 0.1403 self.eps22 = 0.1047 self.fbs = dwl.FloatingBaseSystem() self.fbs.resetFromURDFFile(urdf, yarf) self.LF_HAA = self.fbs.getJointId('lf_haa_joint') self.LF_HFE = self.fbs.getJointId('lf_hfe_joint') self.LF_KFE = self.fbs.getJointId('lf_kfe_joint') self.LH_HAA = self.fbs.getJointId('lh_haa_joint') self.LH_HFE = self.fbs.getJointId('lh_hfe_joint') self.LH_KFE = self.fbs.getJointId('lh_kfe_joint') self.RF_HAA = self.fbs.getJointId('rf_haa_joint') self.RF_HFE = self.fbs.getJointId('rf_hfe_joint') self.RF_KFE = self.fbs.getJointId('rf_kfe_joint') self.RH_HAA = self.fbs.getJointId('rh_haa_joint') self.RH_HFE = self.fbs.getJointId('rh_hfe_joint') self.RH_KFE = self.fbs.getJointId('rh_kfe_joint')
from __future__ import print_function # This lets us use the python3-style print() function even in python2. It should have no effect if you're already running python3. import os import dwl import numpy as np # Configure the printing np.set_printoptions(suppress=True) # Construct an instance of the FloatingBaseSystem class, which wraps the C++ class. fbs = dwl.FloatingBaseSystem() ws = dwl.WholeBodyState() # Initializing the URDF model and whole-body state fpath = os.path.dirname(os.path.abspath(__file__)) fbs.resetFromURDFFile(fpath + "/../hyq.urdf", fpath + "/../../config/hyq.yarf") ws.setJointDoF(fbs.getJointDoF()) # The robot state ws.setBasePosition(np.array([0., 0., 0.])) ws.setBaseRPY(np.array([0., 0., 0.])) ws.setBaseVelocity_W(np.array([0., 0., 0.])) ws.setBaseRPYVelocity_W(np.array([0., 0., 0.])) ws.setBaseAcceleration_W(np.array([0., 0., 0.])) ws.setBaseRPYAcceleration_W(np.array([0., 0., 0.])) ws.setJointPosition(0.75, fbs.getJointId("lf_hfe_joint")) ws.setJointPosition(-1.5, fbs.getJointId("lf_kfe_joint")) ws.setJointPosition(-0.75, fbs.getJointId("lh_hfe_joint")) ws.setJointPosition(1.5, fbs.getJointId("lh_kfe_joint")) ws.setJointPosition(0.75, fbs.getJointId("rf_hfe_joint"))