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)
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: print(pose) robot_s.fk(component_name, pose)
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 import robot_con.ur.ur3_dual_x as ur3dx 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 = 'both_arm' robot_instance = ur3d.UR3Dual() # init_lft_arm_jnt_values = robot_s.lft_arm.get_jnt_values() # init_rgt_arm_jnt_values = robot_s.rgt_arm.get_jnt_values() # full_jnt_values = np.hstack((init_lft_arm_jnt_values, init_rgt_arm_jnt_values)) full_jnt_values = np.hstack( (robot_instance.lft_arm.homeconf, robot_instance.rgt_arm.homeconf)) goal_lft_arm_jnt_values = np.array( [0, -math.pi / 2, -math.pi / 3, -math.pi / 2, math.pi / 6, math.pi / 6]) goal_rgt_arm_jnt_values = np.array( [0, -math.pi / 4, 0, math.pi / 2, math.pi / 2, math.pi / 6]) rrtc_planner = rrtc.RRTConnect(robot_instance) path = rrtc_planner.plan(component_name=component_name, start_conf=full_jnt_values, goal_conf=np.hstack((goal_lft_arm_jnt_values, goal_rgt_arm_jnt_values)),
def __init__(self): self.rtq85 = rtq85.Robotiq85() self.rbt = rbts.UR3Dual()
import robot_sim.end_effectors.suction.mvfln40.mvfln40 as suction import robot_sim.end_effectors.gripper.robotiq85_gelsight.robotiq85_gelsight_pusher as rtqgel import robot_sim.end_effectors.gripper.robotiq85.robotiq85 as rtq import robot_sim.robots.ur3_dual.ur3_dual as ur3d import modeling.geometric_model as gm import modeling.collision_model as cm import numpy as np import visualization.panda.world as wd import basis.robot_math as rm import math base = wd.World(cam_pos=[3.5, 1, 2.5], lookat_pos=[.5, 0, 1.2]) ur3d_s = ur3d.UR3Dual() ur3d_s.gen_meshmodel().attach_to(base) ground = gm.gen_box(np.array([2, 2, .001])) ground.set_rgba([1, 253 / 255, 219 / 255, 1]) ground.attach_to(base) box = cm.gen_box(np.array([.3, .3, .04])) box.set_rgba([153 / 255, 183 / 255, 1, 1]) box.set_pos(np.array([.3, 0, 1.126])) box.attach_to(base) # box = cm.gen_box(np.array([.3, .3, .04])) # box.set_rgba([153 / 255, 183 / 255, 1, 1]) # box.set_pos(np.array([.35, 0, 1.126+0.05])) # box.set_rotmat(rm.rotmat_from_axangle([0,0,1], math.pi/8)) # box.attach_to(base) # # support_box = cm.gen_box(np.array([.5, .3, .05]))