if __name__ == '__main__':
import time
import robot_sim.robots.yumi.yumi as ym
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.annotation.utils as gutil
import numpy as np
import basis.robot_math as rm
base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2])
gm.gen_frame().attach_to(base)
objcm = cm.CollisionModel('tubebig.stl')
robot_s = ym.Yumi(enable_cc=True)
manipulator_name = 'rgt_arm'
hand_name = 'rgt_hnd'
start_conf = robot_s.get_jnt_values(manipulator_name)
goal_homomat_list = []
for i in range(6):
goal_pos = np.array([.55, -.1, .3]) - np.array([i * .1, i * .1, 0])
# goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
goal_rotmat = np.eye(3)
goal_homomat_list.append(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm = objcm.copy()
tmp_objcm.set_rgba([1, 0, 0, .3])
tmp_objcm.set_homomat(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm.attach_to(base)
grasp_info_list = gutil.load_pickle_file(objcm_name='tubebig', file_name='yumi_tube_big.pickle')
grasp_info = grasp_info_list[0]
if toggle_tcp_list:
return jnt_values_list, [[pos_list[i], rotmat_list[i]]
for i in range(len(pos_list))]
else:
return jnt_values_list
if __name__ == '__main__':
import time
import robot_sim.robots.yumi.yumi as ym
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[1.5, 0, 3], lookat_pos=[0, 0, .5])
gm.gen_frame().attach_to(base)
yumi_instance = ym.Yumi(enable_cc=True)
component_name = 'rgt_arm'
start_pos = np.array([.5, -.3, .3])
start_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
goal_pos = np.array([.55, .3, .5])
goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
gm.gen_frame(pos=start_pos, rotmat=start_rotmat).attach_to(base)
gm.gen_frame(pos=goal_pos, rotmat=goal_rotmat).attach_to(base)
inik = IncrementalNIK(yumi_instance)
tic = time.time()
jnt_values_list = inik.gen_linear_motion(component_name,
start_tcp_pos=start_pos,
start_tcp_rotmat=start_rotmat,
goal_tcp_pos=goal_pos,
goal_tcp_rotmat=goal_rotmat)
toc = time.time()
import numpy as np
import math
import basis.robot_math as rm
import visualization.panda.world as wd
import robot_sim.robots.yumi.yumi as ym
import modeling.geometric_model as gm
import motion.optimization_based.incremental_nik as inik
if __name__ == "__main__":
base = wd.World(cam_pos=[3, -1, 1], lookat_pos=[0, 0, 0.5])
gm.gen_frame(length=.2).attach_to(base)
yumi_s = ym.Yumi(enable_cc=True)
inik_svlr = inik.IncrementalNIK(yumi_s)
component_name = 'rgt_arm'
# start_pos = np.array([.1, -.5, .3])
# start_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
# end_pos = np.array([.4, -.35, .45])
# end_rotmat = start_rotmat
# jnt_values_list = inik_svlr.gen_linear_motion(hnd_name, start_pos, start_rotmat, end_pos, end_rotmat)
circle_center_pos = np.array([.3, -.4, .4])
circle_ax = np.array([1, 0, 0])
radius = .1
start_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
end_rotmat = start_rotmat
jnt_values_list = inik_svlr.gen_circular_motion(component_name,
circle_center_pos,
circle_ax,
start_rotmat,
end_rotmat,
radius=radius)
for jnt_values in jnt_values_list: