Esempio n. 1
0
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.))
    defaultFilename = "package://talos_data/urdf/talos_reduced_v2.urdf"
    """
    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, device=None, tracer=None, fromRosParam=False):
        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'
        }

        if fromRosParam:
            ltimeStep = 0.005
            if device is not None:
                ltimeStep = device.getTimeStep()

            print("Using SoT parameter \"/robot_description\"")
            paramName = "/robot_description"
            self.param_server = ParameterServer("param_server")
            self.param_server.init_simple(ltimeStep)

            self.param_server.setParameter("/pg/remap/l_ankle",
                                           "leg_left_6_link")
            self.param_server.setParameter("/pg/remap/r_ankle",
                                           "leg_right_6_link")
            self.param_server.setParameter("/pg/remap/l_wrist",
                                           "arm_left_7_link")
            self.param_server.setParameter("/pg/remap/r_wrist",
                                           "arm_right_7_link")
            self.param_server.setParameter("/pg/remap/body", "base_link")
            self.param_server.setParameter("/pg/remap/torso", "torso_2_link")
            lpn_pre = "/robot/specificities/feet/"
            feet = ['right', 'left']
            for afoot in feet:
                self.param_server.setParameterDbl(
                    lpn_pre + afoot + "/size/height", 0.122)
                self.param_server.setParameterDbl(
                    lpn_pre + afoot + "/size/width", 0.205)
                self.param_server.setParameterDbl(
                    lpn_pre + afoot + "/size/depth", 0.107)
                self.param_server.setParameterDbl(
                    lpn_pre + afoot + "/anklePosition/x", 0.0)
                self.param_server.setParameterDbl(
                    lpn_pre + afoot + "/anklePosition/y", 0.0)
                self.param_server.setParameterDbl(
                    lpn_pre + afoot + "/anklePosition/z", 0.107)
            self.param_server.displayRobotUtil()

            model2_string = self.param_server.getParameter(paramName)

            self.loadModelFromString(
                model2_string,
                rootJointType=pinocchio.JointModelFreeFlyer,
                removeMimicJoints=True)
        else:
            self.loadModelFromUrdf(self.defaultFilename,
                                   rootJointType=pinocchio.JointModelFreeFlyer,
                                   removeMimicJoints=True)

        assert hasattr(self, "pinocchioModel")
        assert hasattr(self, "pinocchioData")

        if device is not None:
            self.device = device
        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.dynamic.displayModel()
        self.dimension = self.dynamic.getDimension()

        self.initializeRobot()

        self.AdditionalFrames.append(
            ("leftFootForceSensor", self.forceSensorInLeftAnkle,
             self.OperationalPointsMap["left-ankle"]))
        self.AdditionalFrames.append(
            ("rightFootForceSensor", self.forceSensorInRightAnkle,
             self.OperationalPointsMap["right-ankle"]))

        # Create operational points based on operational points map
        # (if provided)
        if self.OperationalPointsMap is not None:
            self.initializeOpPoints()
Esempio n. 2
0
class Tiago(AbstractRobot):
    """
    This class defines a Tiago robot
    """

    defaultFilename = "package://tiago_data/robots/tiago_steel.urdf"

    def defineHalfSitting(self, q):
        """
        q is the configuration to fill.

        When this function is called, the attribute pinocchioModel has been filled.
        """
        model = self.pinocchioModel
        # set arm position
        self.setJointValueInConfig(q,
                [ "arm_{}_joint".format(i+1) for i in range(7) ],
                (0., -1.569796, -1.569796, 2.355194, 0., 0., 0.,))

    def __init__(self, robotName, device=None, tracer=None, with_wheels=True, fromRosParam=False):
        self.OperationalPointsMap = {
            'wrist': 'arm_7_joint',
            'right-wheel': 'wheel_right_joint',
            'left-wheel': 'wheel_left_joint',
            'mobilebase': 'root_joint',
            'footprint': 'base_footprint_joint',
            'gaze': 'head_2_joint',
        }

        if fromRosParam:
            print("Using ROS parameter \"/robot_description\"")
            rosParamName = "/robot_description"
            import rospy
            if rosParamName not in rospy.get_param_names():
                raise RuntimeError('"' + rosParamName + '" is not a ROS parameter.')
            s = rospy.get_param(rosParamName)

            self.loadModelFromString(s, rootJointType=pinocchio.JointModelFreeFlyer,
                    removeMimicJoints=True)
        else:
            self.loadModelFromUrdf(self.defaultFilename,
                    rootJointType=pinocchio.JointModelFreeFlyer,
                    removeMimicJoints=True)

        # Clean the robot model. Remove:
        # - caster joints
        # - suspension joints
        # - for hey5 hands, remove every hand_* joints except hand_thumb_joint,
        #   hand_index_joint and hand_mrl_joint (mrl = middle, ring, little)
        jointsToRemove = []
        for name in self.pinocchioModel.names:
            if not with_wheels and name.startswith("wheel_"):
                jointsToRemove.append(name)
            elif name.startswith('caster'):
                jointsToRemove.append(name)
            elif name.startswith('suspension'):
                jointsToRemove.append(name)
            elif name.startswith('hand_') and \
                    name not in ("hand_thumb_joint", "hand_index_joint", "hand_mrl_joint"):
                jointsToRemove.append(name)

        print("Removing joints " + ", ".join(jointsToRemove))
        self.removeJoints(jointsToRemove)

        assert hasattr(self, "pinocchioModel")
        assert hasattr(self, "pinocchioData")

        AbstractRobot.__init__(self, robotName, tracer)

        # 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.dynamic.displayModel()
        self.dimension = self.dynamic.getDimension()

        self.device = device
        self.initializeRobot()

        # Create operational points based on operational points map (if provided)
        if self.OperationalPointsMap is not None:
            self.initializeOpPoints()

    def setClosedLoop(self, closedLoop):
        if closedLoop:
            plug(self.device.robotState, self.dynamic.position)
            self.device.setClosedLoop(True)
        else:
            plug(self.device.state, self.dynamic.position)
            self.device.setClosedLoop(False)

    def _initialize(self):
        AbstractRobot._initialize(self)
        self.OperationalPoints.extend(['wrist', 'left-wheel', 'right-wheel', 'footprint', 'mobilebase', 'gaze'])
Esempio n. 3
0
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.))
    defaultFilename = "package://talos_data/urdf/talos_reduced_v2.urdf"
    """
    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,
                 fromRosParam=False):
        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'
        }

        if fromRosParam:
            print("Using ROS parameter \"/robot_description\"")
            rosParamName = "/robot_description"
            import rospy
            if rosParamName not in rospy.get_param_names():
                raise RuntimeError('"' + rosParamName +
                                   '" is not a ROS parameter.')
            s = rospy.get_param(rosParamName)

            self.loadModelFromString(
                s,
                rootJointType=pinocchio.JointModelFreeFlyer,
                removeMimicJoints=True)
        else:
            self.loadModelFromUrdf(self.defaultFilename,
                                   rootJointType=pinocchio.JointModelFreeFlyer,
                                   removeMimicJoints=True)

        assert hasattr(self, "pinocchioModel")
        assert hasattr(self, "pinocchioData")

        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.dynamic.displayModel()
        self.dimension = self.dynamic.getDimension()

        self.device = device
        self.initializeRobot()

        self.AdditionalFrames.append(
            ("leftFootForceSensor", self.forceSensorInLeftAnkle,
             self.OperationalPointsMap["left-ankle"]))
        self.AdditionalFrames.append(
            ("rightFootForceSensor", self.forceSensorInRightAnkle,
             self.OperationalPointsMap["right-ankle"]))

        # Create operational points based on operational points map (if provided)
        if self.OperationalPointsMap is not None:
            self.initializeOpPoints()