Ejemplo n.º 1
0
    def test_flat_terrain(self):
        tree = RigidBodyTree(
            os.path.join(pydrake.getDrakePath(),
                         "examples/pendulum/Pendulum.urdf"))

        # Test that AddFlatTerrainToWorld is spelled correctly.
        AddFlatTerrainToWorld(tree)
Ejemplo n.º 2
0
    def __init__(self, file_name , Is_fixed = True ):
        rb_tree = RigidBodyTree()
        world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0])
        AddFlatTerrainToWorld(rb_tree, 1000, 10)
        robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.2], [0, 0, 0])

        if Is_fixed is True:
            self.fixed_type = FloatingBaseType.kFixed
        else:
            self.fixed_type = FloatingBaseType.kRollPitchYaw
        # insert a robot from urdf files
        pmap = PackageMap()
        pmap.PopulateFromFolder(os.path.dirname(file_name))
        AddModelInstanceFromUrdfStringSearchingInRosPackages(
            open(file_name, 'r').read(),
            pmap,
            os.path.dirname(file_name),
            self.fixed_type,  # FloatingBaseType.kRollPitchYaw,
            robot_frame,
            rb_tree)

        self.rb_tree = rb_tree
        self.joint_limit_max = self.rb_tree.joint_limit_max
        self.joint_limit_min = self.rb_tree.joint_limit_min

        print(self.joint_limit_max)
        print(self.joint_limit_min)
Ejemplo n.º 3
0
def SetupLittleDog():
    # Build up your Robot World
    rb_tree = RigidBodyTree()
    world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0])
    AddFlatTerrainToWorld(rb_tree, 1000, 10)
    robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.2], [0, 0, 0])

    # insert a robot from urdf files
    pmap = PackageMap()
    pmap.PopulateFromFolder(os.path.dirname(model_path))
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
            open(model_path, 'r').read(),
            pmap,
            os.path.dirname(model_path),
            FloatingBaseType.kFixed,  #FloatingBaseType.kRollPitchYaw,
            robot_frame,
            rb_tree)
    return rb_tree
Ejemplo n.º 4
0
def SetupKinova():
    # Build up your Robot World
    rb_tree = RigidBodyTree()
    world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0],
                                 [0, 0, 0])
    AddFlatTerrainToWorld(rb_tree, 1000, 10)
    robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
                                 [0, 0, 0])

    # insert a robot from urdf files
    pmap = PackageMap()
    pmap.PopulateFromFolder(os.path.dirname(model_path))
    AddModelInstanceFromUrdfStringSearchingInRosPackages(
        open(model_path, 'r').read(),
        pmap,
        os.path.dirname(model_path),
        FloatingBaseType.kFixed,  #FloatingBaseType.kRollPitchYaw,
        robot_frame,
        rb_tree)

    print("Num of Joints : {}, \n Num of Actuators: {}".format(
        rb_tree.get_num_positions(), rb_tree.get_num_actuators()))
    return rb_tree
Ejemplo n.º 5
0
    def test_flat_terrain(self):
        tree = RigidBodyTree(
            FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))

        # Test that AddFlatTerrainToWorld is spelled correctly.
        AddFlatTerrainToWorld(tree)
from systems.robot_coomand_to_rigidbody_converter import RobotCommandToRigidBodyPlantConverter
from systems.robot_state_encoder import RobotStateEncoder

from lcmt import *
model_path = os.path.join(os.getcwd(), 'Model/LittleDog.urdf')

dt = 0

lcm = DrakeLcm()
builder = DiagramBuilder()

# Build up your Robot World
rb_tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0],
                             [0, 0, 0])
AddFlatTerrainToWorld(rb_tree, 1000, 10)
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.5],
                             [0, 0, 0])

# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
    open(model_path, 'r').read(), pmap, os.path.dirname(model_path),
    FloatingBaseType.kRollPitchYaw, robot_frame, rb_tree)


def joints_PID_params(rbtree):
    num_joints = rb_tree.get_num_actuators()

    kp = np.ones(num_joints) * 1