object_holder_gl_rotmat)
object_holder.set_pos(object_holder_gl_pos)
object_holder.set_rotmat(object_holder_gl_rotmat)
gm.gen_frame().attach_to(object_holder)
object_holder_copy = object_holder.copy()
object_holder_copy.attach_to(base)
# object holder goal
object_holder_gl_goal_pos = np.array([.25, -.05, .05])
object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2)
obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos,
object_holder_gl_goal_rotmat)
object_holder_goal_copy = object_holder.copy()
object_holder_goal_copy.set_homomat(obgl_goal_homomat)
object_holder_goal_copy.attach_to(base)
robot_s = cbt.Cobotta()
# robot_s.gen_meshmodel().attach_to(base)
rrtc_s = rrtc.RRTConnect(robot_s)
ppp_s = ppp.PickPlacePlanner(robot_s)
original_grasp_info_list = gpa.load_pickle_file('holder', './',
'cobg_holder_grasps.pickle')
manipulator_name = "arm"
hand_name = "hnd"
start_conf = robot_s.get_jnt_values(manipulator_name)
conf_list, jawwidth_list, objpose_list = \
ppp_s.gen_pick_and_place_motion(hnd_name=hand_name,
objcm=object_holder,
grasp_info_list=original_grasp_info_list,
start_conf=start_conf,
end_conf=start_conf,
import torch
import math
import time
import numpy as np
import pandas as pd
import neuro.ik.cobotta_fitting as cbf
import basis.robot_math as rm
import modeling.geometric_model as gm
import visualization.panda.world as world
import robot_sim.robots.cobotta.cobotta as cbt_s
if __name__ == '__main__':
base = world.World(cam_pos=np.array([1.5, 1, .7]))
gm.gen_frame().attach_to(base)
rbt_s = cbt_s.Cobotta()
rbt_s.fk(jnt_values=np.zeros(6))
rbt_s.gen_meshmodel(toggle_tcpcs=True, rgba=[.5,.5,.5,.3]).attach_to(base)
rbt_s.gen_stickmodel(toggle_tcpcs=True).attach_to(base)
tgt_pos = np.array([.25, .2, .2])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi * 3 / 3)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
contraint_pos = rbt_s.manipulator_dict['arm'].jnts[5]['gl_posq']
contraint_rotmat = rbt_s.manipulator_dict['arm'].jnts[5]['gl_rotmatq']
gm.gen_frame(pos=contraint_pos, rotmat=contraint_rotmat).attach_to(base)
# numerical ik
jnt_values = rbt_s.ik(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat)
rbt_s.fk(jnt_values=jnt_values)
rbt_s.gen_meshmodel(toggle_tcpcs=True, rgba=[.5,.5,.5,.3]).attach_to(base)
rbt_s.gen_stickmodel(toggle_tcpcs=True).attach_to(base)