-
Notifications
You must be signed in to change notification settings - Fork 37
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
Conversation
): | ||
time.sleep(1) | ||
count += 1 | ||
if count > 100: |
There was a problem hiding this comment.
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() |
There was a problem hiding this comment.
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
There was a problem hiding this 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
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" | ||
) | ||
|
There was a problem hiding this comment.
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 >?
@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" | ||
|
There was a problem hiding this comment.
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
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 | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
rewrite this function
- make timeout an argument
- make required precision an argument
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) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
make this configurable
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
rewrite , removing hardcoding
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) | ||
|
There was a problem hiding this comment.
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 ?
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): |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
clean up
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
same redo this function
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") |
There was a problem hiding this comment.
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
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 | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
clean up
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 | ||
} |
There was a problem hiding this comment.
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
obs['state_observation']['tcp_force'] = R_inv @ obs['state_observation']['tcp_force'] | ||
obs['state_observation']['tcp_torque'] = R_inv @ obs['state_observation']['tcp_torque'] |
There was a problem hiding this comment.
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
There was a problem hiding this 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): |
There was a problem hiding this comment.
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): |
There was a problem hiding this comment.
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): |
There was a problem hiding this comment.
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") |
There was a problem hiding this comment.
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', |
There was a problem hiding this comment.
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) |
There was a problem hiding this comment.
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: |
There was a problem hiding this comment.
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) |
There was a problem hiding this comment.
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()
Here's the preliminary combined franka server code for both Franka and Robotiq grippers. Note that this is untested on the real robot.