forked from humans-to-robots-motion/tp-rmp
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: An Thai Le <an.thai.le97@gmail.com>
- Loading branch information
Showing
11 changed files
with
430 additions
and
71 deletions.
There are no files selected for viewing
Binary file not shown.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,66 @@ | ||
| import sys | ||
| import argparse | ||
| import numpy as np | ||
| from os.path import join, dirname, abspath | ||
| import matplotlib | ||
| import logging | ||
| logging.basicConfig() | ||
| logging.getLogger().setLevel(logging.INFO) | ||
| matplotlib.rcParams['pdf.fonttype'] = 42 | ||
| matplotlib.rcParams['ps.fonttype'] = 42 | ||
| matplotlib.rcParams['font.size'] = 16 | ||
|
|
||
| ROOT_DIR = join(dirname(abspath(__file__)), '..') | ||
| sys.path.append(ROOT_DIR) | ||
| from tprmp.utils.loading import load # noqa | ||
| from tprmp.visualization.demonstration import plot_demo, _equalize_axes # noqa | ||
| from tprmp.visualization.dynamics import plot_potential_force_components, plot_weight_map # noqa | ||
| from tprmp.visualization.models import _plot_gmm_global # noqa | ||
| from tprmp.models.tp_rmp import TPRMP # noqa | ||
| from tprmp.demonstrations.base import Demonstration # noqa | ||
| from tprmp.demonstrations.manifold import Manifold # noqa | ||
|
|
||
| parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter, | ||
| description='Example run: python test_tprmp.py test.p') | ||
| parser.add_argument('--loading', help='Load or not', type=bool, default=True) | ||
| parser.add_argument('--task', help='The task folder', type=str, default='test') | ||
| parser.add_argument('--demo', help='The data file', type=str, default='test3.p') | ||
| parser.add_argument('--data', help='The data file', type=str, default='test3.p') | ||
| args = parser.parse_args() | ||
|
|
||
| DATA_DIR = join(ROOT_DIR, 'data', 'tasks', args.task, 'demos') | ||
| data_file = join(DATA_DIR, args.demo) | ||
| # parameters | ||
| dt = 0.01 | ||
| NUM_COMP = 5 | ||
| alpha, beta = 0., 0. | ||
| stiff_scale = 1. | ||
| mass_scale = 1. | ||
| tau = 0.5 | ||
| delta = 2. | ||
| potential_method = 'huber' | ||
| train_method = 'match_energy' | ||
| d_min = 0. | ||
| d_scale = 1. | ||
| energy = 0. | ||
| var_scale = 2. | ||
| res = 0.01 | ||
| max_z = 1000 | ||
| margin = 0.5 | ||
| verbose = False | ||
| # load data | ||
| data = load(data_file) | ||
| demos = [] | ||
| manifold = Manifold.get_euclidean_manifold(2) | ||
| for d in data: | ||
| traj = np.stack(d) | ||
| demo = Demonstration(traj, manifold=manifold, dt=dt) | ||
| demo.add_frame_from_pose(traj[:, 0], 'start') | ||
| demo.add_frame_from_pose(traj[:, -1], 'end') | ||
| demos.append(demo) | ||
| sample = demos[0] | ||
| frames = sample.get_task_parameters() | ||
| model = TPRMP.load(args.task, model_name=args.data) | ||
| x = np.array([2., 2.5]) | ||
| plot_weight_map(model.model, frames, margin=margin, res=res, new_fig=True, show=False) | ||
| plot_potential_force_components(model, frames, x, show=True) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,53 @@ | ||
| import sys | ||
| import argparse | ||
| import numpy as np | ||
| import matplotlib.pyplot as plt | ||
| from os.path import join, dirname, abspath | ||
| import logging | ||
| logging.basicConfig() | ||
| logging.getLogger().setLevel(logging.INFO) | ||
|
|
||
| ROOT_DIR = join(dirname(abspath(__file__)), '..') | ||
| sys.path.append(ROOT_DIR) | ||
| from tprmp.utils.loading import load_demos_2d # noqa | ||
| from tprmp.visualization.demonstration import plot_demo # noqa | ||
| from tprmp.visualization.dynamics import plot_dissipation_field, plot_potential_field, visualize_rmp # noqa | ||
| from tprmp.models.tp_rmp import TPRMP # noqa | ||
| from tprmp.demonstrations.base import Demonstration # noqa | ||
| from tprmp.demonstrations.frame import Frame # noqa | ||
|
|
||
| parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter, | ||
| description='Example run: python test_tprmp.py test.p') | ||
| parser.add_argument('--task', help='The task folder', type=str, default='test') | ||
| parser.add_argument('--data', help='The data file', type=str, default='test3.p') | ||
| parser.add_argument('--plot', help='The data file', type=str, default='dissipative') | ||
| args = parser.parse_args() | ||
| # parameters | ||
| T = 200 | ||
| dt = 0.01 | ||
| res = 0.05 | ||
| max_z = 1000 | ||
| margin = 0.5 | ||
| start_v = 0. | ||
| end_v = 0.6 | ||
| limits = [0., 4.] | ||
| model = TPRMP.load(args.task, model_name=args.data) | ||
| manifold = model.model.manifold | ||
| start_pose = np.array([1.4, 1.1]) | ||
| end_pose = np.array([3.1, 2.6]) | ||
| plt.figure() | ||
| for t in range(T): | ||
| start_pose += start_v * dt | ||
| end_pose += end_v * dt | ||
| A, b = Demonstration.construct_linear_map(manifold, start_pose) | ||
| start_frame = Frame(A, b, manifold=manifold) | ||
| A, b = Demonstration.construct_linear_map(manifold, end_pose) | ||
| end_frame = Frame(A, b, manifold=manifold) | ||
| frames = {'start': start_frame, 'end': end_frame} | ||
| if args.plot == 'potential': | ||
| plot_potential_field(model, frames, margin=margin, max_z=max_z, three_d=True, res=res, limits=limits, new_fig=False, show=False) | ||
| elif args.plot == 'dissipative': | ||
| plot_dissipation_field(model, frames, margin=margin, res=res, limits=limits, new_fig=False, show=False) | ||
| plt.draw() | ||
| plt.pause(0.00001) | ||
| plt.cla() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| import sys | ||
| import argparse | ||
| import numpy as np | ||
| import pybullet as p | ||
| from os.path import join, dirname, abspath | ||
| import logging | ||
| logging.basicConfig() | ||
| logging.getLogger().setLevel(logging.INFO) | ||
|
|
||
| ROOT_DIR = join(dirname(abspath(__file__)), '..') | ||
| sys.path.append(ROOT_DIR) | ||
| from tprmp.utils.loading import load_demos # noqa | ||
| from tprmp.visualization.demonstration import plot_demo # noqa | ||
| from tprmp.visualization.dynamics import plot_heatmap_3d # noqa | ||
| from tprmp.visualization.models import _plot_gmm_global # noqa | ||
| from tprmp.models.tp_rmp import TPRMP # noqa | ||
| from tprmp.demonstrations.base import Demonstration # noqa | ||
| from tprmp.demonstrations.frame import Frame # noqa | ||
| from tprmp.demonstrations.quaternion import q_convert_wxyz, q_from_euler, q_convert_xyzw # noqa | ||
| from tprmp.envs.gym import Environment # noqa | ||
| from tprmp.envs.tasks import PickBox # noqa | ||
|
|
||
| parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter, | ||
| description='Example run: python test_tprmp.py') | ||
| parser.add_argument('--loading', help='Load or not', type=bool, default=True) | ||
| parser.add_argument('--task', help='The task folder', type=str, default='pick') | ||
| parser.add_argument('--demo', help='The data file', type=str, default='sample2.p') | ||
| parser.add_argument('--data', help='The data file', type=str, default='huber.p') | ||
| args = parser.parse_args() | ||
|
|
||
| DATA_DIR = join(ROOT_DIR, 'data', 'tasks', args.task, 'demos') | ||
| data_file = join(DATA_DIR, args.demo) | ||
| # parameters | ||
| N = 5 | ||
| T = 5 | ||
| obj_v = 0.05 * np.array([1., 1., 0.]) | ||
| NUM_COMP = 10 | ||
| alpha, beta = 0., 0. | ||
| stiff_scale = 1. | ||
| mass_scale = 0.2 | ||
| tau = 0.5 | ||
| delta = 3. | ||
| potential_method = 'huber' | ||
| train_method = 'match_energy' | ||
| d_min = 0. | ||
| d_scale = 1. | ||
| energy = 0. | ||
| var_scale = 3. | ||
| res = 0.01 | ||
| margin = 0.05 | ||
| verbose = False | ||
| r = 0.05 | ||
| # load data | ||
| demos = load_demos(data_file, tag='pick_side') | ||
| dt = demos[0].dt | ||
| sampling_hz = int(1. / demos[0].dt) | ||
| manifold = demos[0].manifold | ||
| # plot_demo(demos, only_global=False, plot_quat=False, new_fig=True, new_ax=True, show=True) | ||
| # train tprmp | ||
| if args.loading: | ||
| model = TPRMP.load(args.task, model_name=args.data) | ||
| else: | ||
| model = TPRMP(num_comp=NUM_COMP, name=args.task, stiff_scale=stiff_scale, mass_scale=mass_scale, var_scale=var_scale, tau=tau, delta=delta, potential_method=potential_method, d_scale=d_scale) | ||
| model.train(demos, alpha=alpha, beta=beta, train_method=train_method, d_min=d_min, energy=energy, verbose=verbose) | ||
| model.save(name=args.data) | ||
| # model.model.plot_model(demos, var_scale=1.) | ||
| # test tprmp | ||
| env = Environment(task=PickBox(), disp=True, sampling_hz=sampling_hz) | ||
| ee_pose = np.array(demos[0].traj[:, 0]) | ||
| A, b = Demonstration.construct_linear_map(manifold, ee_pose) | ||
| ee_frame = Frame(A, b, manifold=manifold) | ||
| box_id = env.task.box_id | ||
| obj_rotation = q_convert_xyzw(q_from_euler(np.array([np.pi/2, 0., 0.]))) | ||
| for _ in range(N): | ||
| start_pose = np.array(demos[0].traj[:, 0]) | ||
| start_pose[3:] = q_convert_xyzw(start_pose[3:]) | ||
| obj_position = np.array([0.5, -0.25, env.task.box_size[1] / 2]) + np.random.uniform(low=-r, high=r) * np.array([1, 1, 0]) | ||
| env.setp(start_pose) | ||
| env._ee_pose = start_pose | ||
| env._ee_vel = np.zeros(6) | ||
| for t in np.linspace(0, T, T * env.sampling_hz + 1): | ||
| obj_position += obj_v * dt | ||
| p.resetBasePositionAndOrientation(box_id, obj_position, obj_rotation) | ||
| target = p.getBasePositionAndOrientation(box_id) | ||
| obj_pose = np.append(obj_position, q_convert_wxyz(obj_rotation)) | ||
| A, b = Demonstration.construct_linear_map(manifold, obj_pose) | ||
| obj_frame = Frame(A, b, manifold=manifold) | ||
| frames = {'ee_frame': ee_frame, 'obj_frame': obj_frame} | ||
| x = np.append(env.ee_pose[:3], q_convert_wxyz(env.ee_pose[3:])) | ||
| dx = env.ee_vel | ||
| ddx = model.retrieve(x, dx, frames=frames) | ||
| env.step(ddx) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.