Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

developing combined franka server #5

Closed
wants to merge 2 commits into from
Closed

developing combined franka server #5

wants to merge 2 commits into from

Conversation

charlesxu0124
Copy link
Collaborator

Here's the preliminary combined franka server code for both Franka and Robotiq grippers. Note that this is untested on the real robot.

):
time.sleep(1)
count += 1
if count > 100:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is assume 10HZ, so 10s ?

probably don't use this hardcoding


"""Starts Impedence controller"""
if GRIPPER_TYPE == "Franka":
l = FrankaGripperServer()
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

change the variable name to make it more informative

Copy link
Collaborator

@jianlanluo jianlanluo left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you delete unused function here ?> also see my comments in this py file

also fix the precommit

Comment on lines +270 to +293
try:
roscore = subprocess.Popen("roscore")
time.sleep(1)
except:
pass

"""Starts Impedence controller"""
if GRIPPER_TYPE == "Franka":
l = FrankaGripperServer()
elif GRIPPER_TYPE == "Robotiq":
l = RobotiqServer()
elif GRIPPER_TYPE == "None":
l = RobotServer()
else:
raise NotImplementedError("Gripper Type Not Implemented")

l.start_impedence()
rospy.init_node("franka_control_api")

## Defines the ros topics to publish to
client = Client(
"/cartesian_impedance_controllerdynamic_reconfigure_compliance_param_node"
)

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you reorganize this block of code ? make it a function and move to main >?

Comment on lines +416 to +434
@app.route("/precision_mode", methods=["POST"])
def precision_mode():
client.update_configuration({"translational_Ki": 30})
client.update_configuration({"rotational_Ki": 10})
for direction in ["x", "y", "z", "neg_x", "neg_y", "neg_z"]:
client.update_configuration({"translational_clip_" + direction: 0.1})
client.update_configuration({"rotational_clip" + direction: 0.1})
return "Precision"

## Route for decreasing controller gain
@app.route("/compliance_mode", methods=["POST"])
def compliance_mode():
client.update_configuration({"translational_Ki": 10})
client.update_configuration({"rotational_Ki": 0})
for direction in ["x", "y", "z", "neg_x", "neg_y", "neg_z"]:
client.update_configuration({"translational_clip" + direction: 0.007})
client.update_configuration({"rotational_clip" + direction: 0.04})
return "Compliance"

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

are these even used ?

delete if not used

Comment on lines +51 to +128
def go_to_rest(self, jpos=False):
count = 0
requests.post(self.url + "precision_mode")
if jpos:
restp_new = copy.deepcopy(self.currpos)
restp_new[2] = 0.3
dp = restp_new - self.currpos
count_1 = 0
self._send_pos_command(self.currpos)
requests.post(self.url + "precision_mode")
while (
(np.linalg.norm(dp[:3]) > 0.03 or np.linalg.norm(dp[3:]) > 0.04)
) and count_1 < 50:
if np.linalg.norm(dp[3:]) > 0.05:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.03:
dp[:3] = 0.03 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp_new - self.currpos
count_1 += 1

print("JOINT RESET")
requests.post(self.url + "jointreset")
else:
# print("RESET")
self.update_currpos()
restp = copy.deepcopy(self.resetpos[:])
if self.randomreset:
restp[:2] += np.random.uniform(-0.005, 0.005, (2,))
restp[2] += np.random.uniform(-0.005, 0.005, (1,))
# restyaw += np.random.uniform(-np.pi / 6, np.pi / 6)
# restp[3:] = self.euler_2_quat(np.pi, 0, restyaw)

restp_new = copy.deepcopy(restp)
restp_new[2] = 0.13 #cable
dp = restp_new - self.currpos

height = np.zeros_like(self.resetpos)
height[2] = 0.02
while count < 10:
self._send_pos_command(self.currpos + height)
time.sleep(0.1)
self.update_currpos()
count += 1

count = 0
while count < 200 and (
np.linalg.norm(dp[:3]) > 0.01 or np.linalg.norm(dp[3:]) > 0.03
):
if np.linalg.norm(dp[3:]) > 0.02:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.02:
dp[:3] = 0.02 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp_new - self.currpos
count += 1

dp = restp - self.currpos
count = 0
while count < 20 and (
np.linalg.norm(dp[:3]) > 0.01 or np.linalg.norm(dp[3:]) > 0.01
):
if np.linalg.norm(dp[3:]) > 0.05:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.02:
dp[:3] = 0.02 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp - self.currpos
count += 1
requests.post(self.url + "peg_compliance_mode")
return count < 50

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

rewrite this function

  1. make timeout an argument
  2. make required precision an argument

Comment on lines +148 to +164
except queue.Empty:
input(f'{key} camera frozen. Check connect, then press enter to relaunch...')
cap.close()
# if key == 'side_1':
# cap = RSCapture(name='side_1', serial_number='128422270679', depth=True)
# elif key == 'side_2':
# cap = RSCapture(name='side_2', serial_number='127122270146', depth=True)
if key == 'wrist_1':
cap = RSCapture(name='wrist_1', serial_number='130322274175', depth=False)
elif key == 'wrist_2':
# cap = RSCapture(name='wrist_2', serial_number='127122270572', depth=False)
cap = RSCapture(name='wrist_2', serial_number='127122270572', depth=False)
elif key == 'side_1':
cap = RSCapture(name='side_1', serial_number='128422272758', depth=False)
else:
raise KeyError
self.cap[key] = VideoCapture(cap)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

make this configurable

Comment on lines +171 to +190
pose[:3] = np.clip(
pose[:3], self.xyz_bounding_box.low, self.xyz_bounding_box.high
)

euler = Rotation.from_quat(pose[3:]).as_euler("xyz")
old_sign = np.sign(euler[0])
euler[0] = (
np.clip(
euler[0] * old_sign,
self.rpy_bounding_box.low[0],
self.rpy_bounding_box.high[0],
)
* old_sign
)
euler[1:] = np.clip(
euler[1:], self.rpy_bounding_box.low[1:], self.rpy_bounding_box.high[1:]
)
pose[3:] = Rotation.from_euler("xyz", euler).as_quat()

# Clip xyz to inner box
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

rewrite , removing hardcoding

Comment on lines +198 to +227
def intersect_line_bbox(self, p1, p2, bbox_min, bbox_max):
# Define the parameterized line segment
# P(t) = p1 + t(p2 - p1)
tmin = 0
tmax = 1

for i in range(3):
if p1[i] < bbox_min[i] and p2[i] < bbox_min[i]:
return None
if p1[i] > bbox_max[i] and p2[i] > bbox_max[i]:
return None

# For each axis (x, y, z), compute t values at the intersection points
if abs(p2[i] - p1[i]) > 1e-10: # To prevent division by zero
t1 = (bbox_min[i] - p1[i]) / (p2[i] - p1[i])
t2 = (bbox_max[i] - p1[i]) / (p2[i] - p1[i])

# Ensure t1 is smaller than t2
if t1 > t2:
t1, t2 = t2, t1

tmin = max(tmin, t1)
tmax = min(tmax, t2)

if tmin > tmax:
return None

# Compute the intersection point using the t value
intersection = p1 + tmin * (p2 - p1)

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is this even used after UR5 cable routing ?

Comment on lines +230 to +275
def step(self, action):
start_time = time.time()
action = np.clip(action, self.action_space.low, self.action_space.high)
if self.actionnoise > 0:
a = action[:3] + np.random.uniform(
-self.actionnoise, self.actionnoise, (3,)
)
else:
a = action[:3]

self.nextpos = self.currpos.copy()
self.nextpos[:3] = self.nextpos[:3] + a

### GET ORIENTATION FROM ACTION
self.nextpos[3:] = (
Rotation.from_euler("xyz", action[3:6])
* Rotation.from_quat(self.currpos[3:])
).as_quat()

gripper = action[-1]
if gripper > 0:
if not self.drop_box.contains(self.currpos[:2]):
gripper = (self.currgrip + 1) % 2
self.set_gripper(gripper)

self._send_pos_command(self.clip_safety_box(self.nextpos))

self.curr_path_length += 1
dl = time.time() - start_time

time.sleep(max(0, (1.0 / self.hz) - dl))

self.update_currpos()
ob = self._get_obs()
obs_xyz = ob['state_observation']['tcp_pose'][:3]
obs_rpy = ob['state_observation']['tcp_pose'][3:]
reward = 0
done = self.curr_path_length >= 40 #100
# if not self.xyz_bounding_box.contains(obs_xyz) or not self.rpy_bounding_box.contains(obs_rpy):
# # print('Truncated: Bouding Box')
# print("xyz: ", self.xyz_bounding_box.contains(obs_xyz), obs_xyz)
# print("rortate: ", self.rpy_bounding_box.contains(obs_rpy), obs_rpy)
# return ob, 0, True, True, {}
return ob, int(reward), done, done, {}

def reset(self, jpos=False, gripper=None, require_input=False):
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

clean up

Comment on lines +37 to +113
def go_to_rest(self, jpos=False):
count = 0
requests.post(self.url + "precision_mode")
if jpos:
restp_new = copy.deepcopy(self.currpos)
restp_new[2] = 0.3
dp = restp_new - self.currpos
count_1 = 0
self._send_pos_command(self.currpos)
requests.post(self.url + "precision_mode")
while (
(np.linalg.norm(dp[:3]) > 0.03 or np.linalg.norm(dp[3:]) > 0.04)
) and count_1 < 50:
if np.linalg.norm(dp[3:]) > 0.05:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.03:
dp[:3] = 0.03 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp_new - self.currpos
count_1 += 1

print("JOINT RESET")
requests.post(self.url + "jointreset")
else:
# print("RESET")
self.update_currpos()
restp = copy.deepcopy(self.resetpos[:])
if self.randomreset:
restp[:2] += np.random.uniform(-0.005, 0.005, (2,))
restp[2] += np.random.uniform(-0.005, 0.005, (1,))
# restyaw += np.random.uniform(-np.pi / 6, np.pi / 6)
# restp[3:] = self.euler_2_quat(np.pi, 0, restyaw)

restp_new = copy.deepcopy(restp)
restp_new[2] = 0.15 #cable
dp = restp_new - self.currpos

height = np.zeros_like(self.resetpos)
height[2] = 0.02
while count < 10:
self._send_pos_command(self.currpos + height)
time.sleep(0.1)
self.update_currpos()
count += 1

count = 0
while count < 200 and (
np.linalg.norm(dp[:3]) > 0.01 or np.linalg.norm(dp[3:]) > 0.03
):
if np.linalg.norm(dp[3:]) > 0.02:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.02:
dp[:3] = 0.02 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp_new - self.currpos
count += 1

dp = restp - self.currpos
count = 0
while count < 20 and (
np.linalg.norm(dp[:3]) > 0.01 or np.linalg.norm(dp[3:]) > 0.01
):
if np.linalg.norm(dp[3:]) > 0.05:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.02:
dp[:3] = 0.02 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp - self.currpos
count += 1
requests.post(self.url + "peg_compliance_mode")
return count < 50
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same redo this function

Comment on lines +60 to +101
def go_to_rest(self, jpos=False):
count = 0
if self.currpos[2] < 0.06:
restp_new = copy.deepcopy(self.currpos)
restp_new[2] += 0.02
dp = restp_new - self.currpos
while count < 200 and (
np.linalg.norm(dp[:3]) > 0.01 or np.linalg.norm(dp[3:]) > 0.03
):
if np.linalg.norm(dp[3:]) > 0.02:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.02:
dp[:3] = 0.02 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp_new - self.currpos
count += 1


requests.post(self.url + "precision_mode")
if jpos:
restp_new = copy.deepcopy(self.currpos)
restp_new[2] = 0.2
dp = restp_new - self.currpos
count_1 = 0
self._send_pos_command(self.currpos)
requests.post(self.url + "precision_mode")
while (
(np.linalg.norm(dp[:3]) > 0.03 or np.linalg.norm(dp[3:]) > 0.04)
) and count_1 < 50:
if np.linalg.norm(dp[3:]) > 0.05:
dp[3:] = 0.05 * dp[3:] / np.linalg.norm(dp[3:])
if np.linalg.norm(dp[:3]) > 0.03:
dp[:3] = 0.03 * dp[:3] / np.linalg.norm(dp[:3])
self._send_pos_command(self.currpos + dp)
time.sleep(0.1)
self.update_currpos()
dp = restp_new - self.currpos
count_1 += 1

print("JOINT RESET")
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if reset / get img was called across all envs, should make another file, and import those from that file in env.py

Comment on lines +106 to +163
class SpacemouseIntervention(ProxyEnv):
def __init__(self, env, gripper_enabled=False):
ProxyEnv.__init__(self, env)
self._wrapped_env = env
self.action_space = self._wrapped_env.action_space
self.gripper_enabled = gripper_enabled
if self.gripper_enabled:
assert self.action_space.shape == (7,) # maybe not so elegant
self.observation_space = self._wrapped_env.observation_space
self.expert = SpaceMouseExpert(
xyz_dims=3,
xyz_remap=[0, 1, 2],
xyz_scale=200,
rot_scale=200,
all_angles=True
)
self.last_intervene = 0

