def movep(self, pose, raw=False):
''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame'''
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
q = self.ik(pose)
if q is not None:
self.movej(q, raw=raw)
else:
print("\nNO SOLUTION TO IK\n" * 20)
def saveFrame(self, frame, input_name, parent_tf=None):
try:
pose = transformations.KDLToPose(frame)
name = self._getSavingName(input_name)
if not parent_tf:
parent_tf = TfStorage.DEFAULT_TF_PARENT
#⬢⬢⬢⬢⬢➤ META
meta = {
'parent_tf': parent_tf,
'source_namespace': TfStorage.SOURCE_NAMESPACE
}
self.message_database.replace(name, pose, meta=meta)
except:
pass
def ik(self, pose):
if type(pose) is PyKDL.Frame:
pose = transformations.KDLToPose(pose)
# https://sdk.rethinkrobotics.com/wiki/API_Reference#Inverse_Kinematics_Solver_Service
hdr = Header(stamp=self.node.getCurrentTime(), frame_id='base')
# self.ikreq.seed_mode = self.ikreq.SEED_CURRENT
self.ikreq.pose_stamp = [PoseStamped(header=hdr, pose=pose)]
if self.iksvc is not None:
try:
res = self.iksvc(self.ikreq)
except Exception as e:
print("Service call failed: %s" % (e, ))
return None
joints = dict(zip(res.joints[0].name, res.joints[0].position))
return joints
else:
return None
print("Chessboard TF FOUND!!!!!!!!")
matrix_cube = getTfMatrix(robot_name, scan_name, base_frame)
matrix_tf = matrix_cube
number_of_frames = len(matrix_cube.frames_names)
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
# ▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇▇
sendNewMessage()
moving_msg = Int32()
os.system('clear')
while node.isActive():
moving_msg.data = moving_flag
moving_pub.publish(moving_msg)
tool_pose = Pose()
tool_frame = node.retrieveTransform("/{}/tool".format(robot_name),
"/{}/base_link".format(robot_name), -1)
if tool_frame is not None:
tool_pose = transformations.KDLToPose(tool_frame)
pose_pub.publish(tool_pose)
node.tick()
#addScanPose(msgs, "shape_scan_tf_3")
addScanPose(msgs, "shape_scan_tf_4")
addScanPose(msgs, "shape_scan_tf_5")
addScanPose(msgs, "shape_scan_tf_6")
addScanPose(msgs, "shape_scan_tf_7")
addScanPose(msgs, "shape_scan_tf_8")
addScanPose(msgs, "shape_scan_tf_13")
addScanPose(msgs, "shape_scan_tf_14")
addScanPose(msgs, "shape_scan_tf_15")
addScanPose(msgs, "shape_scan_tf_0")
addScanPose(msgs, "shape_scan_tf_1")
addScanPose(msgs, "shape_extra_s1")
addScanPose(msgs, "shape_extra_s2")
addScanPose(msgs, "shape_extra_s3")
addScanPose(msgs, "shape_extra_s4")
addScanPose(msgs, "shape_extra_s5")
addScanPose(msgs, "shape_extra_s6")
print msgs
sendNewMessage()
while node.isActive():
wrist = node.retrieveTransform("comau_smart_six/link6",
"comau_smart_six/base_link", -1)
if wrist:
pose = transformations.KDLToPose(wrist)
pose_pub.publish(pose)
node.tick()
def saveFrame(self, frame, input_name):
try:
pose = transformations.KDLToPose(frame)
self.message_database.replace(input_name, pose)
except:
pass