def setupValkyrieExample():
# Valkyrie Example
rbt = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rbt.world(), [0, 0, 0],
[0, 0, 0])
from pydrake.multibody.parsers import PackageMap
pmap = PackageMap()
# Note: Val model is currently not installed in drake binary distribution.
pmap.PopulateFromFolder(os.path.join(pydrake.getDrakePath(), "examples"))
# TODO(russt): remove plane.urdf and call AddFlatTerrainTOWorld instead
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(FindResource(os.path.join("underactuated", "plane.urdf")),
'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kFixed,
world_frame,
rbt)
val_start_frame = RigidBodyFrame("val_start_frame", rbt.world(),
[0, 0, 1.5], [0, 0, 0])
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(
pydrake.getDrakePath() +
"/examples/valkyrie/urdf/urdf/valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf",
'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kRollPitchYaw,
val_start_frame,
rbt)
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
pbrv = MeshcatRigidBodyVisualizer(rbt)
return rbt, pbrv
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 test_package_map(self):
pm = PackageMap()
self.assertFalse(pm.Contains("foo"))
self.assertEqual(pm.size(), 0)
pm.Add("foo", os.path.abspath(os.curdir))
self.assertEqual(pm.size(), 1)
self.assertTrue(pm.Contains("foo"))
self.assertEqual(pm.GetPath("foo"), os.path.abspath(os.curdir))
# Populate from folder.
# TODO(eric.cousineau): This mismatch between casing is confusing, with
# `Atlas` being the package name, but `atlas` being the dirctory name.
pm = PackageMap()
self.assertEqual(pm.size(), 0)
pm.PopulateFromFolder(os.path.join(getDrakePath(), "examples",
"atlas"))
self.assertTrue(pm.Contains("Atlas"))
self.assertEqual(pm.GetPath("Atlas"),
os.path.join(getDrakePath(), "examples", "atlas", ""))
# Populate from environment.
pm = PackageMap()
os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"] = os.path.join(
getDrakePath(), "examples")
pm.PopulateFromEnvironment("PYDRAKE_TEST_ROS_PACKAGE_PATH")
self.assertTrue(pm.Contains("Atlas"))
self.assertEqual(pm.GetPath("Atlas"),
os.path.join(getDrakePath(), "examples", "atlas", ""))
del os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"]
# Populate upstream.
pm = PackageMap()
pm.PopulateUpstreamToDrake(
os.path.join(getDrakePath(), "examples", "atlas", "urdf",
"atlas_minimal_contact.urdf"))
self.assertTrue(pm.Contains("Atlas"))
self.assertEqual(pm.GetPath("Atlas"),
os.path.join(getDrakePath(), "examples", "atlas"))
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 LittleDogSimulationDiagram(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
#zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
#zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5
]), np.zeros(12)),
axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# RigidBodyPlant
plant = builder.AddSystem(RigidBodyPlant(rb_tree, dt))
plant.set_name('plant')
# Visualizer
visualizer_publisher = builder.AddSystem(drake_visualizer)
visualizer_publisher.set_name('visualizer_publisher')
visualizer_publisher.set_publish_period(0.02)
builder.Connect(plant.state_output_port(),
visualizer_publisher.get_input_port(0))
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_input_port(0))
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
#PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
#PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
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)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Robot State handle
robot_state_handle = builder.AddSystem(
RobotStateHandle(rb_tree)) # force_sensor_info
robot_state_handle.set_name('robot_state_handle')
builder.Connect(plant.state_output_port(),
robot_state_handle.joint_state_results_input_port())
logger_N = 4
logger = []
for i in range(logger_N):
logger.append([])
# Signal logger
signalLogRate = 100
logger[0] = builder.AddSystem(SignalLogger(num_pos * 2))
logger[0]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(plant.get_output_port(0), logger[0].get_input_port(0))
# cotroller command
logger[1] = builder.AddSystem(SignalLogger(num_actuators))
logger[1]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(PIDcontroller.get_output_port(0),
logger[1].get_input_port(0))
# torque
# other
logger[2] = builder.AddSystem(SignalLogger(3))
builder.Connect(robot_state_handle.com_outport_port(),
logger[2].get_input_port(0))
logger[2]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
logger[3] = builder.AddSystem(SignalLogger(num_actuators))
builder.Connect(plant.torque_output_port(), logger[3].get_input_port(0))
logger[3]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
return builder.Build(), logger, plant
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
# zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
# zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# 1.SceneGraph system
scene_graph = builder.AddSystem(SceneGraph())
scene_graph.RegisterSource('scene_graph_n')
plant = builder.AddSystem(MultibodyPlant())
plant_parser = Parser(plant, scene_graph)
plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body"))
# Add gravity to the model.
plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
# Model is completed
plant.Finalize()
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.get_continuous_state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_actuation_input_port())
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
# PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
# PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
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)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Signal logger
logger = builder.AddSystem(SignalLogger(num_pos * 2))
builder.Connect(plant.get_continuous_state_output_port(),
logger.get_input_port(0))
return builder.Build(), logger, plant
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_input_port(0))
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
rb = rigid_body_tree = RigidBodyTree()
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)
#PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
controller = RbtInverseDynamicsController(rb, kp, ki, kd, False)
PIDcontroller = builder.AddSystem(controller)
PIDcontroller.set_name('PID_controller')
controller_test = builder.AddSystem(Robot_controller_test(rb_tree, kp, ki, kd))
controller_test.set_name('controller_test')