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]))
