def playbackMotion(data1, data2, data3, data4, times):
data = np.concatenate((data2, data1, data4, data3), axis=0)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(Player(data, times))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
#print(robot.get_output_port(0).size())
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Simulate for 10 seconds
simulator.StepTo(times[-1] + 0.5)
def urdfViz(plant):
"""
Visualize the position/orientation of the vehicle along a trajectory using the PlanarRigidBodyVisualizer and
a basic .urdf file representing the vehicle, start, goal, and state constraints.
"""
tree = RigidBodyTree(FindResource("/notebooks/6832-code/ben_uav.urdf"),
FloatingBaseType.kRollPitchYaw)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-1, 15], ylim=[-0.5, 2.5])
tf = plant.ttraj[-1]
times = np.linspace(0, tf, 200)
# organize the relevant states for the visualizer
posn = np.zeros((13, len(times)))
for i, t in enumerate(times):
x = plant.xdtraj_poly.value(t)
u = plant.udtraj_poly.value(t)
posn[0:6, i] = 0
posn[6, i] = (x[0] - plant.xdtraj[:,0].min()) / 14 # x
posn[7, i] = 0 # z
posn[8, i] = x[1] # y
posn[9, i] = 0 # yz plane
posn[10, i] = np.pi / 2 - x[4] # pitch down
posn[11, i] = 0 # yaw
posn[12, i] = -u[1] # tail angle
test_poly = PiecewisePolynomial.FirstOrderHold(times, posn)
return vis.animate(test_poly, repeat=False)
def simulateRobot(time, B, v_command):
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
controller = builder.AddSystem(DController(tree, B, v_command))
builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
def simulate_acrobot():
builder = DiagramBuilder()
acrobot = builder.AddSystem(AcrobotPlant())
saturation = builder.AddSystem(Saturation(min_value=[-10], max_value=[10]))
builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0))
wrapangles = WrapToSystem(4)
wrapangles.set_interval(0, 0, 2. * math.pi)
wrapangles.set_interval(1, -math.pi, math.pi)
wrapto = builder.AddSystem(wrapangles)
builder.Connect(acrobot.get_output_port(0), wrapto.get_input_port(0))
controller = builder.AddSystem(BalancingLQR())
builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
saturation.get_input_port(0))
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
context = simulator.get_mutable_context()
state = context.get_mutable_continuous_state_vector()
parser = argparse.ArgumentParser()
parser.add_argument("-N",
"--trials",
type=int,
help="Number of trials to run.",
default=5)
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run each sim.",
default=4.0)
args = parser.parse_args()
print(AcrobotPlant)
for i in range(args.trials):
context.set_time(0.)
state.SetFromVector(UprightState().CopyToVector() +
0.05 * np.random.randn(4, ))
simulator.StepTo(args.duration)
def __init__(self, scene_graph):
LeafSystem.__init__(self)
assert scene_graph
mbp = MultibodyPlant(0.0)
parser = Parser(mbp, scene_graph)
model_id = parser.AddModelFromFile(
FindResource("models/glider/glider.urdf"))
mbp.Finalize()
self.source_id = mbp.get_source_id()
self.body_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[0])
self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[1])
self.DeclareVectorInputPort("state", BasicVector(7))
self.DeclareAbstractOutputPort(
"geometry_pose", lambda: AbstractValue.Make(FramePoseVector()),
self.OutputGeometryPose)
drake_theta_pub_echo.publish(x_cmd[1,times])
times=times+1
#'current x state is :{}'.format(statex)
rospy.loginfo(u_cmd[:,times])
rate.sleep()
if __name__ == '__main__':
rospy.init_node('drake_control', anonymous=True)
#fig, ax = plt.subplots(2,1,figsize=(8,8))
i=0
sum_x=0
#x_pub = rospy.Publisher('/curretx', catpolexMsg, queue_size=100)
drake_cmd_pub = rospy.Publisher('/chassis_world_effort_controller/command', Float64, queue_size=100)
#drake_cmd_pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=100)
drake_cmd_pub_echo = rospy.Publisher('/drake_cmdu0_pub_echo', Float64, queue_size=100)
drake_state_pub_echo = rospy.Publisher('/drake_x_pub_echo', Float64, queue_size=100)
drake_theta_pub_echo = rospy.Publisher('/drake_theta_pub_echo', Float64, queue_size=100)
#/chassis_world_effort_controller/command
# sub = rospy.Subscriber("/curretx",catpolexMsg,currentCallback, queue_size=1, buff_size=100)#1000HZ
plant = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)
file_name = FindResource("/home/cby/catkin_ws/src/cartpole/urdf/cartpole2.urdf")
xu_cmd=drake_trajectory_generation(file_name)#u_cmd with 400points effort input
u_cmd=u_cmd_drake
x_cmd=x_cmd_drake
rospy.loginfo("trajectory complete!! sending")
drake_cmd_publisher(u_cmd,x_cmd)
#rospy.spin()
import argparse
import numpy as np
from pydrake.all import (DiagramBuilder, FloatingBaseType, RigidBodyPlant,
RigidBodyTree, Simulator, VectorSystem)
from underactuated import (FindResource, PlanarRigidBodyVisualizer,
SliderSystem)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
builder = DiagramBuilder()
acrobot = builder.AddSystem(RigidBodyPlant(tree))
parser = argparse.ArgumentParser()
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-4., 4.],
ylim=[-4., 4.]))
builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, 'Torque', -5., 5.))
builder.Connect(torque_system.get_output_port(0),
acrobot.get_input_port(0))
import argparse
import numpy as np
from pydrake.all import (DiagramBuilder, FloatingBaseType, RigidBodyPlant,
RigidBodyTree, Simulator, VectorSystem)
from underactuated import (FindResource, PlanarRigidBodyVisualizer,
SliderSystem)
tree = RigidBodyTree(FindResource("cartpole/cartpole.urdf"),
FloatingBaseType.kFixed)
builder = DiagramBuilder()
cartpole = builder.AddSystem(RigidBodyPlant(tree))
parser = argparse.ArgumentParser()
parser.add_argument("-T", "--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-2.5, 2.5],
ylim=[-1, 2.5]))
builder.Connect(cartpole.get_output_port(0), visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, 'Force', -30., 30.))
builder.Connect(torque_system.get_output_port(0),
cartpole.get_input_port(0))
import argparse
import numpy as np
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
PlanarSceneGraphVisualizer, Simulator)
from underactuated import FindResource, SliderSystem
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("cartpole/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Force", -30., 30.))
builder.Connect(torque_system.get_output_port(0),
plant.get_actuation_input_port())
import matplotlib.pyplot as plt
from pydrake.all import (DirectCollocation, FloatingBaseType,
PiecewisePolynomial, RigidBodyTree, RigidBodyPlant,
SolutionResult)
#The following imports come from Mark Petersen's quadrotor example
from pydrake.all import (ConstantVectorSource, DiagramBuilder, FramePoseVector,
MeshcatVisualizer, SceneGraph, Simulator)
from pydrake.all import (MultibodyPlant, Parser)
from underactuated import (FindResource, PlanarRigidBodyVisualizer)
#Create Plant
tree = RigidBodyTree(
FindResource("diff_drive/diff_drive.urdf"), FloatingBaseType.kRollPitchYaw
) #Use same floating base as 2D quadrotor, or perhaps 3D, kRollPitchYaw will likely be best
plant = RigidBodyPlant(tree)
context = plant.CreateDefaultContext()
builder = DiagramBuilder()
diff_drive = builder.AddSystem(RigidBodyPlant(tree))
diagram = builder.Build()
context = simulator.get_mutable_context()
x0 = np.zeros((4, 1))
context.SetContinuousState(x0)
#Setup Controller
#Setup Visualization
import argparse
import numpy as np
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, Parser,
PlanarSceneGraphVisualizer, Simulator)
from underactuated import FindResource, SliderSystem
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
file_name = FindResource("models/cartpole.urdf")
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10000.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-2.5, 2.5], ylim=[-1, 2.5]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
ax = visualizer.fig.add_axes([.2, .95, .6, .025])
torque_system = builder.AddSystem(SliderSystem(ax, "Force", -30., 30.))
builder.Connect(torque_system.get_output_port(0),
plant.get_actuation_input_port())
# unstable equilibrium point
x_star = [0, np.pi, 0, 0]
# weight matrices for the lqr controller
Q = np.eye(4)
R = np.eye(1)
"""Then we construct the block diagram with the cart-pole in closed loop with the LQR controller."""
# start construction site of our block diagram
builder = DiagramBuilder()
# instantiate the cart-pole and the scene graph
cartpole, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
urdf_path = FindResource('models/undamped_cartpole.urdf')
Parser(cartpole).AddModelFromFile(urdf_path)
cartpole.Finalize()
# set the operating point (vertical unstable equilibrium)
context = cartpole.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x_star)
# fix the input port to zero and get its index for the lqr function
cartpole.get_actuation_input_port().FixValue(context, [0])
input_i = cartpole.get_actuation_input_port().get_index()
# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
lqr = builder.AddSystem(lqr)
def getPredictedMotion(B, v_command, time):
#object_positions = object_positions + 0.1
#manipulator_positions = manipulator_positions + 0.1
#object_velocities = object_velocities + 0.1
#manipulator_velocities = manipulator_velocities + 0.1
#object_positions = object_positions[:, range(0,object_positions.shape[1],2)]
#manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)]
#object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)]
#manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)]
#times = times[range(0,times.size,2)]
#import pdb; pdb.set_trace()
step = 0.01
A = 10 * np.eye(3)
tree = RigidBodyTree(
FindResource(
os.path.dirname(os.path.realpath(__file__)) +
"/block_pusher2.urdf"), FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
#robot = builder.AddSystem(RigidBodyPlant(tree))
robot = builder.AddSystem(
QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B)))
controller = builder.AddSystem(QController(tree, v_command, step))
#builder.Connect(robot.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), robot.get_input_port(0))
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
Tview,
xlim=[-2.8, 4.8],
ylim=[-2.8, 10]))
builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0))
logger = builder.AddSystem(
SignalLogger(tree.get_num_positions() + tree.get_num_velocities()))
builder.Connect(robot.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# Set the initial conditions
context = simulator.get_mutable_context()
state = context.get_mutable_discrete_state_vector()
start1 = 3 * np.pi / 16
start2 = 15 * np.pi / 16
#np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps
state.SetFromVector(
(start1, start2, -start1, -start2, np.pi + start1, start2,
np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.,
0., 0.)) # (theta1, theta2, theta1dot, theta2dot)
# Simulate for 10 seconds
simulator.StepTo(time)
#import pdb; pdb.set_trace()
return (logger.data()[8:11, :], logger.data()[:8, :],
logger.data()[19:22, :], logger.data()[11:19, :],
logger.sample_times())
In particular, we will not use it for simulation.
The reason for this resides in the scuffing issue described above: when the swing foot is sliding on the ground at zero height, any good physic simulator detects a collision and applies a friction force to the foot.
This would cause the compass gait ot stumble and fall down, no matter the trajectory we found.
Note however that more complex robots, such as the [kneed walker](http://underactuated.mit.edu/simple_legs.html#kneed_walker), do not have this issue, and, for these, the workflow we use in this notebook can be used both for the detection of the limit cycle and for simulation.
**An important implementation detail.**
Behind the scenes, the optimization solvers we use require the knowledge of the derivaties of the cost function and the constraint values with respect to the decision variables.
This is needed to understand in which direction the current solution should be corrected to find a feasible point or reduce the cost.
This process used to be very tedious some years ago, when graduate students had to spend many hours writing down these derivatives "by hand".
Nowadays, we use [automatic differentiation](https://en.wikipedia.org/wiki/Automatic_differentiation), which through the construction of a computational graph is able to evaluate a function and its derivatives very quickly and exactly (cf. [finite difference](https://en.wikipedia.org/wiki/Finite_difference)).
To allow the evaluation of the `MultibodyPlant` functions (e.g. the mass matrix method) with `AutoDiffXd` variables, we need to call the `MultibodyPlant.ToAutoDiffXd()` function which returns a copy of the `MultibodyPlant` that can work with autodiff variables.
"""
# parse urdf and create the MultibodyPlant
compass_gait = MultibodyPlant(time_step=0)
file_name = FindResource('models/compass_gait_limit_cycle.urdf')
Parser(compass_gait).AddModelFromFile(file_name)
compass_gait.Finalize()
# overwrite MultibodyPlant with its autodiff copy
compass_gait = compass_gait.ToAutoDiffXd()
# number of configuration variables
nq = compass_gait.num_positions()
# number of components of the contact forces
nf = 2
"""## Helper Functions for the `MathematicalProgram`
When writing a `MathematicalProgram` in Drake, optimization variables are `symbolic.Variable` objects.
# Desired pendulum parameters.
length = 2.
b = .1
# Control gains for stabilizing the second joint.
kp = 1
kd = .1
# Cancel double pend dynamics and inject single pend dynamics.
torque[:] = Cv - tauG + \
M.dot([-self.g / length * sin(q[0]) - b * v[0],
-kp * q[1] + kd * v[1]])
# Load the double pendulum from Universal Robot Description Format
tree = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
FloatingBaseType.kFixed)
# Set up a block diagram with the robot (dynamics), the controller, and a
# visualization block.
builder = DiagramBuilder()
robot = builder.AddSystem(RigidBodyPlant(tree))
parser = argparse.ArgumentParser()
parser.add_argument("-g",
"--gravity",
type=float,
help="Gravitational constant (m/s^2).",
default=9.8)
parser.add_argument("-T",
"--duration",
When writing the controller, we will take care of the first (position of the base) and ensure that it oscillates as required.
Then the problem will be reduced to the control of the pendulum only.
"""
# think of the builder as the construction site of our block diagram
builder = DiagramBuilder()
# instantiate the vibrating pendulum and the scene graph
# the scene graph is a container for the geometries of all the physical systems in our diagram
vibrating_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
builder,
time_step=0. # discrete update period , set to zero since system is continuous
)
# parse the urdf and populate the vibrating pendulum
urdf_path = FindResource('models/vibrating_pendulum.urdf')
Parser(vibrating_pendulum).AddModelFromFile(urdf_path)
vibrating_pendulum.Finalize()
"""## Write the Controller
In this section we define two controllers:
1. An inner controller that makes the base oscillate with the harmonic motion. We wrote this for you.
2. The outer controller to make the pendulum spin at constant velocity. You will write part of this.
The final diagram will have the following structure:
"""
# this controller enforces the harmonic motion to the base
class InnerController(VectorSystem):
from pydrake.all import (AddMultibodyPlantSceneGraph,
DiagramBuilder,
Parser,
Simulator)
from underactuated import FindResource, PlanarSceneGraphVisualizer
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
from underactuated import (FindResource, PlanarRigidBodyVisualizer)
timestep = 0.0
# tree = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
# FloatingBaseType.kFixed)
# tree = RigidBodyTree(FindResource("cubli/cubli.sdf"),
# FloatingBaseType.kFixed)
builder = DiagramBuilder()
# AddModelInstancesFromSdfString(
# open("underactuated/src/cubli/cubli.sdf", 'r').read(),
# FloatingBaseType.kFixed,
# None, tree)
# tree
tree = RigidBodyTree(FindResource("cubli/cubli.urdf"), FloatingBaseType.kFixed)
#
# AddFlatTerrainToWorld(tree, 1000, -1)
# plant = RigidBodyPlant(tree, timestep)
plant = RigidBodyPlant(tree)
nx = tree.get_num_positions() + tree.get_num_velocities()
allmaterials = CompliantMaterial()
allmaterials.set_youngs_modulus(1E9) # default 1E9
allmaterials.set_dissipation(1.0) # default 0.32
allmaterials.set_friction(1.0) # default 0.9.
plant.set_default_compliant_material(allmaterials)
context = plant.CreateDefaultContext()
