class HumanoidRobot(AbstractHumanoidRobot): def __init__(self, name, pinocchio_model, pinocchio_data, initialConfig, OperationalPointsMap=None, tracer=None): AbstractHumanoidRobot.__init__(self, name, tracer) self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.OperationalPointsMap = OperationalPointsMap self.dynamic = DynamicPinocchio(self.name + "_dynamic") self.dynamic.setModel(pinocchio_model) self.dynamic.setData(pinocchio_data) self.dimension = self.dynamic.getDimension() self.device = RobotSimu(self.name + "_device") self.device.resize(self.dynamic.getDimension()) self.halfSitting = initialConfig self.device.set(self.halfSitting) plug(self.device.state, self.dynamic.position) 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., ) 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., ) if self.OperationalPointsMap is not None: self.initializeOpPoints()
class HumanoidRobot(AbstractHumanoidRobot): def __init__(self, name, pinocchio_model, pinocchio_data, initialConfig, OperationalPointsMap = None, tracer = None): AbstractHumanoidRobot.__init__(self, name, tracer) self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.OperationalPointsMap = OperationalPointsMap self.dynamic = DynamicPinocchio(self.name + "_dynamic") self.dynamic.setModel(pinocchio_model) self.dynamic.setData(pinocchio_data) self.dimension = self.dynamic.getDimension() self.device = RobotSimu(self.name + "_device") self.device.resize(self.dynamic.getDimension()) self.halfSitting = initialConfig self.device.set(self.halfSitting) plug(self.device.state, self.dynamic.position) 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.,) 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.,) if self.OperationalPointsMap is not None: self.initializeOpPoints()
class Talos(AbstractHumanoidRobot): """ This class defines a Talos robot """ forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.)) forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.)) """ TODO: Confirm the position and existence of these sensors accelerometerPosition = np.matrix (( (1., 0., 0., -.13,), (0., 1., 0., 0.,), (0., 0., 1., .118,), (0., 0., 0., 1.,), )) gyrometerPosition = np.matrix (( (1., 0., 0., -.13,), (0., 1., 0., 0.,), (0., 0., 1., .118,), (0., 0., 0., 1.,), )) """ def smallToFull(self, config): #Gripper position in full configuration: 27:34, and 41:48 #Small configuration: 36 DOF #Full configuration: 50 DOF res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * ( 0., ) + config[34:] return res 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()