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 load_robot_from_urdf(urdf_file):
"""
This function demonstrates how to pass a complete
set of arguments to Drake's URDF parser. It is also
possible to load a robot with a much simpler syntax
that uses default values, such as:
robot = RigidBodyTree(urdf_file)
"""
urdf_string = open(urdf_file).read()
base_dir = os.path.dirname(urdf_file)
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
# Load our model from URDF
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string,
package_map,
base_dir,
floating_base_type,
weld_frame,
robot)
return robot
def test_urdf(self):
"""Test that an instance of a URDF model can be loaded into a
RigidBodyTree by passing a complete set of arguments to Drake's URDF
parser.
"""
urdf_file = os.path.join(
getDrakePath(),
"examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
with open(urdf_file) as f:
urdf_string = f.read()
base_dir = os.path.dirname(urdf_file)
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string, package_map, base_dir, floating_base_type, weld_frame,
robot)
expected_num_bodies = 86
self.assertEqual(robot.get_num_bodies(),
expected_num_bodies,
msg='Incorrect number of bodies: {0} vs. {1}'.format(
robot.get_num_bodies(), expected_num_bodies))
def add_fixed_model(tree, filename, xyz=np.zeros(3), rpy=np.zeros(3)):
with open(filename) as f:
urdf_string = f.read()
base_dir = os.path.dirname(filename)
package_map = PackageMap()
floating_base_type = FloatingBaseType.kFixed
weld_frame = RigidBodyFrame("weld_frame", tree.FindBody("world"), xyz, rpy)
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string, package_map, base_dir, floating_base_type, weld_frame,
tree)
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_urdf(self):
"""Test that an instance of a URDF model can be loaded into a
RigidBodyTree by passing a complete set of arguments to Drake's URDF
parser.
"""
urdf_file = os.path.join(
getDrakePath(),
"examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
with open(urdf_file) as f:
urdf_string = f.read()
base_dir = os.path.dirname(urdf_file)
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string, package_map, base_dir, floating_base_type, weld_frame,
robot)
expected_num_bodies = 86
self.assertEqual(robot.get_num_bodies(),
expected_num_bodies,
msg='Incorrect number of bodies: {0} vs. {1}'.format(
robot.get_num_bodies(), expected_num_bodies))
# Check actuators.
actuator = robot.GetActuator("head_pan_motor")
self.assertIsInstance(actuator, RigidBodyActuator)
self.assertEqual(actuator.name, "head_pan_motor")
self.assertIs(actuator.body, robot.FindBody("head_pan_link"))
self.assertEqual(actuator.reduction, 6.0)
self.assertEqual(actuator.effort_limit_min, -2.645)
self.assertEqual(actuator.effort_limit_max, 2.645)
# Check full number of actuators.
self.assertEqual(len(robot.actuators), robot.get_num_actuators())
for actuator in robot.actuators:
self.assertIsInstance(actuator, RigidBodyActuator)
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.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')
builder.Connect(controller.get_output_port(0),
controller_test.robot_state_input_port())
builder.Connect(