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, draw_collision=True)
return rbt, pbrv
from pydrake.all import (Box,
ConstantVectorSource,
DiagramBuilder,
FindResourceOrThrow,
FloatingBaseType,
Isometry3,
RigidBodyTree,
SignalLogger,
Simulator,
VisualElement)
from pydrake.examples.compass_gait import (CompassGait, CompassGaitParams)
from underactuated import (PlanarRigidBodyVisualizer)
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
def make_real_dircol_mp(expmt="cartpole", seed=1776):
global tree
global plant
global context
global dircol
# TODO: use the seed in some meaningful way:
# https://github.com/RobotLocomotion/drake/blob/master/systems/stochastic_systems.h
assert expmt in ("cartpole", "acrobot")
# expmt = "cartpole" # State: (x, theta, x_dot, theta_dot) Input: x force
# expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque
if expmt == "cartpole":
tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf",
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
else:
tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf",
FloatingBaseType.kFixed)
plant = AcrobotPlant()
context = plant.CreateDefaultContext()
if expmt == "cartpole":
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.1,
maximum_timestep=0.4)
else:
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.05,
maximum_timestep=0.2)
dircol.AddEqualTimeIntervalsConstraints()
if expmt == "acrobot":
# Add input limits.
torque_limit = 8.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
# More elegant version is blocked on drake #8315:
# dircol.AddLinearConstraint(dircol.initial_state() == initial_state)
if expmt == "cartpole":
final_state = (0., math.pi, 0., 0.)
else:
final_state = (math.pi, 0., 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
# dircol.AddLinearConstraint(dircol.final_state() == final_state)
# R = 10 # Cost on input "effort".
# u = dircol.input()
# dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
return dircol, tree
def RunSimulation(robobee_plantBS_torque, control_law, x0=np.random.random((13, 1)), duration=30):
robobee_controller = RobobeeController(control_law)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(RobobeePlantAero(
m = robobee_plantBS_torque.m,
Ixx = robobee_plantBS_torque.Ixx,
Iyy = robobee_plantBS_torque.Iyy,
Izz = robobee_plantBS_torque.Izz,
g = robobee_plantBS_torque.g,
rw_l = robobee_plantBS_torque.rw_l,
bw = robobee_plantBS_torque.bw,
input_max = robobee_plantBS_torque.input_max))
# Rigidbody_selector = builder.AddSystem(RigidBodySelection())
print("1. Connecting plant and controller\n")
controller = builder.AddSystem(robobee_controller)
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
# Create a logger to capture the simulation of our plant
set_time_interval = 0.01
time_interval_multiple = 1;
publish_period = set_time_interval*time_interval_multiple
print("2. Connecting plant to the logger\n")
input_log = builder.AddSystem(SignalLogger(4))
# input_log._DeclarePeriodicPublish(publish_period, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(13))
# state_log._DeclarePeriodicPublish(publish_period, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Drake visualization
print("3. Connecting plant output to DrakeVisualizer\n")
rtree = RigidBodyTree(FindResourceOrThrow("drake/examples/robobee/robobee_arena.urdf"), FloatingBaseType.kQuaternion)
lcm = DrakeLcm()
visualizer = builder.AddSystem(DrakeVisualizer(tree=rtree,
lcm=lcm, enable_playback=True))
builder.Connect(plant.get_output_port(0),visualizer.get_input_port(0))
print("4. Building diagram\n")
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
print("5. Create simulation\n")
simulator = Simulator(diagram, context)
simulator.Initialize()
# simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
simulator.get_integrator().set_fixed_step_mode(True)
simulator.get_integrator().set_maximum_step_size(set_time_interval)
# Simulate for the requested duration.
simulator.StepTo(duration)
visualizer.ReplayCachedSimulation()
return input_log, state_log
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()
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))
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = dircol.Solve()
assert(result == SolutionResult.kSolutionFound)
x_trajectory = dircol.ReconstructStateTrajectory()
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
u_trajectory = dircol.ReconstructInputTrajectory()
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()
plt.plot(times, u_values)
plt.xlabel('time (seconds)')
plt.ylabel('force (Newtons)')
plt.show()
def get_figure_eight_traj_simple():
tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", tree.world(), [0, 0, 0],
[0, 0, 0])
kuka_urdf_path = getKukaUrdfPath()
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed,
world_frame, tree)
Nq = tree.get_num_positions()
eps = 1e-5
ee_idx = tree.FindBodyIndex("iiwa_link_7")
body_pt = [0, 0, 0.0635]
x = np.array([
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004, 0.6556285000000004,
0.6556285000000004, 0.6556285000000004
])
y = np.array([
0.13686922001827645, 0.1281183229143938, 0.11926059413541247,
0.11030432312999372, 0.1012577993467988, 0.09212931223448896,
0.08292715124172548, 0.0736596058171696, 0.06433496540948258,
0.05496151946732569, 0.045547557439360176, 0.03610136877424733,
0.026631242920648335, 0.017145469327224508, 0.007652337442637103,
-0.001839863284452653, -0.011322843405383448, -0.020788313471494096,
-0.030227984034123273, -0.039633565644609764, -0.0489967688542923,
-0.05830930421450961, -0.06756288227660043, -0.07674921359190354,
-0.08586000871175764, -0.09488697818749958, -0.1038655258329727,
-0.11272375591745916, -0.12145267616480321, -0.13004109492853494,
-0.13848752180117063, -0.1467858524541864, -0.1549281455978588,
-0.1629165235905652, -0.1707482796892309, -0.17841599130217783,
-0.18591882035832843, -0.1932502389635062, -0.20040076105913676,
-0.20735798772338723, -0.21411519853223893, -0.22066551450768562,
-0.2269966386554578, -0.2331053523296066, -0.23897989082424964,
-0.24461177603116246, -0.24999586174091798, -0.255127737887603,
-0.2600037985005273, -0.264623973362925, -0.2689800086072113,
-0.27306787696210033, -0.2768840041676995, -0.2804279779836409,
-0.28369462501981374, -0.28667669709392635, -0.2893761036490012,
-0.29178950599107006, -0.29391158123626243, -0.29574034972478985,
-0.29727453396452347, -0.29851358128156347, -0.2994544451742904,
-0.30009482660988346, -0.30043907902783173, -0.30048644363405164,
-0.3002369544043975, -0.29968819678814346, -0.2988444313574429,
-0.2977048064508765, -0.2962719042333858, -0.29454317198576974,
-0.2925200231035754, -0.2902045731500091, -0.2876022809123594,
-0.2847133796914938, -0.28154140071122247, -0.27809048676073667,
-0.27436270583704647, -0.2703665745543449, -0.26610524813698555,
-0.26158238012185503, -0.25680474076422527, -0.2517742032662073,
-0.2464935808285531, -0.24096598110942027, -0.23519855715870805,
-0.22919065210989617, -0.22294815964121067, -0.216477551020404,
-0.20978904844956445, -0.20289009717093992, -0.19578529751522875,
-0.18849136560284127, -0.1810102720333381, -0.17335332623615154,
-0.16552932116242403, -0.15754446533330008, -0.1494055335341655,
-0.141119776212369, -0.13269158757842503, -0.12412621123921029,
-0.11543344583116043, -0.10661783052843572, -0.09768974155699829,
-0.08866263788534111, -0.07955032956578706, -0.07036309089673469,
-0.06111613853088287, -0.051824258760106816, -0.04250384833178472,
-0.03316965050170948, -0.02383739609051122, -0.014517898480146497,
-0.00521285872839692, 0.004084069854356758, 0.013377716369566438,
0.022678964415954628, 0.03199791841264746, 0.04134623887290685,
0.05072829056150665, 0.06014998301207159, 0.06960936374549687,
0.07908892160637689, 0.08855068744478754, 0.09794356384062872,
0.10720878615596977, 0.11629326145552335, 0.12514991959047528,
0.13374545516319478, 0.14207020091055017, 0.15012393938707272,
0.15792746608076888, 0.16551126865840357, 0.1729042968130615,
0.18013571994835326, 0.1872239892183252, 0.19418299168723718,
0.20100573965434845, 0.2076839316858779, 0.2142059797503866,
0.22054968628960595, 0.22669845070103078, 0.2326348093954518,
0.2383522458039774, 0.24384287073319427, 0.2491022174907651,
0.2541293098371375, 0.2589202699947854, 0.2634683865999179,
0.2677700650071298, 0.27181899023759926, 0.2756002672767167,
0.2791048721375289, 0.28231577838682104, 0.28522354073704403,
0.2878230537497963, 0.2901076694828819, 0.2920783437903407,
0.29374310403406084, 0.295107993386028, 0.2961895263765827,
0.2970056784978085, 0.2975819271104611, 0.29794495005548816,
0.298109264082986, 0.2980740741579594, 0.2978323971097816,
0.2973591836734637, 0.296622924641549, 0.2955794967043364,
0.29418154658089085, 0.2923728011959445, 0.2900962060295761,
0.2873037962354698, 0.28396753800504204, 0.28032650994085717,
0.2763886650341475, 0.27216195627614015, 0.26765433665806226,
0.2628737591711411, 0.2578281768066035, 0.25252554255567694,
0.2469738094095883, 0.2411809303595648, 0.2351548583968336,
0.2289035465126218, 0.22243494769815644, 0.21575701494466482,
0.208877701243374, 0.201804959585511, 0.19454674296230312,
0.1871110043649775, 0.179505696784761, 0.1717387732128811,
0.16381818664056474, 0.1557518900590391, 0.14754783645953137,
0.1392139788332685, 0.1307582701714778
])
z = np.array([
0.31756840547539744, 0.32748579299353975, 0.3378005717933035,
0.3484792783644661, 0.3594884491968049, 0.3707946207800972,
0.38236432960412026, 0.39416411215865155, 0.40616050493346834,
0.4183200444183479, 0.4306092671030677, 0.44299470947740494,
0.455442908031137, 0.4679203992540413, 0.48039371963589506,
0.49282940566647565, 0.5051939938355604, 0.5174540206329267,
0.5295760225483518, 0.541526536071613, 0.5532720976924878,
0.5647792439007534, 0.5760145111861872, 0.5869444360385665,
0.5975355549476685, 0.607754404403259, 0.617684862624283,
0.6269833110602612, 0.6356398015809976, 0.6436439919363848,
0.6509877754875409, 0.6576613603932012, 0.6636610682008525,
0.6689826360732843, 0.67361769235995, 0.6775587102765505,
0.6807894425694675, 0.6832982853518634, 0.6850685995152646,
0.6860878343790213, 0.6863475050397387, 0.6858378423067206,
0.6845542073433015, 0.6824970237151333, 0.679674950734998,
0.6760903644476528, 0.6717601780334646, 0.6666986364356934,
0.6609289904640975, 0.6544774785385726, 0.6473731907857357,
0.6396420054814256, 0.6313175949625012, 0.6224353167682047,
0.6130293234274724, 0.6031408593626014, 0.592806201725962,
0.5820708170060365, 0.5709740379170426, 0.5595578699404182,
0.5478698928627949, 0.5359537973679934, 0.5238581689279385,
0.5116302430406027, 0.4993208702348768, 0.48697884889804044,
0.4746532265736393, 0.4623956012593628, 0.4502485492310045,
0.4382596910462214, 0.42647866542842733, 0.41494764320821875,
0.4037154159777041, 0.39282851027202587, 0.3823335341683771,
0.37227688282859595, 0.36269856168881953, 0.3536372194160615,
0.3451304536909037, 0.33721132075471055, 0.32990808625336293,
0.32324779496703726, 0.3172587080115004, 0.31196097533352374,
0.3073723368592661, 0.3035111825862362, 0.3003931753833552,
0.29803437903462704, 0.2964420439874006, 0.29562574916758594,
0.2955914743255737, 0.29633569503272444, 0.29786307504133164,
0.3001661781693752, 0.30323465990169074, 0.3070520940451594,
0.3115998595664622, 0.31685456748736623, 0.3227967201975068,
0.32939373258736826, 0.336620401368088, 0.34444137126548074,
0.35282689981652365, 0.3617486647489115, 0.37117885795827055,
0.3810878162585716, 0.39144908831456365, 0.4022280096944361,
0.41338530657545247, 0.42488278556704895, 0.43667841400710394,
0.4487240639652002, 0.46096913859882693, 0.47336419850958344,
0.48584480962195276, 0.4983474665337738, 0.5108078008561817,
0.5231640943169621, 0.5353543092708679, 0.5473260617454424,
0.5590277695983947, 0.5704169154454125, 0.5814512174681081,
0.5920942512854084, 0.6023199392964617, 0.6120999705542476,
0.6214094905886763, 0.6302189177557694, 0.6384948605857313,
0.6462095585403849, 0.6533240617001391, 0.6598152959296799,
0.6656559786178493, 0.6708180864798622, 0.6752789417653723,
0.6790134023420558, 0.6819951301275096, 0.6841972139799184,
0.6856012933475145, 0.6861863032005122, 0.6859428344922742,
0.6848659833744687, 0.6829691340687087, 0.6802671521438632,
0.6767869940428872, 0.6725559197263683, 0.6675988289655357,
0.6619506036376687, 0.6556389703050904, 0.6486908922059266,
0.6411394935332501, 0.6330153548211616, 0.6243554553896911,
0.6151955559582207, 0.6055661079526033, 0.5954998822170263,
0.5850395750753018, 0.5742180207932119, 0.563083467349193,
0.5516783030193103, 0.5400466850891197, 0.5282336225055959,
0.5162846587691582, 0.5042482411832561, 0.49216898910506657,
0.4800977462683095, 0.4680759949262634, 0.45614115835012375,
0.4443280391401759, 0.4326669709393109, 0.42118572108087715,
0.40991017010577024, 0.3988536909103188, 0.3880358943577357,
0.37746325851094437, 0.3677153816185399, 0.3583626678734707,
0.34943548878219216, 0.34096421585115233, 0.33297922058679913,
0.3255108744955808, 0.3185895490839454, 0.31224561585834093,
0.30650944632521565, 0.30141141199101745, 0.2969818843621946,
0.29325123494519506, 0.29024983524646697, 0.28800805677245844,
0.2865562710296175, 0.28592484952439223, 0.2861441637632308,
0.28724458525258123, 0.2892564854988917, 0.2922102360086102,
0.2961362082881848, 0.30106477384406377, 0.30702630418269494,
0.3140511708105266, 0.32216974523400665
])
newx = np.array([])
newy = np.array([])
newz = np.array([])
for i in range(200):
newx = np.concatenate((newx, getInterp(x, i)))
newy = np.concatenate((newy, getInterp(y, i)))
newz = np.concatenate((newz, getInterp(z, i)))
# xyz = np.vstack((x, y, z))
xyz = np.vstack((newx, newy, newz))
# compute inverse kinematics
ik_options = IKoptions(tree)
ik_options.setDebug(1)
q_seed = np.array([0, np.pi / 4, 0, -np.pi / 4, 0, -np.pi / 4, 0])
print q_seed
for i in range(len(x)):
ee_des = xyz[:, i]
cnstr = WorldPositionConstraint(tree, ee_idx, body_pt, ee_des - eps,
ee_des + eps)
res = InverseKin(tree, q_seed, q_seed, [cnstr], ik_options)
print res.q_sol[0]
print res.info
kinsol = tree.doKinematics(res.q_sol[0])
ee_idx = tree.FindBodyIndex("iiwa_link_7")
print tree.transformPoints(kinsol, np.zeros(3), 0, ee_idx)
break