def _toggle_jntcs(self,
parentmodel,
jntcs_thickness,
jntcs_length=None,
alpha=1):
"""
:param parentmodel: where to draw the frames to
:return:
author: weiwei
date: 20201125
"""
if jntcs_length is None:
jntcs_length = jntcs_thickness * 15
for id in self.jlobject.tgtjnts:
gm.gen_dashframe(pos=self.jlobject.jnts[id]['gl_pos0'],
rotmat=self.jlobject.jnts[id]['gl_rotmat0'],
length=jntcs_length,
thickness=jntcs_thickness,
alpha=alpha).attach_to(parentmodel)
gm.gen_frame(pos=self.jlobject.jnts[id]['gl_posq'],
rotmat=self.jlobject.jnts[id]['gl_rotmatq'],
length=jntcs_length,
thickness=jntcs_thickness,
alpha=alpha).attach_to(parentmodel)
def _toggle_tcpcs(self,
parentmodel,
tcp_jntid,
tcp_localpos,
tcp_localrotmat,
tcpic_rgba,
tcpic_thickness,
tcpcs_thickness=None,
tcpcs_length=None):
"""
:param parentmodel: where to draw the frames to
:param tcp_jntid: single id or a list of ids
:param tcp_localpos:
:param tcp_localrotmat:
:param tcpic_rgba: color that used to render the tcp indicator
:param tcpic_thickness: thickness the tcp indicator
:param tcpcs_thickness: thickness the tcp coordinate frame
:return:
author: weiwei
date: 20201125
"""
if tcp_jntid is None:
tcp_jntid = self.jlobject.tcp_jntid
if tcp_localpos is None:
tcp_localpos = self.jlobject.tcp_localpos
if tcp_localrotmat is None:
tcp_localrotmat = self.jlobject.tcp_localrotmat
if tcpcs_thickness is None:
tcpcs_thickness = tcpic_thickness
if tcpcs_length is None:
tcpcs_length = tcpcs_thickness * 15
tcp_globalpos, tcp_globalrotmat = self.jlobject.get_gl_tcp(
tcp_jntid, tcp_localpos, tcp_localrotmat)
if isinstance(tcp_globalpos, list):
for i, jid in enumerate(tcp_jntid):
jgpos = self.jlobject.joints[jid]['g_posq']
gm.gen_dumbbell(spos=jgpos,
epos=tcp_globalpos[i],
thickness=tcpic_thickness,
rgba=tcpic_rgba).attach_to(parentmodel)
gm.gen_frame(pos=tcp_globalpos[i],
rotmat=tcp_globalrotmat[i],
length=tcpcs_length,
thickness=tcpcs_thickness,
alpha=1).attach_to(parentmodel)
else:
jgpos = self.jlobject.joints[tcp_jntid]['g_posq']
gm.gen_dumbbell(spos=jgpos,
epos=tcp_globalpos,
thickness=tcpic_thickness,
rgba=tcpic_rgba).attach_to(parentmodel)
gm.gen_frame(pos=tcp_globalpos,
rotmat=tcp_globalrotmat,
length=tcpcs_length,
thickness=tcpcs_thickness,
alpha=1).attach_to(parentmodel)
import visualization.panda.world as wd
import modeling.geometric_model as gm
import pickle
import vision.rgb_camera.util_functions as rgbu
import cv2.aruco as aruco
import numpy as np
import basis.robot_math as rm
origin, origin_rotmat = pickle.load(open("origin.pkl", "rb"))
origin_homomat = np.eye(4)
origin_homomat[:3, :3] = origin_rotmat
origin_homomat[:3, 3] = origin
print(origin, origin_rotmat)
# base = wd.World(cam_pos=origin, lookat_pos=origin_rotmat[:,0]-origin_rotmat[:,1]+origin_rotmat[:,2])
base = wd.World(cam_pos=np.zeros(3), lookat_pos=np.array([0, 0, 10]))
gm.gen_frame(length=1, thickness=.1).attach_to(base)
# base.run()
pk_obj = pk.PyKinectAzure()
pk_obj.device_open()
pk_obj.device_start_cameras()
gm.gen_frame(pos=origin, rotmat=origin_rotmat, length=1, thickness=.03).attach_to(base)
# base.run()
# pcd_list = []
# marker_center_list = []
# def update(pk_obj, pcd_list, marker_center_list, task):
# if len(pcd_list) != 0:
# for pcd in pcd_list:
# pcd.detach()
# pcd_list.clear()
# if len(marker_center_list) != 0:
# for marker_center in marker_center_list:
def debugpos(pos, rot, base):
gm.gen_frame(pos, rot).attach_to(base)
# kadai02_01.py
# 2021/04/30 実験C第二回の課題1
# glassと星形のモデルを作成し，glass1(黄色)とstar1(黄色)間，star1(黄色)とstar2(紫色)間，glass2(紫色)とstar2(紫色)間の干渉をチェックしました．
# 凸包の三角メッシュのものは黄色で，もともとの三角メッシュのものは紫色で表示しました．
import math
import numpy as np
from basis import robot_math as rm
import modeling.geometric_model as gm
import modeling.collision_model as cm
import visualization.panda.world as wd
if __name__ == '__main__':
base = wd.World(cam_pos=np.array([.6, .3, .8]), lookat_pos=np.zeros(3))
gm.gen_frame(length=.2, thickness=.01).attach_to(base) # グローバル座標系
# glassと星形のモデルのファイルを用いてCollisionModelを初期化します
# glass1, star1はmesh(convex_hull)の設定で作成します
glass1 = cm.CollisionModel(initor="objects/glass1.stl",
cdprimit_type="surface_balls",
cdmesh_type="convex_hull")
glass1.set_rgba([.9, .75, .35, 1]) # 黄色に変更
glass1.set_pos(np.array([0, -.06, 0]))
star1 = cm.CollisionModel(initor="objects/star2.stl",
cdprimit_type="surface_balls",
cdmesh_type="convex_hull")
star1.set_rgba([.9, .75, .35, 1]) # 黄色に変更
star1.set_pos(np.array([0, .01, .07]))
star1.set_rotmat(rm.rotmat_from_axangle(axis=[0, 0, 1],
angle=math.pi / 2.))
import numpy as np
import modeling.geometric_model as gm
import visualization.panda.world as wd
import basis.robot_math as rm
import math
base = wd.World(cam_pos=np.array([.7, -.7, 1]),
lookat_pos=np.array([0.3, 0, 0]))
gm.gen_frame().attach_to(base)
sec1_spos = np.array([0, 0, 0])
sec_len = np.array([.2, 0, 0])
sec1_epos = sec1_spos + sec_len
sec2_spos = sec1_epos
angle = math.pi / 5.7
sec2_rotaxis = np.array([0, 0, 1])
sec2_rotmat = rm.rotmat_from_axangle(sec2_rotaxis, angle)
sec2_epos = sec2_spos + sec2_rotmat.dot(sec_len)
rgba = [1, .4, 0, .3]
gm.gen_stick(spos=sec1_spos, epos=sec1_epos, rgba=rgba,
thickness=.015).attach_to(base)
gm.gen_frame(pos=sec2_spos, alpha=.2).attach_to(base)
# gm.gen_stick(spos=sec2_spos, epos=sec2_spos+sec_len, rgba=rgba, thickness=.015).attach_to(base)
# gm.gen_frame(pos=sec2_spos, rotmat=sec2_rotmat, alpha=.2).attach_to(base)
gm.gen_stick(spos=sec2_spos, epos=sec2_epos, rgba=rgba,
thickness=.015).attach_to(base)
# gm.gen_circarrow(axis=sec2_rotaxis, center=sec2_rotaxis / 13+sec2_spos, starting_vector=np.array([-0,-1,0]),radius=.025, portion=.5, thickness=.003, rgba=[1,.4,0,1]).attach_to(base)
#
sec2_rotaxis2 = np.array([1, 0, 0])
sec2_rotmat2 = rm.rotmat_from_axangle(sec2_rotaxis2, angle)
new_arm_tcp_rotmat = rel_rotmat.dot(current_arm_tcp_rotmat)
last_jnt_values = rbt_s.get_jnt_values()
new_jnt_values = rbt_s.ik(tgt_pos=new_arm_tcp_pos,
tgt_rotmat=new_arm_tcp_rotmat,
seed_jnt_values=current_jnt_values)
# if new_jnt_values is None:
# continue
rbt_s.fk(jnt_values=new_jnt_values)
toc = time.time()
start_frame_id = math.ceil((toc - tic) / .01)
# rbt_x.arm_move_jspace_path([last_jnt_values, new_jnt_values], time_interval=.1, start_frame_id=start_frame_id)
onscreen.append(rbt_s.gen_meshmodel())
onscreen[-1].attach_to(base)
# print(pre_pos)
# print(onscreen)
operation_count += 1
# time.sleep(1/30)
return task.cont
if __name__ == '__main__':
gm.gen_frame(length=3, thickness=0.01).attach_to(base)
# threading.Thread(target=agv_move, args=()).start()
taskMgr.doMethodLater(1 / 60,
agv_move,
"agv_move",
extraArgs=None,
appendTask=True)
base.run()
import visualization.panda.world as wd
import modeling.collision_model as cm
import grasping.planning.antipodal as gpa
import robot_sim.end_effectors.grippers.yumi_gripper.yumi_gripper as yg
import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as hnde
from direct.gui.OnscreenText import OnscreenText
from panda3d.core import TextNode
import basis.robot_math as rm
import modeling.geometric_model as gm
base = wd.World(cam_pos=[0.06, 0.03, 0.09],
w=960,
h=540,
lookat_pos=[0, 0, 0.0])
gm.gen_frame(
length=.01,
thickness=.0005,
).attach_to(base)
# p=[[ 5.00000000e+00,8.00000000e+00, -1.00000000e+01],
# [ 5.00000000e+00, -8.00000000e+00, 1.00000000e+01],
# [-2.60197991e-12, -1.00000000e+00, -3.99900278e-07],
# [-2.60197991e-12, 1.00000000e+00, 3.99900278e-07],
# [-1.00000000e-07, -1.00000000e+01, 0.00000000e+00],
# [-1.00000000e-07, 1.00000000e+01, 0.00000000e+00]]
p = [[0, 0, 0], [0, .100, 0], [0, 0, .100], [.100, 0, 0], [.100, .100, .100],
[.100, .100, 0], [.100, 0, .100], [0, .100, .100]]
m = [[.050, 0, 0], [.050, .150, 0], [.050, 0, .150], [.150, 0, 0],
[.150, .150, .150], [.150, .150, 0], [.150, 0, .150], [.050, .150, .150]]
# mesh = trimesh.Trimesh(p).convex_hull
# body=mesh.convex_hull
# b = cm.CollisionModel(mesh)
import time
import math
import numpy as np
from basis import robot_math as rm
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import robot_sim.robots.xarm_shuidi.xarm_shuidi as xss
base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
# object
object = cm.CollisionModel("./objects/bunnysim.stl")
object.set_pos(np.array([.85, 0, .37]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
# robot_s
component_name = 'arm'
robot_s = xss.XArmShuidi()
robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
# base.run()
seed_jnt_values = robot_s.get_jnt_values(component_name=component_name)
for y in range(-5, 5):
tgt_pos = np.array([.3, y * .1, 1])
tgt_rotmat = rm.rotmat_from_euler(0, math.pi / 2, 0)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
tic = time.time()
jnt_values = robot_s.ik(component_name=component_name,
tgt_pos=tgt_pos,
tgt_rotmat=tgt_rotmat,
max_niter=500,
import numpy as np
import math, time
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
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)
# ik test
manipulator_name = 'rgt_arm'
tgt_pos = np.array([.1, -.5, .3])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
tgt_pos1 = np.array([.4, -.35, .45])
tgt_rotmat1 = tgt_rotmat
# tgt_rotmat1 = rm.rotmat_from_axangle([0, 1, 0], -math.pi / 3).dot(tgt_rotmat)
tgt_pos2 = np.array([.6, -.2, .6])
tgt_rotmat2 = tgt_rotmat
# tgt_rotmat2 = rm.rotmat_from_axangle([0, 1, 0], -math.pi/6).dot(tgt_rotmat1)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
tic = time.time()
# jnt_values = robot_s.ik(hnd_name, tgt_pos, tgt_rotmat, seed_jnt_values=np.array([.0,.0,.0,.0,.0,.0,.0]))
jnt_values = yumi_s.ik(manipulator_name, tgt_pos, tgt_rotmat)
# jnt_values1 = robot_s.ik(hnd_name, tgt_pos1, tgt_rotmat1)
# jnt_values2 = robot_s.ik(hnd_name, tgt_pos2, tgt_rotmat2)
jnt_values1 = yumi_s.ik(manipulator_name,
tgt_pos1,
tgt_rotmat1,
seed_jnt_values=jnt_values)
def showbaseframe(self, length=0.10, thickness=0.005):
gm.gen_frame(length=length, thickness=thickness).attach_to(base)
import math
import numpy as np
import basis.robot_math as rm
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85
import grasping.annotation.utils as gu
import pickle
base = wd.World(cam_pos=[.3, .3, .3], lookat_pos=[0, 0, 0])
gm.gen_frame(length=.05, thickness=.0021).attach_to(base)
# object
object_bunny = cm.CollisionModel("objects/bunnysim.stl")
object_bunny.set_rgba([.9, .75, .35, .3])
object_bunny.attach_to(base)
# hnd_s
# contact_pairs, contact_points = gpa.plan_contact_pairs(object_bunny,
# max_samples=10000,
# min_dist_between_sampled_contact_points=.014,
# angle_between_contact_normals=math.radians(160),
# toggle_sampled_points=True)
# for p in contact_points:
# gm.gen_sphere(p, radius=.002).attach_to(base)
# base.run()
# pickle.dump(contact_pairs, open( "save.p", "wb" ))
contact_pairs = pickle.load(open("save.p", "rb"))
for i, cp in enumerate(contact_pairs):
contact_p0, contact_n0 = cp[0]
contact_p1, contact_n1 = cp[1]
rgba = rm.get_rgba_from_cmap(i)
import visualization.panda.world as wd import modeling.geometric_model as gm import basis.robot_math as rm import math import numpy as np base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) frame_o = gm.gen_frame(length=.2) frame_o.attach_to(base) # rotmat = rm.rotmat_from_axangle([1,1,1],math.pi/4) rotmat_a = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) # frame_a = gm.gen_mycframe(length=.2, rotmat=rotmat) frame_a = gm.gen_dashframe(length=.2, rotmat=rotmat_a) frame_a.attach_to(base) # point in a pos_a = np.array([.15, .07, .05]) # pos_start = rotmat_a.dot(pos_a) # pos_end = rotmat_a.dot(np.array([pos_a[0], pos_a[1], 0])) # # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_stick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) # pos_start = rotmat_a.dot(np.array([pos_a[0], pos_a[1], 0])) # pos_end = rotmat_a.dot(np.array([pos_a[0], 0, 0])) # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # # pos_start = rotmat_a.dot(pos_a) # pos_end = rotmat_a.dot(np.array([pos_a[0], 0, pos_a[2]])) # # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_stick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) # pos_start = rotmat_a.dot(np.array([pos_a[0], 0, pos_a[2]]))
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import robot_sim.robots.ur3_dual.ur3_dual as ur3d
import math
import numpy as np
import basis.robot_math as rm
if __name__ == '__main__':
base = wd.World(cam_pos=[9, 4, 4], lookat_pos=[0, 0, .7])
gm.gen_frame(length=.7, thickness=.02).attach_to(base)
# object
object = cm.CollisionModel("./objects/bunnysim.stl")
object.set_pos(np.array([.55, -.3, 1.3]))
object.set_rotmat(
rm.rotmat_from_euler(-math.pi / 3, math.pi / 6, math.pi / 9))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
gm.gen_frame(length=.3, thickness=.015).attach_to(object)
# robot_s
robot_s = ur3d.UR3Dual()
robot_meshmodel = robot_s.gen_meshmodel(rgba=[.3, .3, .3, .3])
robot_meshmodel.attach_to(base)
base.run()
import opt_ik
base = wd.World(cam_pos=[-15, 2.624 - 0.275, 15],
lookat_pos=[-1.726 - 0.35, 2.624 - 0.275, 5.323],
auto_cam_rotate=False)
mcn_s = mcn.TBM()
mcn_s.fk(np.array([math.pi / 60]))
mcn_s.gen_meshmodel().attach_to(base)
rbt_s = rbt.TBMChanger7R(pos=np.array([-1.726 - 0.35, 2.624 - 0.275, 5.323]))
# rbt_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
# ik_s = opt_ik.OptIK(rbt_s, component_name='arm', obstacle_list=[])
# base.run()
for cutter in mcn_s.cutters['0.75']:
tgt_pos = cutter.pos
tgt_rotmat = rm.rotmat_from_euler(math.pi / 2, 0, 0).dot(cutter.rotmat)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat, thickness=.05,
length=1).attach_to(base)
seed0 = np.zeros(7)
seed0[3] = math.pi / 2
seed0[4] = math.pi / 2
seed0[5] = -math.pi / 2
seed1 = np.zeros(7)
seed1[3] = -math.pi / 2
seed1[4] = -math.pi / 2
seed1[5] = -math.pi / 2
# try:
# jnt_values, _ = ik_s.solve(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, seed_jnt_values= seed0)
# except:
# jnt_values = None
# if jnt_values is None:
# try:
# jnt_values, _ = ik_s.solve(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, seed_jnt_values= seed1)
# c1 = cm.gen_box(extent=[.006 * 20, 0.006 * 30, .001], homomat=rm.homomat_from_posrot([0.006 * 19, 0.006 * 4.5, 0],
# rm.rotmat_from_axangle(
# [0, 0, 1],
# 0 * np.pi / 2)),
# rgba=[0, 0, 0, 0.2])
# c2 = cm.gen_box(extent=[.006 * 3, 0.006 * 3, .001], homomat=rm.homomat_from_posrot([0.006 * 4, 0.006 * 12, 0],
# rm.rotmat_from_axangle(
# [0, 0, 1],
# 0 * np.pi / 2)),
# rgba=[0, 0, 0, 0.2])
cut_list = [c1, c2, c3, c4, c5, c6, c7, c8]
# cut_list = [c1]
gm.gen_frame(
pos=matrix[0][0],
length=.01,
thickness=.0005,
).attach_to(base)
# cut_list = []
for model in cut_list:
model.attach_to(base)
grid = Grid(np.array(matrix), interval)
node = Node(grid, height=0.006, origin_offset=0.001)
matrix_infos = node.node_matrix_infos
for key in matrix_infos.keys():
element = Element(matrix_infos[key],
radius=0.0006,
id=key,
cut=cut_list,
support=True)
# element.get_stl()
self.win.requestProperties(win_props)
def set_origin(self, origin: np.ndarray):
"""
:param origin: 1x2 np array describe the left top corner of the window
"""
win_props = WindowProperties()
win_props.setOrigin(origin[0], origin[1])
self.win.requestProperties(win_props)
if __name__ == "__main__":
import modeling.geometric_model as gm
base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2])
gm.gen_frame(length=.2).attach_to(base)
# extra window 1
ew = ExtraWindow(base, cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2])
ew.set_origin((0, 40))
# ImgOnscreen()
img = cv2.imread("img.png")
on_screen_img = ImgOnscreen(img.shape[:2][::-1], parent_np=ew)
on_screen_img.update_img(img)
# extra window 2
ew2 = ExtraWindow(base, cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2])
ew2.set_origin((0, ew.size[1]))
gm.gen_frame(length=.2).objpdnp.reparentTo(ew2.render)
base.run()
def showSptAxis(self, num):
axisnode = NodePath("normal")
axisnode.setMat(self.p[int(num * 3)])
gm.gen_frame(length=0.050).attach_to(axisnode)
axisnode.reparentTo(base.render)
# intolist = [self.jlc.lnks[4],
# self.jlc.lnks[5],
# self.jlc.lnks[6]]
# self.cc.set_cdpair(fromlist, intolist)
# fromlist = [self.jlc.lnks[3]]
# intolist = [self.jlc.lnks[6]]
# self.cc.set_cdpair(fromlist, intolist)
if __name__ == '__main__':
import time
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[-5, -5, 3], lookat_pos=[3, 0, 0])
gm.gen_frame().attach_to(base)
seed0 = np.zeros(6)
seed0[3] = math.pi / 2
seed1 = np.zeros(6)
seed1[3] = -math.pi / 2
manipulator_instance = TBMArm(enable_cc=True)
manipulator_meshmodel = manipulator_instance.gen_meshmodel(toggle_jntscs=True)
manipulator_meshmodel.attach_to(base)
manipulator_instance.gen_stickmodel(toggle_tcpcs=True, toggle_jntscs=True).attach_to(base)
# base.run()
seed_jnt_values = manipulator_instance.get_jnt_values()
for x in np.linspace(1, 3, 7).tolist():
for y in np.linspace(-2, 2, 14).tolist():
print(x)
tgt_pos = np.array([x, y, 0])
tgt_rotmat = np.eye(3)
import os
import math
import numpy as np
import basis.robot_math as rm
import robot_sim._kinematics.jlchain as jl
import robot_sim.manipulators.manipulator_interface as mi
class XArm7(mi.ManipulatorInterface):
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
homeconf=np.zeros(7),
name='xarm7',
enable_cc=True):
super().__init__(pos=pos, rotmat=rotmat, name=name)
this_dir, this_filename = os.path.split(__file__)
self.jlc = jl.JLChain(pos=pos,
rotmat=rotmat,
homeconf=homeconf,
name=name)
# seven joints, n_jnts = 7+2 (tgt ranges from 1-7), nlinks = 7+1
jnt_saferngmargin = math.pi / 18.0
self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, .267])
self.jlc.jnts[1]['motion_rng'] = [
-math.pi + jnt_saferngmargin, math.pi - jnt_saferngmargin
]
self.jlc.jnts[2]['loc_pos'] = np.array([0, 0, 0])
self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(-1.5708, 0, 0)
self.jlc.jnts[2]['motion_rng'] = [
-2.18 + jnt_saferngmargin, 2.18 - jnt_saferngmargin
import numpy as np
import basis.robot_math as rm
import visualization.panda.world as wd
import robot_sim.end_effectors.grippers.robotiqhe.robotiqhe as rtq_he
import modeling.geometric_model as gm
if __name__ == "__main__":
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0])
gm.gen_frame(length=.2).attach_to(base)
grpr = rtq_he.RobotiqHE(enable_cc=True)
grpr.jaw_to(.05)
grpr.gen_meshmodel(rgba=[.3,.3,.3,.3]).attach_to(base)
grpr.gen_stickmodel(toggle_tcpcs=True, toggle_jntscs=True).attach_to(base)
grpr.fix_to(pos=np.array([-.1, .2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05))
grpr.gen_meshmodel().attach_to(base)
grpr.show_cdmesh()
grpr.fix_to(pos=np.array([.1, -.2, 0]), rotmat=rm.rotmat_from_axangle([1, 0, 0], .05))
grpr.gen_meshmodel().attach_to(base)
grpr.show_cdprimit()
base.run()
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.planning.antipodal as gpa
import math
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.cobotta.cobotta as cbt
import manipulation.pick_place_planner as ppp
import motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15])
gm.gen_frame().attach_to(base)
# ground
ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7])
ground.set_pos(np.array([0, 0, -.51]))
ground.attach_to(base)
# object holder
object_holder = cm.CollisionModel("objects/holder.stl")
object_holder.set_rgba([.5, .5, .5, 1])
object_holder_gl_pos = np.array([-.15, -.3, .0])
object_holder_gl_rotmat = np.eye(3)
obgl_start_homomat = rm.homomat_from_posrot(object_holder_gl_pos,
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])
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.planning.antipodal as gpa
import math
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.xarm_shuidi.xarm_shuidi as xsm
import robot_sim.end_effectors.gripper.xarm_gripper.xarm_gripper as xag
import manipulation.approach_depart_planner as adp
import motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3])
gm.gen_frame().attach_to(base)
ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7])
ground.set_pos(np.array([0, 0, -.5]))
ground.attach_to(base)
object_box = cm.gen_box(extent=[.02, .06, .7], rgba=[.7, .5, .3, .7])
object_box_gl_pos = np.array([.5, -.3, .35])
object_box_gl_rotmat = np.eye(3)
object_box.set_pos(object_box_gl_pos)
object_box.set_rotmat(object_box_gl_rotmat)
gm.gen_frame().attach_to(object_box)
robot_s = xsm.XArmShuidi()
rrtc_s = rrtc.RRTConnect(robot_s)
adp_s = adp.ADPlanner(robot_s)
grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle')
Matrix([x,y,z])-
def sym_axangle(ax, angle):
st = sympy.sin(angle)
ct = sympy.cos(angle)
# ax = ax.T[0, :]
_rotmat = [
[ct + ax[0] ** 2 * (1 - ct), ax[0] * ax[1] * (1 - ct) - ax[2] * st, ax[2] * ax[0] * (1 - ct) + ax[1] * st],
[ax[0] * ax[1] * (1 - ct) + ax[2] * st, ct + ax[1] ** 2 * (1 - ct), ax[1] * ax[2] * (1 - ct) - ax[0] * st],
[ax[2] * ax[0] * (1 - ct) - ax[1] * st, ax[1] * ax[2] * (1 - ct) + ax[0] * st, ct + ax[2] ** 2 * (1 - ct)]]
return sympy.Matrix(_rotmat)
base = world.World(cam_pos=np.array([1.5, 1, .7]))
gm.gen_frame(alpha=.3).attach_to(base)
q1, q2, q3, q4, q5, q6 = sympy.symbols("q1 q2 q3 q4 q5 q6")
x, y, z, r00, r01, r02, r10, r11, r12, r20, r21, r22 = sympy.symbols("x y z r00 r01 r02 r10 r11 r12 r20 r21 r22")
rbt_s = cbt_s.Cobotta()
print("Computing joint 1...")
# rotmat
rotmat0 = Matrix(rbt_s.manipulator_dict['arm'].jnts[0]['gl_rotmatq'])
ax1 = rotmat0 * Matrix(rbt_s.manipulator_dict['arm'].jnts[1]['loc_motionax'])
rotmat1 = sym_axangle(ax1, q1)
accumulated_rotmat1 = rotmat1
# pos
pos0 = Matrix(rbt_s.manipulator_dict['arm'].jnts[0]['gl_posq'])
pos1 = pos0 + rotmat0 * Matrix(rbt_s.manipulator_dict['arm'].jnts[1]['loc_pos'])
self.robot_s.fk(component_name, jnt_values_bk)
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)
import visualization.panda.world as wd
import modeling.geometric_model as gm
import modeling.collision_model as cm
import grasping.planning.antipodal as gpa
import math
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.cobotta.cobotta as cbt
import manipulation.pick_place_planner as ppp
import motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15])
gm.gen_frame().attach_to(base)
# ground
ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7])
ground.set_pos(np.array([0, 0, -.51]))
ground.attach_to(base)
robot_s = cbt.Cobotta()
robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
seed_jnt_values = None
for z in np.linspace(.1, .6, 5):
goal_pos = np.array([.25, -.1, z])
goal_rot = rm.rotmat_from_axangle(np.array([0, 1, 0]), math.pi * 1 / 2)
gm.gen_frame(goal_pos, goal_rot).attach_to(base)
jnt_values = robot_s.ik(tgt_pos=goal_pos, tgt_rotmat=goal_rot, seed_jnt_values=seed_jnt_values)
print(jnt_values)
if jnt_values is not None:
robot_s.fk(jnt_values=jnt_values)
seed_jnt_values = jnt_values
import os
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.cobotta.cobotta_ripps as cbtr
import robot_sim.end_effectors.gripper.cobotta_pipette.cobotta_pipette as cbtg
import modeling.collision_model as cm
import visualization.panda.world as wd
import modeling.geometric_model as gm
import utils
if __name__ == '__main__':
base = wd.World(cam_pos=[1.7, -1, .7], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
this_dir, this_filename = os.path.split(__file__)
file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl")
frame_bottom = cm.CollisionModel(file_frame)
frame_bottom.set_rgba([.55, .55, .55, 1])
frame_bottom.attach_to(base)
table_plate = cm.gen_box(extent=[.405, .26, .003])
table_plate.set_pos([0.07 + 0.2025, .055, .0015])
table_plate.set_rgba([.87, .87, .87, 1])
table_plate.attach_to(base)
file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl")
tip_rack = utils.Rack96(file_tip_rack)
tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1])
tip_rack.set_pose(pos=np.array([.25, 0.0, .003]),
rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2))
tip_rack.attach_to(base)
import os
import time
import math
import basis
import numpy as np
import modeling.geometric_model as gm
import modeling.collision_model as cm
import robot_sim.robots.xarm7_shuidi_mobile.xarm7_shuidi_mobile as xav
if __name__ == '__main__':
import copy
import motion.probabilistic.rrt_connect as rrtc
import visualization.panda.rpc.rviz_client as rv_client
# # local code
global_frame = gm.gen_frame()
# define robot_s and robot_s anime info
robot_s = xav.XArm7YunjiMobile()
robot_meshmodel_parameters = [
None, # tcp_jntid
None, # tcp_loc_pos
None, # tcp_loc_rotmat
False, # toggle_tcpcs
False, # toggle_jntscs
[0, .7, 0, .3], # rgba
'auto'
] # name
# define object and object anime info
objfile = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl')
obj = cm.CollisionModel(objfile)
obj_parameters = [[.3, .2, .1, 1]] # rgba
