def __init__(self): super(NaoEnv, self).__init__() self.simulation_manager = SimulationManager() self.client = self.simulation_manager.launchSimulation(gui=True) self.simulation_manager.setLightPosition(self.client, [0, 0, 100]) self.robot = self.simulation_manager.spawnNao(self.client, spawn_ground_plane=True) time.sleep(1.0) # stand pose parameters pose = NaoPosture('Stand') pose_dict = {} for joint_name, joint_value in zip(pose.joint_names, pose.joint_values): pose_dict[joint_name] = joint_value # joint parameters self.joint_names = [ 'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll' ] self.lower_limits = [] self.upper_limits = [] self.init_angles = [] for joint_name in self.joint_names: joint = self.robot.joint_dict[joint_name] self.lower_limits.append(joint.getLowerLimit()) self.upper_limits.append(joint.getUpperLimit()) self.init_angles.append(pose_dict[joint_name]) self.link_names = [] for joint_name in self.joint_names: linkName = p.getJointInfo( self.robot.getRobotModel(), self.robot.joint_dict[joint_name].getIndex())[12].decode( "utf-8") self.link_names.append(linkName) self.action_space = spaces.Box(np.array(self.lower_limits), np.array(self.upper_limits)) self.observation_space = spaces.Box( np.array([-1] * len(self.joint_names)), np.array([1] * len(self.joint_names)))
def __init__(self): super(NaoEnvReal, self).__init__() self.session = qi.Session() self.robot_url = '169.254.204.242' self.session.connect(self.robot_url) self.motion = self.session.service("ALMotion") self.motion.setStiffnesses('Body', 1) self.memory = self.session.service("ALMemory") self.posture = self.session.service("ALRobotPosture") self.posture.goToPosture('Stand', 1) # joint parameters minAngle = {} maxAngle = {} limits = self.motion.getLimits("Body") jointNames = self.motion.getBodyNames("Body") for name, limit in zip(jointNames, limits): minAngle[name] = limit[0] maxAngle[name] = limit[1] self.joint_names = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'] self.lower_limits = [minAngle[name] for name in self.joint_names] self.upper_limits = [maxAngle[name] for name in self.joint_names] # stand pose parameters pose = NaoPosture('Stand') pose_dict = {} for joint_name, joint_value in zip(pose.joint_names, pose.joint_values): pose_dict[joint_name] = joint_value self.init_angles = [] for joint_name in self.joint_names: self.init_angles.append(pose_dict[joint_name]) # self.action_space = spaces.Box(np.array(self.lower_limits), np.array(self.upper_limits)) self.obs_history = [] self.obs_length = 10 self.action_space = spaces.Box(low=-0.5, high=0.5, shape=(len(self.joint_names),), dtype="float32") self.observation_space = spaces.Box(low=-float('inf'), high=float('inf'), shape=(len(self._get_obs())*self.obs_length,), dtype="float32") self._max_episode_steps = 1000
class NaoVirtual(RobotVirtual): """ Class representing the virtual instance of the NAO robot """ ID_CAMERA_TOP = 0 ID_CAMERA_BOTTOM = 1 FRAME_WORLD = 1 FRAME_ROBOT = 2 URDF_PATH = "robot_data/nao_V40/nao.urdf" P_STAND = NaoPosture("Stand") P_STAND_INIT = NaoPosture("StandInit") P_STAND_ZERO = NaoPosture("StandZero") P_CROUCH = NaoPosture("Crouch") P_SIT = NaoPosture("Sit") P_SIT_RELAX = NaoPosture("SitRelax") P_LYING_BELLY = NaoPosture("LyingBelly") P_LYING_BACK = NaoPosture("LyingBack") def __init__(self): """ Constructor """ RobotVirtual.__init__(self, NaoVirtual.URDF_PATH) self.camera_top = None self.camera_bottom = None # TODO: add constraints and speeds def loadRobot(self, translation, quaternion, physicsClientId=0): """ Overloads @loadRobot from the @RobotVirtual class. Update max velocity for the fingers and thumbs, based on the hand joints. Add self collision exceptions. Add the cameras. """ pybullet.setAdditionalSearchPath(os.path.dirname( os.path.realpath(__file__)), physicsClientId=physicsClientId) # Add 0.36 meters on the z component, avoing to spawn NAO in the ground translation = [translation[0], translation[1], translation[2] + 0.36] RobotVirtual.loadRobot(self, translation, quaternion, physicsClientId=physicsClientId) balance_constraint = pybullet.createConstraint( parentBodyUniqueId=self.robot_model, parentLinkIndex=-1, childBodyUniqueId=-1, childLinkIndex=-1, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], parentFrameOrientation=[0, 0, 0, 1], childFramePosition=translation, childFrameOrientation=quaternion, physicsClientId=self.physics_client) self.goToPosture("Stand", 1.0) pybullet.setCollisionFilterPair(self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict["Head"].getIndex(), 0, physicsClientId=self.physics_client) for side in ["R", "L"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Thigh"].getIndex(), self.link_dict[side + "Hip"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Bicep"].getIndex(), self.link_dict[side + "ForeArm"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Pelvis"].getIndex(), self.link_dict[side + "Thigh"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Tibia"].getIndex(), self.link_dict[side.lower() + "_ankle"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger11_link"].getIndex(), self.link_dict[side + "Finger13_link"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger21_link"].getIndex(), self.link_dict[side + "Finger23_link"].getIndex(), 0, physicsClientId=self.physics_client) for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict[base_link].getIndex(), 0, physicsClientId=self.physics_client) for name, link in self.link_dict.items(): for wrist in ["r_wrist", "l_wrist"]: if wrist[0] + "finger" in name.lower() or\ wrist[0] + "thumb" in name.lower(): pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[wrist].getIndex(), link.getIndex(), 0, physicsClientId=self.physics_client) pybullet.removeConstraint(balance_constraint, physicsClientId=self.physics_client) for joint_name in list(self.joint_dict): if 'RFinger' in joint_name or 'RThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["RHand"].getMaxVelocity()) elif 'LFinger' in joint_name or 'LThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["LHand"].getMaxVelocity()) self.camera_top = CameraRgb(self.robot_model, self.link_dict["CameraTop_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) self.camera_bottom = CameraRgb( self.robot_model, self.link_dict["CameraBottom_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) # eventual constraints and lasers # TODO: implement a moveTo # TODO: implement a move def setAngles(self, joint_names, joint_values, percentage_speed): """ Overloads @setAngles from the @RobotVirtual class. Handles the finger mimic behaviour. Parameters: joint_names - List of string (or string if only one joint) containing the name of the joints to be controlled joint_values - List of values (or value if only one joint) corresponding to the angles in radians to be applied percentage_speed - Percentage (or percentages) of the max speed to be used for the movement """ try: if type(joint_names) is str: assert type(joint_values) is int or type(joint_values) is float names = [joint_names] values = [joint_values] else: assert type(joint_names) is type(joint_values) is list names = list(joint_names) values = list(joint_values) if isinstance(percentage_speed, list): speeds = list(percentage_speed) else: speeds = [percentage_speed] * len(names) except AssertionError: raise pybullet.error("Error in the parameters given to the\ setAngles method") for hand in ["RHand", "LHand"]: for i in range(names.count(hand)): index = names.index(hand) value = values[index] speed = speeds[index] names.pop(index) values.pop(index) speeds.pop(index) finger_names, finger_values = self._mimicHand(hand, value) names.extend(finger_names) values.extend(finger_values) speeds.extend([speed] * len(finger_names)) RobotVirtual.setAngles(self, names, values, speeds) def getAnglesPosition(self, joint_names): """ Overloads @getAnglesPosition from the @RobotVirtual class. Handles the finger mimicked position for the hands (when getting the position of RHand or LHand, will return the hand's opening percentage). Parameters: joint_names - List of string (or string if only one joint) containing the name of the joints Returns: joint_positions - List of float (or float if only one joint) containing the joint's positions in radians """ if type(joint_names) is str: names = [joint_names] else: names = list(joint_names) joint_positions = RobotVirtual.getAnglesPosition(self, names) for hand, finger in zip(["RHand", "LHand"], ["RFinger11", "LFinger11"]): for i in range(names.count(hand)): index = names.index(hand) joint_positions[index] =\ RobotVirtual.getAnglesPosition(self, [finger]).pop() /\ (1/(self.joint_dict[finger].getUpperLimit() - self.joint_dict[finger].getLowerLimit())) +\ self.joint_dict[finger].getLowerLimit() if len(joint_positions) == 1: return joint_positions.pop() else: return joint_positions def getAnglesVelocity(self, joint_names): """ Overloads @getAnglesVelocity from the @RobotVirtual class. The method won't return the velocity of RHand and LHand joints. Parameters: joint_names - List of string (or string if only one joint) containing the name of the joints Returns: joint_velocities - List of float (or float if only one joint) containing the joint's velocities in rad/s """ if type(joint_names) is str: names = [joint_names] else: names = list(joint_names) joint_velocities = RobotVirtual.getAnglesVelocity(self, names) if len(joint_velocities) == 1: return joint_velocities.pop() else: return joint_velocities def goToPosture(self, posture_name, percentage_speed): """ Position the virtual robot into a particular posture. The different available postures are NaoPosture objects. Parameters: posture_name - String containing the name of the posture. The posture name is not case-sensitive percentage_speed - Percentage of the max speed to be used for the movement Returns: Boolean - True if the posture can be applied, False otherwise """ posture_list = [ NaoVirtual.P_STAND, NaoVirtual.P_STAND_INIT, NaoVirtual.P_STAND_ZERO, NaoVirtual.P_CROUCH, NaoVirtual.P_SIT, NaoVirtual.P_SIT_RELAX, NaoVirtual.P_LYING_BELLY, NaoVirtual.P_LYING_BACK ] for posture in posture_list: if posture.isPostureName(posture_name): self.setAngles(posture.getPostureJointNames(), posture.getPostureJointValues(), percentage_speed) return True return False def subscribeCamera(self, camera_id, resolution=Camera.K_QVGA): """ Subscribe to the camera holding the camera id. WARNING: at the moment, only one camera can be subscribed. Parameters: camera_id - The id of the camera to be subscribed resolution - CameraResolution object, the resolution of the camera """ if camera_id == NaoVirtual.ID_CAMERA_TOP: self.camera_top.subscribe(resolution=resolution) elif camera_id == NaoVirtual.ID_CAMERA_BOTTOM: self.camera_bottom.subscribe(resolution=resolution) def unsubscribeCamera(self, camera_id): """ Unsubscribe from a camera, the one holding the camera id. Parameters: camera_id - The id of the camera to be unsubscribed """ if camera_id == NaoVirtual.ID_CAMERA_TOP: self.camera_top.unsubscribe() elif camera_id == NaoVirtual.ID_CAMERA_BOTTOM: self.camera_bottom.unsubscribe() def getCameraFrame(self): """ Returns a camera frame. Be advised that the subscribeCamera method needs to be called beforehand. Returns: frame - The current camera frame as a formatted numpy array, directly exploitable from OpenCV """ if self.camera_top.isActive(): return self.camera_top.getFrame() elif self.camera_bottom.isActive(): return self.camera_bottom.getFrame() def getCameraResolution(self): """ Returns the resolution of the active camera. If no camera is active, returns None Returns: resolution - a CameraResolution object describing the resolution of the active camera """ if self.camera_top.isActive(): return self.camera_top.getResolution() elif self.camera_bottom.isActive(): return self.camera_bottom.getResolution() def _mimicHand(self, hand, value, multiplier=0.999899, offset=0.0): """ Used to propagate a joint value on the fingers attached to the hand. The formula used to mimic a joint is the following (with a multiplier of 0.999899 and an offset of 0.0): finger_value = (hand_value * multiplier) + offset Parameters: hand - String, RHand or LHand value - the joint value to be propagated Returns: finger_names - Names of the finger to be controlled finger_values - Values of the fingers to be controlled """ finger_names = list() finger_values = list() for joint_name in self.joint_dict.keys(): if (hand[0] + "Finger") in joint_name or\ (hand[0] + "Thumb") in joint_name: finger_names.append(joint_name) finger_values.append((value * multiplier) + offset) return finger_names, finger_values
class NaoVirtual(RobotVirtual): """ Class representing the virtual instance of the NAO robot """ ID_CAMERA_TOP = 0 ID_CAMERA_BOTTOM = 1 FRAME_WORLD = 1 FRAME_ROBOT = 2 URDF_FILE = "/nao.urdf" P_STAND = NaoPosture("Stand") P_STAND_INIT = NaoPosture("StandInit") P_STAND_ZERO = NaoPosture("StandZero") P_CROUCH = NaoPosture("Crouch") P_SIT = NaoPosture("Sit") P_SIT_RELAX = NaoPosture("SitRelax") P_LYING_BELLY = NaoPosture("LyingBelly") P_LYING_BACK = NaoPosture("LyingBack") def __init__(self): """ Constructor """ # Install the robot meshes and URDFs if they are not already installed. # The installation process won't be covered by the unit tests if not tools._check_resources_installed(): # pragma: no cover tools._install_resources() # Specify the URDF path RobotVirtual.__init__( self, tools._get_resources_folder() + NaoVirtual.URDF_FILE) self.camera_top = None self.camera_bottom = None # TODO: add constraints and speeds def loadRobot(self, translation, quaternion, physicsClientId=0): """ Overloads @loadRobot from the @RobotVirtual class, loads the robot into the simulated instance. This method also updates the max velocity of the robot's fingers, adds self collision filters to the model and defines the cameras of the model in the camera_dict. Parameters: translation - List containing 3 elements, the translation [x, y, z] of the robot in the WORLD frame quaternion - List containing 4 elements, the quaternion [x, y, z, q] of the robot in the WORLD frame physicsClientId - The id of the simulated instance in which the robot is supposed to be loaded """ pybullet.setAdditionalSearchPath( os.path.dirname(os.path.realpath(__file__)), physicsClientId=physicsClientId) # Add 0.36 meters on the z component, avoing to spawn NAO in the ground translation = [translation[0], translation[1], translation[2] + 0.36] RobotVirtual.loadRobot( self, translation, quaternion, physicsClientId=physicsClientId) balance_constraint = pybullet.createConstraint( parentBodyUniqueId=self.robot_model, parentLinkIndex=-1, childBodyUniqueId=-1, childLinkIndex=-1, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], parentFrameOrientation=[0, 0, 0, 1], childFramePosition=translation, childFrameOrientation=quaternion, physicsClientId=self.physics_client) self.goToPosture("Stand", 1.0) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict["Head"].getIndex(), 0, physicsClientId=self.physics_client) for side in ["R", "L"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Thigh"].getIndex(), self.link_dict[side + "Hip"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Bicep"].getIndex(), self.link_dict[side + "ForeArm"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Pelvis"].getIndex(), self.link_dict[side + "Thigh"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Tibia"].getIndex(), self.link_dict[side.lower() + "_ankle"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger11_link"].getIndex(), self.link_dict[side + "Finger13_link"].getIndex(), 0, physicsClientId=self.physics_client) pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[side + "Finger21_link"].getIndex(), self.link_dict[side + "Finger23_link"].getIndex(), 0, physicsClientId=self.physics_client) for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]: pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict["torso"].getIndex(), self.link_dict[base_link].getIndex(), 0, physicsClientId=self.physics_client) for name, link in self.link_dict.items(): for wrist in ["r_wrist", "l_wrist"]: if wrist[0] + "finger" in name.lower() or\ wrist[0] + "thumb" in name.lower(): pybullet.setCollisionFilterPair( self.robot_model, self.robot_model, self.link_dict[wrist].getIndex(), link.getIndex(), 0, physicsClientId=self.physics_client) for joint in self.joint_dict.values(): pybullet.resetJointState( self.robot_model, joint.getIndex(), 0.0, physicsClientId=self.physics_client) pybullet.removeConstraint( balance_constraint, physicsClientId=self.physics_client) for joint_name in list(self.joint_dict): if 'RFinger' in joint_name or 'RThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["RHand"].getMaxVelocity()) elif 'LFinger' in joint_name or 'LThumb' in joint_name: self.joint_dict[joint_name].setMaxVelocity( self.joint_dict["LHand"].getMaxVelocity()) camera_top = CameraRgb( self.robot_model, NaoVirtual.ID_CAMERA_TOP, self.link_dict["CameraTop_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) camera_bottom = CameraRgb( self.robot_model, NaoVirtual.ID_CAMERA_BOTTOM, self.link_dict["CameraBottom_optical_frame"], hfov=60.9, vfov=47.6, physicsClientId=self.physics_client) self.camera_dict = { NaoVirtual.ID_CAMERA_TOP: camera_top, NaoVirtual.ID_CAMERA_BOTTOM: camera_bottom} # The frequency of the IMU is set to 100Hz self.imu = Imu( self.robot_model, self.link_dict["torso"], 100.0, physicsClientId=self.physics_client) # FSRs fsr_dict = dict() for name in NaoFsr.LFOOT + NaoFsr.RFOOT: fsr_dict[name] = Fsr( self.robot_model, self.link_dict[name], physicsClientId=self.physics_client) self._setFsrHandler(FsrHandler(fsr_dict)) # TODO: implement a moveTo # TODO: implement a move def setAngles(self, joint_names, joint_values, percentage_speed): """ Overloads @setAngles from the @RobotVirtual class. Handles the finger mimic behaviour. Parameters: joint_names - List of string (or string if only one joint) containing the name of the joints to be controlled joint_values - List of values (or value if only one joint) corresponding to the angles in radians to be applied percentage_speed - Percentage (or percentages) of the max speed to be used for the movement """ try: if type(joint_names) is str: assert type(joint_values) is int or type(joint_values) is float names = [joint_names] values = [joint_values] else: assert type(joint_names) is type(joint_values) is list names = list(joint_names) values = list(joint_values) if isinstance(percentage_speed, list): speeds = list(percentage_speed) else: speeds = [percentage_speed]*len(names) except AssertionError: raise pybullet.error("Error in the parameters given to the\ setAngles method") for hand in ["RHand", "LHand"]: for i in range(names.count(hand)): index = names.index(hand) value = values[index] speed = speeds[index] names.pop(index) values.pop(index) speeds.pop(index) finger_names, finger_values = self._mimicHand(hand, value) names.extend(finger_names) values.extend(finger_values) speeds.extend([speed]*len(finger_names)) RobotVirtual.setAngles( self, names, values, speeds) def getAnglesPosition(self, joint_names): """ Overloads @getAnglesPosition from the @RobotVirtual class. Handles the finger mimicked position for the hands (when getting the position of RHand or LHand, will return the hand's opening percentage). Parameters: joint_names - List of string (or string if only one joint) containing the name of the joints Returns: joint_positions - List of float (or float if only one joint) containing the joint's positions in radians """ if type(joint_names) is str: names = [joint_names] else: names = list(joint_names) joint_positions = RobotVirtual.getAnglesPosition(self, names) for hand, finger in zip( ["RHand", "LHand"], ["RFinger11", "LFinger11"]): for i in range(names.count(hand)): index = names.index(hand) joint_positions[index] =\ RobotVirtual.getAnglesPosition(self, [finger]).pop() /\ (1/(self.joint_dict[finger].getUpperLimit() - self.joint_dict[finger].getLowerLimit())) +\ self.joint_dict[finger].getLowerLimit() if len(joint_positions) == 1: return joint_positions.pop() else: return joint_positions def getAnglesVelocity(self, joint_names): """ Overloads @getAnglesVelocity from the @RobotVirtual class. The method won't return the velocity of RHand and LHand joints. Parameters: joint_names - List of string (or string if only one joint) containing the name of the joints Returns: joint_velocities - List of float (or float if only one joint) containing the joint's velocities in rad/s """ if type(joint_names) is str: names = [joint_names] else: names = list(joint_names) joint_velocities = RobotVirtual.getAnglesVelocity(self, names) if len(joint_velocities) == 1: return joint_velocities.pop() else: return joint_velocities def goToPosture(self, posture_name, percentage_speed): """ Position the virtual robot into a particular posture. The different available postures are NaoPosture objects. Parameters: posture_name - String containing the name of the posture. The posture name is not case-sensitive percentage_speed - Percentage of the max speed to be used for the movement Returns: Boolean - True if the posture can be applied, False otherwise """ posture_list = [ NaoVirtual.P_STAND, NaoVirtual.P_STAND_INIT, NaoVirtual.P_STAND_ZERO, NaoVirtual.P_CROUCH, NaoVirtual.P_SIT, NaoVirtual.P_SIT_RELAX, NaoVirtual.P_LYING_BELLY, NaoVirtual.P_LYING_BACK] for posture in posture_list: if posture.isPostureName(posture_name): self.setAngles( posture.getPostureJointNames(), posture.getPostureJointValues(), percentage_speed) return True return False def _mimicHand( self, hand, value, multiplier=0.999899, offset=0.0): """ Used to propagate a joint value on the fingers attached to the hand. The formula used to mimic a joint is the following: finger_value = (hand_value * multiplier) + offset Parameters: hand - String, RHand or LHand value - The joint value to be propagated multiplier - The multiplier coefficient (0.999899 by default) offset - The offset coefficient (0.0 by default) Returns: finger_names - Names of the finger to be controlled finger_values - Values of the fingers to be controlled """ finger_names = list() finger_values = list() for joint_name in self.joint_dict.keys(): if (hand[0] + "Finger") in joint_name or\ (hand[0] + "Thumb") in joint_name: finger_names.append(joint_name) finger_values.append((value * multiplier) + offset) return finger_names, finger_values