def expert_action(self, action):
'''
Input:
- action: policy action
Output:
- action: spacemouse action if nonezero; else, policy action
'''
controller_a, _, left, right = self.expert.get_action()
expert_a = np.zeros((6,))
if self.gripper_enabled:
expert_a = np.zeros((7,))
expert_a[-1] = np.random.uniform(-1, 0)

expert_a[:3] = controller_a[:3] # XYZ
expert_a[3] = controller_a[4] # Roll
expert_a[4] = controller_a[5] # Pitch
expert_a[5] = -controller_a[6] # Yaw

if self.gripper_enabled:
if left:
expert_a[6] = np.random.uniform(0, 1)
self.last_intervene = time.time()

if np.linalg.norm(expert_a[:6]) > 0.001:
self.last_intervene = time.time()
else:
if np.linalg.norm(expert_a) > 0.001:
self.last_intervene = time.time()

if time.time() - self.last_intervene < 0.5:
return expert_a, left, right
return action, left, right

def step(self, action):
expert_action, left, right = self.expert_action(action)
o, r, done, truncated, env_info = self._wrapped_env.step(expert_action)
env_info['expert_action'] = expert_action
env_info['right'] = right
return o, r, done, truncated, env_info

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

clean up

Comment on lines +190 to +194
obs['state_observation']),
'wrist_1': obs['image_observation']['wrist_1'][...,::-1], # flip color channel
'wrist_2': obs['image_observation']['wrist_2'][...,::-1], # flip color channel
# 'side_1': obs['image_observation']['side_1'][...,::-1], # flip color channel
}
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we should probably do this in the env side

Comment on lines +246 to +247
obs['state_observation']['tcp_force'] = R_inv @ obs['state_observation']['tcp_force']
obs['state_observation']['tcp_torque'] = R_inv @ obs['state_observation']['tcp_torque']
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what are you doing here? F/T is always in EE frame

Copy link
Member

@youliangtan youliangtan left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is my first pass of the code review. Overall more work needs to be done for the refactoring (configurable configs, cleaning up of wrappers and env, etc.). Many of the impl can be made common in the FrankaRobotiqEnv, so reduce code duplications in pcb, cable and bin-pick. Also would recommend making this robot_infra as another package.

I can give another review after the first cleanups. Thanks for the hardwork.

self.wrist2.release()


class FrankaRobotiq(gym.Env):
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Rename this FrankaRobotiqEnv

yaw, pitch, roll = quat.yaw_pitch_roll
return yaw + np.pi, pitch, roll

def euler_2_quat(self, yaw=np.pi / 2, pitch=0.0, roll=np.pi):
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Move euler_2_quat and quat_2_euler to a utils.py

state_observation=state_observation
))

def go_to_rest(self, jpos=False):
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This entire code block is quite unreadable. Requires more cleanup (e.g. what is jpos). Add those val (retry_count, tolerance, thresh, etc.) as configs/params and also add comments. type cast jpos: np.narray is always recommended

elif GRIPPER_TYPE == "None":
pass
else:
raise NotImplementedError("Gripper Type Not Implemented")
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use argparser

@@ -0,0 +1,17 @@
from setuptools import setup
setup(name='franka_env',
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Now serl is organized as

  • serl
    • franka_sim
    • serl_launcher
    • examples
    • franka_env (TODO, or other names, serl_franka_infra)

Thus, if it make sense try to organize the robot code into a separate pkg, and add executables to examples or scripts

# elif key == 'side_2':
# cap = RSCapture(name='side_2', serial_number='127122270146', depth=True)
if key == 'wrist_1':
cap = RSCapture(name='wrist_1', serial_number='130322274175', depth=False)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hard coded serial_num again. should have all these in a init_rs() function. All these detailed impl of getting the cam can be placed in rs_capture.py too




try:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

have all these being placed within if __name__ == '__main__':?

Also, if this script flask server script is getting too long, would split both the flask server and "robot ros server" into 2 different scripts. I would also rospy.init_node() the node in the class init.

@app.route("/pose", methods=["POST"])
def pose():
pos = np.array(request.json["arr"])
print("Moving to", pos)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if these print out is getting overwhelming, can consider use logging.debug()

@jianlanluo jianlanluo closed this Jan 11, 2024
@youliangtan youliangtan mentioned this pull request Jan 11, 2024
@charlesxu0124 charlesxu0124 deleted the infra_dev branch January 29, 2024 02:45
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants