def setup_robot(self):
if not self.handonly:
#self.robot = p.loadURDF(os.environ["HOME"]+"/ros/src/franka_ros/franka_description/robots/model.urdf") #fixme, point somewhere less fragile
self.robot = p.loadURDF("../../models/robots/model.urdf")
set_point(self.robot, (0, 0, 0.01))
start_joints = (0.09186411075857098, 0.02008522792588543,
0.03645461729775788, -1.9220854528910314,
0.213232566443952983, 1.647271913704007, 0.0, 0.0,
0.0)
start_joints = [
2.78892560e-04, -7.85089406e-01, 4.81729135e-05,
-2.35613802e+00, 4.95981896e-04, 1.57082514e+00,
7.85833531e-01, 0, 0
]
self.grasp_joint = 10
p.changeDynamics(self.robot, -1, mass=0)
ut.set_joint_positions(self.robot,
ut.get_movable_joints(self.robot),
start_joints)
else:
self.robot = p.loadURDF(
os.environ["HOME"] +
"/ros/src/franka_ros/franka_description/robots/hand.urdf"
) #fixme, point somewhere less fragile
init_pos = (0, 0, 0.35)
init_quat = (1, 0, 0, 0)
self.grasp_joint = 0 #kinda sketchy tbh, try to just use the whole robot
import ipdb
ipdb.set_trace()
ut.set_joint_position(self.robot, 9, 0.03)
ut.set_joint_position(self.robot, 10, 0.03)
set_pose(self.robot, (init_pos, init_quat))
def turn_handle(self, visualize=False):
start_pose = ut.get_link_pose(self.robot, self.grasp_joint)
start_quat = start_pose[1]
amount_rotate = np.pi / 4.
rot_y = RigidTransform.x_axis_rotation(amount_rotate)
end_rot = np.dot(rot_y, Quaternion(start_quat).rotation_matrix)
end_quat = Quaternion(matrix=end_rot).elements
step_size = np.pi / 16.
quat_waypoints = ut.get_quaternion_waypoints(start_pose, start_quat,
end_quat)
theta = np.pi
end_theta = np.pi + amount_rotate
n_waypoints = np.round((end_theta - theta) / (step_size))
thetas = np.linspace(theta, end_theta, n_waypoints)
r = 0.10
center = self.door_knob_center.copy()
center[1] -= 0.055 #franka_tool to 9/10 pose
pos_traj = []
for theta, quat in zip(thetas, quat_waypoints):
new_pt = (center[0] + r * np.cos(theta), center[1],
center[2] + r * np.sin(theta))
pos_traj.append((new_pt, quat[1]))
sph = ut.create_sphere(0.025)
ut.set_point(sph, new_pt),
joints_to_plan_for = ut.get_movable_joints(self.robot)[:-2]
traj = []
for pos in pos_traj:
new_jts = ut.inverse_kinematics(
self.robot,
self.grasp_joint,
pos,
movable_joints=joints_to_plan_for,
null_space=self.get_null_space(joints_to_plan_for),
ori_tolerance=0.02)
if new_jts is not None:
traj.append(new_jts)
else:
new_jts = ut.inverse_kinematics(
self.robot,
self.grasp_joint,
pos,
movable_joints=joints_to_plan_for,
null_space=None,
ori_tolerance=0.03)
if new_jts is None:
print("no jt solution found")
else:
traj.append(new_jts)
assert (len(traj) > 0)
if visualize:
self.visualize_traj(traj)
n_pts = min(10, len(traj))
mask = np.round(np.linspace(0, len(traj) - 1, n_pts)).astype(np.int32)
traj = np.array(traj)[mask]
assert (traj is not None)
return traj
def setup_robot(self):
if not self.handonly:
self.robot = p.loadURDF("../../models/robots/model.urdf"
) #fixme, point somewhere less fragile
set_point(self.robot, (-0.4, 0, 0.005))
p.changeDynamics(self.robot, -1, mass=0)
else:
self.robot = p.loadURDF("../../models/robots/hand.urdf")
init_pos = (0, 0, 0.35)
init_quat = (1, 0, 0, 0)
set_pose(self.robot, (init_pos, init_quat))
self.cid = p.createConstraint(self.robot, -1, -1, -1,
p.JOINT_FIXED, [0, 0, 1], [0, 0, 0],
[0, 0, 0], [0, 0, 0, 1],
[0, 0, 0, 1])
p.changeConstraint(self.cid, init_pos, init_quat, maxForce=20)
def setup_robot(self):
if not self.handonly:
#self.robot = p.loadURDF(os.environ["HOME"]+"/ros/src/franka_ros/franka_description/robots/model.urdf") #fixme, point somewhere less fragile
self.robot = p.loadURDF("../../models/robots/model.urdf")
set_point(self.robot, (0, 0, 0.01))
start_joints = [
2.28650215, -1.36063288, -1.4431576, -1.93011263, 0.23962597,
2.6992652, 0.82820212, 0.0, 0.0
]
self.grasp_joint = 8
p.changeDynamics(self.robot, -1, mass=0)
ut.set_joint_positions(self.robot,
ut.get_movable_joints(self.robot),
start_joints)
else:
self.robot = p.loadURDF(
os.environ["HOME"] +
"/ros/src/franka_ros/franka_description/robots/hand.urdf"
) #fixme, point somewhere less fragile
init_pos = (0, 0, 0.35)
init_quat = (1, 0, 0, 0)
self.grasp_joint = 0 #kinda sketchy tbh, try to just use the whole robot
set_pose(self.robot, (init_pos, init_quat))
def visualize_points(self, pt_set):
for pt in pt_set:
pb_pt = ut.create_sphere(0.005)
ut.set_point(pb_pt, pt)
def setup_workspace(self,
rectangle_loc=[[0.562, 0.003, 0.016], [1, 0., 0., 0]],
load_previous=False,
circle_loc=[[0.425, 0.101, 0.01], [1, 0., 0., 0]],
square_loc=None,
obstacle_loc=[[0.53172045, -0.03062703, 0.07507126],
[1, -0., 0., 0]],
board_loc=[[0.479, 0.0453, 0.013],
[0.707, 0.707, 0., 0.]],
hole_goal=[[0.55, 0.08, 0.0], [1, 0, 0, 0]]):
#RigidTransform(rotation=np.array([[-5.78152806e-02, -9.98327119e-01, 4.84639353e-07],
# [-9.98327425e-01, 5.78157598e-02, 3.97392158e-08],
# [ 4.07518811e-07, -6.59092487e-08, -9.99999635e-01]]), translation=np.array([ 0.53810962, 0.08998347, -0.00768057]), from_frame='peg_center', to_frame='world')
#hole_pose = (np.array([ 0.53810962, 0.08998347, -0.00768057]), np.array([-2.89198577e-02, -9.19833769e-08, 1.37694750e-07, 9.99581685e-01]))
#self.floor = p.loadURDF("../../models/short_floor.urdf")
#make board
width = 0.4
length = 0.3
fake_board_thickness = 0.05
height = 0.01
block_height = 0.015
self.hole_depth = block_height
self.block_height = block_height
self.board = ut.create_box(width,
length,
height + fake_board_thickness,
color=(1, 0.7, 0, 1))
self.frontcamera_block = ut.create_box(0.4,
0.2,
0.4,
color=(.2, 0.7, 0, 0.2))
self.topcamera_block = ut.create_box(0.3,
0.08,
0.15,
color=(1, 0.7, 0, 0.2))
self.controlpc_block = ut.create_box(0.4,
0.4,
0.42,
color=(1, 0.7, 0, 0.2))
board_loc[0][-1] -= 0.5 * fake_board_thickness
ut.set_pose(self.board, board_loc)
ut.set_point(self.frontcamera_block, (0.6 + .15, 0, 0.1))
ut.set_point(self.topcamera_block, (0.5, 0, 0.95))
ut.set_point(self.controlpc_block, (-0.1, 0.45, 0.2))
#make circle
#make rectangle
self.hole = ut.create_box(0.092, 0.069, 0.001, color=(0.1, 0, 0, 1))
board_z = 0.013 + 0.005
#The perception z axis estimates are bad so let's use prior information to give it the right pose
for loc in [rectangle_loc, circle_loc, square_loc]:
if loc is not None:
loc[0][-1] = board_z + 0.5 * block_height
hole_goal[0][-1] = board_z + 0.5 * 0.001
if load_previous:
rectangle_loc = np.load("saves/rectangle_loc.npy",
allow_pickle=True)
circle_loc = np.load("saves/circle_loc.npy", allow_pickle=True)
obstacle_loc = np.load("saves/obstacle_loc.npy", allow_pickle=True)
square_loc = np.load("saves/square_loc.npy", allow_pickle=True)
hole_goal = np.load("saves/hole_loc.npy", allow_pickle=True)
for loc in [
rectangle_loc, circle_loc, obstacle_loc, hole_goal,
square_loc
]:
if loc is None or loc[0].any() is None:
loc = None
else:
np.save("saves/rectangle_loc.npy", rectangle_loc)
np.save("saves/circle_loc.npy", circle_loc)
np.save("saves/obstacle_loc.npy", obstacle_loc)
np.save("saves/hole_loc.npy", hole_goal)
np.save("saves/square_loc.npy", square_loc)
if obstacle_loc is not None:
self.obstacle = ut.create_box(0.08,
0.04,
0.1,
color=(0.5, 0.5, 0.5, 1))
obstacle_loc[0][-1] = board_z + 0.5 * 0.1
ut.set_pose(self.obstacle, obstacle_loc)
else:
self.obstacle = None
if circle_loc is not None:
radius = 0.078 / 2
self.circle = ut.create_cylinder(radius,
block_height,
color=(1, 1, 0, 1))
ut.set_pose(self.circle, circle_loc)
else:
self.circle = None
if rectangle_loc is not None:
self.rectangle = ut.create_box(0.092,
0.069,
block_height,
color=(0.3, 0, 0.1, 1))
ut.set_pose(self.rectangle, rectangle_loc)
else:
self.rectangle = None
if square_loc is not None:
self.square = ut.create_box(0.072,
0.072,
block_height,
color=(0.8, 0, 0.1, 1))
ut.set_pose(self.square, square_loc)
self.hole_goal = hole_goal
self.shape_name_to_shape = {}
self.shape_name_to_shape[Obstacle.__name__] = self.obstacle
ut.set_pose(self.hole, hole_goal)
self.shape_name_to_shape[Circle.__name__] = self.circle
self.shape_name_to_shape[Rectangle.__name__] = self.rectangle
self.shape_name_to_shape[Square.__name__] = self.square
input("workspace okay?")
p.saveBullet("curr_state.bt")
def setup_workspace(self, square=False):
self.floor = p.loadURDF("../../models/short_floor.urdf")
p.changeDynamics(self.floor, -1, mass=0)
#self.hollow = p.loadURDF("../models/hollow.urdf", (0,0,0), globalScaling=0.020)
length = 0.04
sec_width = 0.008
thick = 0.003
angle_correction = -0.25 #not a nice hack to make the pipe look better
box_w = 0.02
puzzle_w = 0.2
box_h = 0.04
self.box_h = box_h
self.box_l = 0.08
if square:
#make 4 boxes
clearance = 0.005
top_box_w = 0.5 * (puzzle_w - box_w) - clearance
box_top = create_box(top_box_w, puzzle_w, box_h)
side_box_w = (0.5) * (puzzle_w - 2 * clearance - box_w)
box_bottom = create_box(top_box_w, puzzle_w, box_h)
box_right = create_box(top_box_w, side_box_w, box_h)
box_left = create_box(top_box_w, side_box_w, box_h)
set_point(box_right, (0, 0.5 * (side_box_w + box_w +
(2 * clearance)), 0.5 * box_h))
set_point(box_left, (0, -0.5 * (side_box_w + box_w +
(2 * clearance)), 0.5 * box_h))
set_point(box_top,
(0.5 * (top_box_w + box_w) + clearance, 0, 0.5 * box_h))
set_point(box_bottom,
(-0.5 * (top_box_w + box_w) - clearance, 0, 0.5 * box_h))
self.hollow = [box_top, box_bottom, box_right, box_left]
else:
self.hollow = make_cylinder(12, sec_width, length, thick,
angle_correction)
p.changeDynamics(self.hollow, -1, mass=0)
set_pose(self.hollow, ((0.0, 0, 0.0), (0, 0.8, 0.8, 0)))
if square:
self.pipe = create_box(box_w,
box_w,
self.box_l,
mass=1,
color=(0, 0, 1, 1))
else:
self.pipe = create_cylinder(0.01, 0.1, mass=1, color=(0, 0, 1, 1))
p.changeDynamics(self.pipe,
-1,
mass=0.1,
lateralFriction=0.999,
rollingFriction=0.99,
spinningFriction=0.99,
restitution=0.05)
p.changeDynamics(self.robot,
9,
lateralFriction=0.999,
rollingFriction=0.99,
spinningFriction=0.99,
restitution=0.05)
p.changeDynamics(self.robot,
10,
lateralFriction=0.999,
rollingFriction=0.99,
spinningFriction=0.99,
restitution=0.05)
set_point(self.pipe, (0, 0.132, self.box_l / 2.))