def Ptp_lin (self, parsed_code, command):
global active_frame
param_dict=dict()
pose=Pose()
X = float(str(parsed_code.get("X")).strip("[]")) / 100
Y = float(str(parsed_code.get("Y")).strip("[]")) / 100
Z = float(str(parsed_code.get("Z")).strip("[]")) / 100
qX = float(str(parsed_code.get("qX")).strip("[]"))
qY = float(str(parsed_code.get("qY")).strip("[]"))
qZ = float(str(parsed_code.get("qZ")).strip("[]"))
qW = float(str(parsed_code.get("qW")).strip("[]"))
Relative = parsed_code.get("relative")
Vscale = str(parsed_code.get("vscale")).strip("[]")
if X is None: #See if we have Joint or Cartesian space by looking if X coordinate returns None, if yes get Joint values and append to list.
pose = []
J1 = float(str(parsed_code.get("X")).strip("[]"))
J2 = float(str(parsed_code.get("X")).strip("[]"))
J3 = float(str(parsed_code.get("X")).strip("[]"))
J4 = float(str(parsed_code.get("X")).strip("[]"))
J5 = float(str(parsed_code.get("X")).strip("[]"))
J6 = float(str(parsed_code.get("X")).strip("[]"))
pose.append(math.radians(J1))
pose.append(math.radians(J2))
pose.append(math.radians(J3))
pose.append(math.radians(J4))
pose.append(math.radians(J5))
pose.append(math.radians(J6))
else:
pose.position = Point(X, Y, Z)
if qX is not None:
pose.orientation = Quaternion(qX, qY, qZ, qW)
if qX is None and R is not None:
R = float(str(parsed_code.get("R")).strip("[]"))
P = float(str(parsed_code.get("P")).strip("[]"))
Y = float(str(parsed_code.get("Y")).strip("[]"))
pose.orientation = robot.from_euler(math.radians(R), math.radians(P), math.radians(Y))
if Relative is not None:
param_dict['relative'] = True
if Vscale is not None:
param_dict['vel_scale'] = float(Vscale)
if active_frame is not "prbt_base":
param_dict['reference_frame'] = active_frame
self.execute(command, pose, param_dict)
