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