def serve(host="localhost:18300"):
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0])
_ONE_DAY_IN_SECONDS = 60 * 60 * 24
options = [('grpc.max_send_message_length', 100 * 1024 * 1024),
('grpc.max_receive_message_length', 100 * 1024 * 1024)]
server = grpc.server(futures.ThreadPoolExecutor(max_workers=10), options=options)
rvs = rs.RVizServer()
rv_rpc.add_RVizServicer_to_server(rvs, server)
server.add_insecure_port(host)
server.start()
print("The RViz server is started!")
base.run()
def __init__(self,
pos=np.zeros(3),
rotmat=np.eye(3),
use_real=False,
create_sim_world=True,
lft_robot_ip='10.2.0.50',
rgt_robot_ip='10.2.0.51',
pc_ip='10.2.0.100',
cam_pos=np.array([2, 1, 3]),
lookat_pos=np.array([0, 0, 1.1]),
auto_cam_rotate=False):
self.robot_s = ur3ds.UR3Dual(pos=pos, rotmat=rotmat)
self.rrt_planner = rrtc.RRTConnect(self.robot_s)
self.inik_solver = inik.IncrementalNIK(self.robot_s)
self.pp_planner = ppp.PickPlacePlanner(self.robot_s)
if use_real:
self.robot_x = ur3dx.UR3DualX(lft_robot_ip=lft_robot_ip,
rgt_robot_ip=rgt_robot_ip,
pc_ip=pc_ip)
if create_sim_world:
self.sim_world = wd.World(cam_pos=cam_pos,
lookat_pos=lookat_pos,
auto_cam_rotate=auto_cam_rotate)
super().enable_cc()
self.cc.add_cdlnks(self.jlc, [1, 2, 3, 4, 5, 6, 7])
activelist = [
self.jlc.lnks[1], self.jlc.lnks[2], self.jlc.lnks[3],
self.jlc.lnks[4], self.jlc.lnks[5], self.jlc.lnks[6],
self.jlc.lnks[7]
]
self.cc.set_active_cdlnks(activelist)
fromlist = [self.jlc.lnks[1], self.jlc.lnks[2]]
intolist = [self.jlc.lnks[5], self.jlc.lnks[6], self.jlc.lnks[7]]
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=[2, 0, 1], lookat_pos=[0, 0, 0.5])
gm.gen_frame().attach_to(base)
manipulator_instance = SIA5(enable_cc=True)
manipulator_meshmodel = manipulator_instance.gen_meshmodel()
manipulator_meshmodel.attach_to(base)
manipulator_meshmodel.show_cdprimit()
manipulator_instance.gen_stickmodel(toggle_jntscs=True).attach_to(base)
tic = time.time()
print(manipulator_instance.is_collided())
toc = time.time()
print(toc - tic)
base.run()
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 numpy as np
import basis.robot_math as rm
import modeling.geometric_model as gm
import robot_sim.robots.ur3_dual.ur3_dual as ur3d
import robot_sim.robots.ur3e_dual.ur3e_dual as ur3ed
import robot_sim.robots.sda5f.sda5f as sda5
import motion.probabilistic.rrt_connect as rrtc
import pickle
if __name__ == '__main__':
base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
phoxicam = cm.gen_box(extent=np.array([1.000, .43, .1]), homomat=np.eye(4))
phoxicam.set_pos(
np.array([100 + 200, 0, 1360 + 175 + 200 + 90 / 2]) / 1000)
phoxicam.attach_to(base)
object = cm.CollisionModel("./objects/lshape-30.stl")
object.set_scale([0.001, 0.001, 0.001])
object.set_pos(np.array([1.05, -.3, 1.3]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
component_name = 'rgt_arm'
# robot_instance = ur3d.UR3Dual()
robot_instance = ur3ed.UR3EDual()
# robot_instance = sda5.SDA5F()
seed_jnt_values = jnt_values
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,
def copy(self):
return BDModel(self)
if __name__ == "__main__":
import os
import numpy as np
import basis
import basis.robot_math as rm
import visualization.panda.world as wd
import random
# base = wd.World(cam_pos=[1000, 300, 1000], lookat_pos=[0, 0, 0], toggle_debug=True)
base = wd.World(cam_pos=[.3, .3, 1],
lookat_pos=[0, 0, 0],
toggle_debug=False)
base.setFrameRateMeter(True)
objpath = os.path.join(basis.__path__[0], "objects", "bunnysim.stl")
# objpath = os.path.join(basis.__path__[0], "objects", "block.stl")
bunnycm = BDModel(objpath, mass=1, type="box")
objpath2 = os.path.join(basis.__path__[0], "objects", "bowlblock.stl")
bunnycm2 = BDModel(objpath2, mass=0, type="triangles", dynamic=False)
bunnycm2.set_rgba(np.array([0, 0.7, 0.7, 1.0]))
bunnycm2.set_pos(np.array([0, 0, 0]))
bunnycm2.start_physics()
base.attach_internal_update_obj(bunnycm2)
def update(bunnycm, task):
if base.inputmgr.keymap['space'] is True:
jawwidthlist_approach + jawwidthlist_middle + jawwidthlist_depart, \
objpose_list_approach + objpose_list_middle + objpose_list_depart
return None, None, None
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))
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')
rgba=rgba).attach_to(mm_collection)
self.rgt_hnd.gen_meshmodel(toggle_tcpcs=False,
toggle_jntscs=toggle_jntscs,
rgba=rgba).attach_to(mm_collection)
for obj_info in self.lft_oih_infos:
objcm = obj_info['collision_model']
objcm.set_pos(obj_info['gl_pos'])
objcm.set_rotmat(obj_info['gl_rotmat'])
objcm.copy().attach_to(mm_collection)
for obj_info in self.rgt_oih_infos:
objcm = obj_info['collision_model']
objcm.set_pos(obj_info['gl_pos'])
objcm.set_rotmat(obj_info['gl_rotmat'])
objcm.copy().attach_to(mm_collection)
return mm_collection
if __name__ == '__main__':
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[5, 0, 3], lookat_pos=[0, 0, 1])
gm.gen_frame().attach_to(base)
u3ed = UR3EDual()
# u3ed.fk(.85)
u3ed_meshmodel = u3ed.gen_meshmodel(toggle_tcpcs=True)
u3ed_meshmodel.attach_to(base)
# u3ed_meshmodel.show_cdprimit()
# u3ed.gen_stickmodel().attach_to(base)
base.run()
import numpy as np
import drivers.devices.kinect_azure.pykinectazure as pk
import visualization.panda.world as wd
import modeling.geometric_model as gm
from vision.depth_camera.calibrator import load_calibration_data
import basis.robot_math as rm
affine_matrix, _, _ = load_calibration_data()
base = wd.World(cam_pos=[10, 2, 7], lookat_pos=[0, 0, 0])
gm.gen_frame().attach_to(base)
pkx = pk.PyKinectAzure()
pcd_list = []
ball_center_list = []
para_list = []
ball_list = []
counter = [0]
def update(pk_obj, pcd_list, ball_center_list, counter, task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
# if len(ball_center_list) != 0:
# for ball_center in ball_center_list:
# ball_center.detach()
# ball_center_list.clear()
while True:
pk_obj.device_get_capture()
author: weiwei
date: 20170221
"""
objtrm = trm.load_mesh(objpath)
pdnp = nodepath_from_vvnf(objtrm.vertices, objtrm.vertex_normals,
objtrm.faces)
return pdnp
if __name__ == '__main__':
import os, math, basis
import basis.trimesh as trimesh
import visualization.panda.world as wd
from panda3d.core import TransparencyAttrib
wd.World(cam_pos=[1.0, 1, .0, 1.0], lookat_pos=[0, 0, 0])
objpath = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl')
bt = trimesh.load(objpath)
btch = bt.convex_hull
pdnp = nodepath_from_vfnf(bt.vertices, bt.face_normals, bt.faces)
pdnp.reparentTo(base.render)
pdnp_cvxh = nodepath_from_vfnf(btch.vertices, btch.face_normals,
btch.faces)
pdnp_cvxh.setTransparency(TransparencyAttrib.MDual)
pdnp_cvxh.setColor(0, 1, 0, .3)
pdnp_cvxh.reparentTo(base.render)
pdnp2 = nodepath_from_vvnf(bt.vertices, bt.vertex_normals, bt.faces)
pdnp2.setPos(0, 0, .1)
pdnp2.reparentTo(base.render)
base.run()
robot.fk(component_name=component_name, jnt_values=jnt_values)
if hndfa.is_mesh_collided(slopeforcd_high, toggle_debug=False):
robot.gen_meshmodel(toggle_tcpcs=False,
rgba=(1, 0, 0, 0.05)).attach_to(base)
# pass
else:
robot.gen_meshmodel(toggle_tcpcs=False,
rgba=(0, 1, 0, 0.5)).attach_to(base)
print(f"IK Done!, feasible grasp ID {i}")
if __name__ == '__main__':
# world
base = wd.World(cam_pos=[2.01557, 0.637317, 1.88133],
w=960,
h=540,
lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
this_dir, this_filename = os.path.split(__file__)
object = cm.CollisionModel("./objects/test_long.stl")
object.set_scale((.001, .001, .001))
object.set_rgba([.5, .7, .3, 1])
gm.gen_frame().attach_to(object)
# object_goal = object.copy()
# object_goal_pos = np.array([0.800, -.300, 0.900])
# object_goal_rotmat = np.eye(3)
# object_goal_homomat = rm.homomat_from_posrot(object_goal_pos, object_goal_rotmat)
# object_goal.set_pos(object_goal_pos)
# object_goal.set_rotmat(object_goal_rotmat)
# fromlist = [self.jlc.lnks[2]]
# 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])
import visualization.panda.world as wd
import numpy as np
from panda3d.core import Vec3, Point3, TransformState
from panda3d.bullet import BulletWorld
from panda3d.bullet import BulletPlaneShape, BulletDebugNode
from panda3d.bullet import BulletRigidBodyNode
from panda3d.bullet import BulletBoxShape, BulletTriangleMeshShape, BulletSphereShape, BulletCylinderShape, ZUp
from panda3d.bullet import BulletCapsuleShape, BulletConeShape,BulletConvexHullShape, BulletTriangleMesh
import modeling.collision_model as cm
import modeling.geometric_model as gm
import basis.data_adapter as da
import copy
base = wd.World(cam_pos=np.array([1000, 0, 3000]), lookat_pos=np.array([0, 0, 0]), toggle_debug=True)
# World
# world = BulletWorld()
# world.setGravity(Vec3(0, 0, -9810))
# gm.gen_frame().attach_to(base)
# Plane
shape = BulletBoxShape(Vec3(500, 500, 100))
# shape = BulletPlaneShape(Vec3(0, 0, 1), 1)
node = BulletRigidBodyNode('Ground')
node.addShape(shape)
np = base.render.attachNewNode(node)
np.setPos(0, 0, 0)
base.physicsworld.attachRigidBody(node)
# Box
# shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5))
# shape = BulletSphereShape(radius=.1)
import os
import numpy as np
import basis.robot_math as rm
import robot_sim.robots.cobotta.cobotta_ripps as cbtr
import robot_sim.manipulators.cobotta_arm.cobotta_arm as cbta
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
import motion.probabilistic.rrt_connect as rrtc
import motion.trajectory.piecewisepoly_toppra as trajp
if __name__ == '__main__':
base = wd.World(cam_pos=[.7, 1.4, 1], lookat_pos=[0, 0, 0.2])
traj_gen = trajp.PiecewisePolyTOPPRA()
max_vels = [np.pi * .6, np.pi * .4, np.pi, np.pi, np.pi, np.pi * 1.5]
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_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl")
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 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,
picnode = [None, None]
taskMgr.doMethodLater(0.1,
update,
"update",
extraArgs=[
objmnp, counter, testnode, normalnode,
graspnode, textNPose, textNPose_lp, picnode
],
appendTask=True)
# taskMgr.add(showCamPos, "showCamPos")
base.run()
if __name__ == '__main__':
base = wd.World(cam_pos=[0.752962, -0.653211, 0.562782],
w=960,
h=540,
lookat_pos=[0, 0, 0.1])
# gm.gen_frame(length=0.10, thickness=0.005).attach_to(base)
# objname = "yuanbox_small
# objname = "box20"
objname = "test_long"
# objname = "lofted"
# objname = "angle"
# objname = "wedge"
# objname = "bar"
# objname = "polysolid"
# objname = "tjunction-s-c"
# objname = "smallvbase"
# objname = "tjunction-show"
# objname = "housing-cal"
# objname = "bracket-box"
import visualization.panda.world as wd
import pickle
import modeling.collision_model as cm
base = wd.World(cam_pos=[0, -2, -5], lookat_pos=[0, -1, 5])
pcd_list = pickle.load(open("pcdlist.pkl", "rb"))
attached_list = []
counter = [0]
def update(attached_list, pcd_list, counter, task):
if counter[0] >= len(pcd_list):
counter[0] = 0
if len(attached_list) != 0:
for objcm in attached_list:
objcm.detach()
attached_list.clear()
pcd = pcd_list[counter[0]]
attached_list.append(cm.CollisionModel(pcd))
attached_list[-1].attach_to(base)
counter[0]+=1
return task.again
taskMgr.doMethodLater(0.01, update, "update",
extraArgs=[attached_list, pcd_list, counter],
appendTask=True)
base.run()
import numpy as np
import modeling.geometric_model as gm
import modeling.collision_model as cm
import visualization.panda.world as wd
import basis.robot_math as rm
import math
from scipy.spatial import cKDTree
import vision.depth_camera.surface.rbf_surface as rbfs
base = wd.World(cam_pos=np.array([-.3, -.7, .42]),
lookat_pos=np.array([0, 0, 0]))
# gm.gen_frame().attach_to(base)
bowl_model = cm.CollisionModel(initor="./objects/bowl.stl")
bowl_model.set_rgba([.3, .3, .3, .3])
bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0))
# bowl_model.attach_to(base)
pn_direction = np.array([0, 0, -1])
bowl_samples, bowl_sample_normals = bowl_model.sample_surface(
toggle_option='normals', radius=.002)
selection = bowl_sample_normals.dot(-pn_direction) > .1
bowl_samples = bowl_samples[selection]
bowl_sample_normals = bowl_sample_normals[selection]
tree = cKDTree(bowl_samples)
surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2])
surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base)
pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True)
tmp_direction = np.cross(pn_direction, pt_direction)
plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
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=[.25, -1, .4], lookat_pos=[.25, 0, .3])
gm.gen_frame().attach_to(base)
rbt_s = cbtr.CobottaRIPPS()
# rbt_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
rbt_s.jaw_to(jaw_width=0.03)
tgt_pos = np.array([.25, .0, .1])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], np.pi)
jnt_values = rbt_s.ik(component_name="arm",
tgt_pos=tgt_pos,
tgt_rotmat=tgt_rotmat)
print(jnt_values)
if jnt_values is not None:
rbt_s.fk(component_name="arm", jnt_values=jnt_values)
rbt_s.gen_meshmodel().attach_to(base)
base.run()
import grasping.annotation.utils as gau
if __name__ == '__main__':
import numpy as np
import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85
import modeling.collision_model as cm
import visualization.panda.world as wd
base = wd.World(cam_pos=[.5, .5, .3], lookat_pos=[0, 0, 0])
gripper_s = rtq85.Robotiq85(enable_cc=True)
objcm = cm.CollisionModel("./objects/bunnysim.stl")
objcm.set_pos(np.array([.5, -.3, 1.2]))
objcm.attach_to(base)
objcm.show_localframe()
grasp_info_list = gau.define_grasp_with_rotation(
gripper_s,
objcm,
gl_jaw_center_pos=np.array([0, 0, 0]),
gl_jaw_center_z=np.array([1, 0, 0]),
gl_hndx=np.array([0, 1, 0]),
jaw_width=.04,
gl_rotation_ax=np.array([0, 0, 1]))
for grasp_info in grasp_info_list:
jaw_width, gl_jaw_center, pos, rotmat = grasp_info
gic = gripper_s.copy()
gic.fix_to(pos, rotmat)
gic.jaw_to(jaw_width)
print(pos, rotmat)
gic.gen_meshmodel().attach_to(base)
base.run()
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)
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)
import math
import numpy as np
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 motion.probabilistic.rrt_connect as rrtc
base = wd.World(cam_pos=[2, 1, 3], lookat_pos=[0, 0, 1.1])
gm.gen_frame().attach_to(base)
# object
object = cm.CollisionModel("objects/bunnysim.stl")
object.set_pos(np.array([.55, -.3, 1.3]))
object.set_rgba([.5, .7, .3, 1])
object.attach_to(base)
# robot_s
component_name = 'lft_arm'
robot_s = ur3d.UR3Dual()
# possible right goal np.array([0, -math.pi/4, 0, math.pi/2, math.pi/2, math.pi / 6])
# possible left goal np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6])
rrtc_planner = rrtc.RRTConnect(robot_s)
path = rrtc_planner.plan(component_name=component_name,
start_conf=robot_s.lft_arm.homeconf,
goal_conf=np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6]),
obstacle_list=[object],
ext_dist=.2,
max_time=300)
print(path)
for pose in path:
import visualization.panda.world as wd from panda3d.ode import OdeWorld, OdeBody, OdeMass, OdeBallJoint import modeling.collision_model as cm import modeling.geometric_model as gm import basis.data_adapter as da import numpy as np base = wd.World(cam_pos=[7, 0, 0], lookat_pos=[0, 0, -.5], toggle_debug=True) radius = .1 sphere_a = cm.gen_sphere(radius=radius) sphere_a.set_pos([0, .3, -.3]) sphere_a.set_rgba([1, .2, .3, 1]) sphere_a.attach_to(base) sphere_b = cm.gen_sphere(radius=radius) sphere_b.set_pos([0, 1.25, -.7]) sphere_b.set_rgba([.3, .2, 1, 1]) sphere_b.attach_to(base) gm.gen_linesegs([[np.zeros(3), sphere_a.get_pos()]], thickness=.05, rgba=[0, 1, 0, 1]).attach_to(base) gm.gen_linesegs([[sphere_a.get_pos(), sphere_b.get_pos()]], thickness=.05, rgba=[0, 0, 1, 1]).attach_to(base) # Setup our physics world and the body world = OdeWorld() world.setGravity(0, 0, -9.81) body_sphere_a = OdeBody(world) M = OdeMass() M.setSphere(7874, radius) body_sphere_a.setMass(M)
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 basis.trimesh as trimesh
import trimesh.boolean as tb
import trimesh.exchange.stl as tel
# from pandaplotutils import pandactrl as pandactrl
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],
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 numpy as np import basis.robot_math as rm import visualization.panda.world as wd import robot_sim.end_effectors.gripper.robotiq85.robotiq85 as rtq85 import robot_sim.end_effectors.gripper.robotiqhe.robotiqhe as rtqhe base = wd.World(cam_pos=np.array([1,1,1])) pos0 = np.array([0,0.07,.3]) rotmat0 = rm.rotmat_from_axangle([1,0,0], np.pi/12) rotmat0 = rm.rotmat_from_axangle([0,1,0], np.pi/12) rotmat0 = rm.rotmat_from_axangle([0,0,1], np.pi/9) hnd0 = rtqhe.RobotiqHE() hnd0.grip_at_with_jcpose(gl_jaw_center_pos=pos0, gl_jaw_center_rotmat=rotmat0, jaw_width=.005) hnd0.gen_meshmodel().attach_to(base) pos1 = np.array([0,-0.07,.3]) rotmat1 = rm.rotmat_from_axangle([-1,0,0], np.pi/12) rotmat1 = rm.rotmat_from_axangle([0,-1,0], np.pi/12) rotmat1 = rm.rotmat_from_axangle([0,0,-1], np.pi/9) hnd1 = rtqhe.RobotiqHE() hnd1.grip_at_with_jcpose(gl_jaw_center_pos=pos1, gl_jaw_center_rotmat=rotmat1, jaw_width=.005) hnd1.gen_meshmodel().attach_to(base) base.run()
