def test_flat_terrain(self):
tree = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
# Test that AddFlatTerrainToWorld is spelled correctly.
AddFlatTerrainToWorld(tree)
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)
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
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
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