def __init__(self, filename=MODELPATH[0] + 'baxter_description/urdf/baxter.urdf'): RobotWrapper.initFromURDF(self, filename, MODELPATH) self.Q_INIT = np.zeros(self.nq - NQ_OFFSET) self.q_def = np.zeros(self.nq) self.v_def = np.zeros(self.nv) # self.q0 = np.matrix( [ # 0.0, 0.0, 0.648702, 0.0, 0.0 , 0.0, 1.0, # Free flyer 0-6 # 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 # 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM 11-17 # 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM 18-24 # 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 # 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 # ] ).T self.ff = list(range(7)) self.head = list(range(7, 7)) self.l_arm = list(range(8, 14)) self.r_arm = list(range(15, 21)) self.opCorrespondances = { "lh": "left_w2", "rh": "right_w2", } for op, name in list(self.opCorrespondances.items()): idx = self.__dict__[op] = self.index(name)
def __init__(self, name, initialConfig, device=None, tracer=None): self.OperationalPointsMap = { 'left-wrist': 'arm_left_7_joint', 'right-wrist': 'arm_right_7_joint', 'left-ankle': 'leg_left_6_joint', 'right-ankle': 'leg_right_6_joint', 'gaze': 'head_2_joint', 'waist': 'root_joint', 'chest': 'torso_2_joint' } from rospkg import RosPack rospack = RosPack() urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf" urdfDir = [rospack.get_path('talos_data') + "/../"] # Create a wrapper to access the dynamic model provided through an urdf file. from pinocchio.robot_wrapper import RobotWrapper import pinocchio as se3 from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio pinocchioRobot = RobotWrapper() pinocchioRobot.initFromURDF(urdfPath, urdfDir, se3.JointModelFreeFlyer()) self.pinocchioModel = pinocchioRobot.model self.pinocchioData = pinocchioRobot.data AbstractHumanoidRobot.__init__(self, name, tracer) self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') # Create rigid body dynamics model and data (pinocchio) self.dynamic = DynamicPinocchio(self.name + "_dynamic") self.dynamic.setModel(self.pinocchioModel) self.dynamic.setData(self.pinocchioData) self.dimension = self.dynamic.getDimension() # Initialize device self.device = device self.timeStep = self.device.getTimeStep() self.device.resize(self.dynamic.getDimension()) # TODO For position limit, we remove the first value to get # a vector of the good size because SoT use euler angles and not # quaternions... self.device.setPositionBounds( self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:], self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:]) self.device.setVelocityBounds( (-self.pinocchioModel.velocityLimit).T.tolist()[0], self.pinocchioModel.velocityLimit.T.tolist()[0]) self.device.setTorqueBounds( (-self.pinocchioModel.effortLimit).T.tolist()[0], self.pinocchioModel.effortLimit.T.tolist()[0]) self.halfSitting = initialConfig self.device.set(self.halfSitting) plug(self.device.state, self.dynamic.position) self.AdditionalFrames.append( ("leftFootForceSensor", self.forceSensorInLeftAnkle, self.OperationalPointsMap["left-ankle"])) self.AdditionalFrames.append( ("rightFootForceSensor", self.forceSensorInRightAnkle, self.OperationalPointsMap["right-ankle"])) self.dimension = self.dynamic.getDimension() self.plugVelocityFromDevice = True self.dynamic.displayModel() # Initialize velocity derivator if chosen if self.enableVelocityDerivator: self.velocityDerivator = Derivator_of_Vector('velocityDerivator') self.velocityDerivator.dt.value = self.timeStep plug(self.device.state, self.velocityDerivator.sin) plug(self.velocityDerivator.sout, self.dynamic.velocity) else: self.dynamic.velocity.value = self.dimension * (0., ) # Initialize acceleration derivator if chosen if self.enableAccelerationDerivator: self.accelerationDerivator = \ Derivator_of_Vector('accelerationDerivator') self.accelerationDerivator.dt.value = self.timeStep plug(self.velocityDerivator.sout, self.accelerationDerivator.sin) plug(self.accelerationDerivator.sout, self.dynamic.acceleration) else: self.dynamic.acceleration.value = self.dimension * (0., ) # Create operational points based on operational points map (if provided) if self.OperationalPointsMap is not None: self.initializeOpPoints()