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 go_to_conf(self, conf):
control_joints(
self.pw.robot,
get_movable_joints(self.pw.robot) + list(self.finger_joints),
tuple(conf) + (self.target_grip, self.target_grip))
simulate_for_duration(0.3)
self.steps_taken += 0.3
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 make_traj(self, goal_pose, shape_class=Rectangle, in_board=False):
state = p.saveState()
#quat = p.getLinkState(self.robot, self.grasp_joint)[1]
quat = [1, 0, 0, 0]
#all of this only really makes sense with a full robot
joints_to_plan_for = ut.get_movable_joints(
self.robot)[:-2] #all but the fingers
end_conf = self.compute_IK(goal_pose,
joints_to_plan_for,
cur_pose=ut.get_joint_positions(
self.robot, joints_to_plan_for))
if end_conf is None:
print("No IK solution found")
p.restoreState(state)
return None
end_conf = end_conf[:len(joints_to_plan_for)]
p.restoreState(state)
#for attachment in self.in_hand:
# attachment.assign()
obstacles = [
obs for obs in [
self.obstacle, self.square, self.circle, self.topcamera_block,
self.frontcamera_block
] if obs is not None
and obs != self.shape_name_to_shape[shape_class.__name__]
]
if not in_board:
obstacles.append(self.board)
#disabled_body_collisions = [(self.board, self.rectangle), (self.hole, self.rectangle),(self.board, self.square),(self.robot, self.shape_name_to_shape[shape_class.__name__])]
disabled_body_collisions = [
(self.hole, self.rectangle),
(self.robot, self.shape_name_to_shape[shape_class.__name__])
]
#if len(self.in_hand) == 1:
# disabled_collisions.append((self.in_hand[0].child, self.board))
# disabled_collisions.append((self.board, self.in_hand[0].child))
traj = ut.plan_joint_motion(
self.robot,
joints_to_plan_for,
end_conf,
obstacles=obstacles,
attachments=self.in_hand,
self_collisions=True,
disabled_body_collisions=disabled_body_collisions,
weights=None,
resolutions=None,
smooth=100,
restarts=20,
iterations=100)
p.restoreState(state)
return traj
def get_null_space(self, joints_to_plan_for):
rest = np.mean(np.vstack([
ut.get_min_limits(self.robot, joints_to_plan_for),
ut.get_max_limits(self.robot, joints_to_plan_for)
]),
axis=0)
rest = [
ut.get_joint_positions(self.robot,
ut.get_movable_joints(self.robot))
]
lower = ut.get_min_limits(self.robot, joints_to_plan_for)
upper = ut.get_max_limits(self.robot, joints_to_plan_for)
ranges = 10 * np.ones(len(joints_to_plan_for))
null_space = [lower, upper, ranges, [rest]]
return null_space
def fk_rigidtransform(self, joint_vals, return_rt=True):
state = p.saveState()
ut.set_joint_positions(
self.robot,
ut.get_movable_joints(self.robot)[:len(joint_vals)], joint_vals)
link_trans, link_quat = ut.get_link_pose(self.robot, self.grasp_joint)
p.restoreState(state)
if return_rt:
link_trans = list(link_trans)
link_trans[-1] -= 0.055 #conversion to franka_tool
link_quat = np.hstack([link_quat[-1], link_quat[0:3]])
rt = RigidTransform(translation=link_trans,
rotation=Quaternion(link_quat).rotation_matrix)
return rt
else:
return (link_trans, link_quat)
def make_traj(self, goal_pose, shape_class=Rectangle, in_board=False):
state = p.saveState()
#quat = p.getLinkState(self.robot, self.grasp_joint)[1]
quat = [1, 0, 0, 0]
#all of this only really makes sense with a full robot
joints_to_plan_for = ut.get_movable_joints(
self.robot)[:-2] #all but the fingers
null_space = self.get_null_space(joints_to_plan_for)
end_conf = ut.inverse_kinematics(
self.robot,
self.grasp_joint,
goal_pose,
movable_joints=joints_to_plan_for,
null_space=null_space) #end conf to be in the goal loc
if end_conf is None:
end_conf = ut.inverse_kinematics(
self.robot,
self.grasp_joint,
goal_pose,
movable_joints=joints_to_plan_for,
null_space=None) #end conf to be in the goal loc
if end_conf is None:
print("No IK solution found")
p.restoreState(state)
return None
end_conf = end_conf[:len(joints_to_plan_for)]
p.restoreState(state)
#for attachment in self.in_hand:
# attachment.assign()
obstacles = []
traj = ut.plan_joint_motion(self.robot,
joints_to_plan_for,
end_conf,
obstacles=obstacles,
attachments=self.in_hand,
self_collisions=True,
disabled_collisions=set(self.in_hand),
weights=None,
resolutions=None,
smooth=100,
restarts=5,
iterations=100)
p.restoreState(state)
return traj
def compute_IK(self,
goal_pose,
joints_to_plan_for,
cur_pose=None,
max_iter=100):
solutions = []
good_num_solutions = 10
for i in range(max_iter):
rest = np.mean(np.vstack([
ut.get_min_limits(self.robot, joints_to_plan_for),
ut.get_max_limits(self.robot, joints_to_plan_for)
]),
axis=0)
rest = [
ut.get_joint_positions(self.robot,
ut.get_movable_joints(self.robot))
]
lower = ut.get_min_limits(self.robot, joints_to_plan_for)
upper = ut.get_max_limits(self.robot, joints_to_plan_for)
lower = [
-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973
]
upper = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]
null_space = None
end_conf = ut.inverse_kinematics(
self.robot,
self.grasp_joint,
goal_pose,
movable_joints=joints_to_plan_for,
null_space=null_space) #end conf to be in the goal loc
random_jts = []
for i in range(7):
random_jts.append(
np.random.uniform(low=lower[i], high=upper[i]))
self.set_joints(random_jts)
#ranges = np.array(upper)-np.array(lower)#2*np.ones(len(joints_to_plan_for))
#null_space = [lower, upper, ranges, [rest]]
if end_conf is not None:
solutions.append(end_conf)
if len(solutions) > good_num_solutions or cur_pose is None:
break
return solutions[np.argmin([
np.linalg.norm(np.array(cur_pose) - np.array(solution[:-2]))
for solution in solutions
])]
def get_push_down_traj(self, traj, shape_class=Rectangle):
state = p.saveState()
#Move in that last bit
joint_vals = traj[-1]
ut.set_joint_positions(
self.robot,
ut.get_movable_joints(self.robot)[:len(joint_vals)], joint_vals)
goal_pose = self.fk_rigidtransform(traj[-1], return_rt=False)
new_trans = (goal_pose[0][0], goal_pose[0][1],
goal_pose[0][2] - 1 * self.hole_depth)
end_traj = self.make_traj((new_trans, goal_pose[1]),
shape_class=shape_class,
in_board=True)
if end_traj is None:
import ipdb
ipdb.set_trace()
print("No push down")
return traj
p.restoreState(state)
traj = np.vstack([traj, end_traj])
return traj
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 set_joints(self, joint_vals):
ut.set_joint_positions(
self.robot,
ut.get_movable_joints(self.robot)[:len(joint_vals)], joint_vals)
for attachment in self.in_hand:
attachment.assign()
def go_to_pose(self,
target_pose,
obstacles=[],
attachments=[],
cart_traj=False,
use_policy=False,
maxForce=100):
total_traj = []
if self.pw.handonly:
p.changeConstraint(self.pw.cid,
target_pose[0],
target_pose[1],
maxForce=maxForce)
for i in range(80):
simulate_for_duration(self.dt_pose)
self.pw.steps_taken += self.dt_pose
if self.pw.steps_taken >= self.total_timeout:
return total_traj
ee_loc = p.getBasePositionAndOrientation(self.pw.robot)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
total_traj.append(ee_loc)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
for i in range(50):
end_conf = inverse_kinematics_helper(self.pw.robot,
self.ee_link, target_pose)
if not use_policy:
motion_plan = plan_joint_motion(self.pw.robot,
get_movable_joints(
self.pw.robot),
end_conf,
obstacles=obstacles,
attachments=attachments)
if motion_plan is not None:
for conf in motion_plan:
self.go_to_conf(conf)
ee_loc = p.getLinkState(self.pw.robot, 8)
if cart_traj:
total_traj.append(ee_loc[0] + ee_loc[1])
else:
total_traj.append(conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
ee_loc = p.getLinkState(self.pw.robot, 8)
next_loc = self.policy.predict(
np.array(ee_loc[0] + ee_loc[1]).reshape(1, 7))[0]
next_pos = next_loc[0:3]
next_quat = next_loc[3:]
next_conf = inverse_kinematics_helper(
self.pw.robot, self.ee_link, (next_pos, next_quat))
if cart_traj:
total_traj.append(next_loc)
else:
total_traj.append(next_conf)
self.go_to_conf(next_conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
ee_loc = p.getLinkState(self.pw.robot, 8)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
return total_traj