def set_fts_transform(self, T):
""" Set the homogenous transform from the force-torque-sensor frame to eff frame"""
self.S2E = nu.array(T, float) #Transform point in sensor frame to eff
self.E2S = scipy.linalg.inv(
self.S2E) #Transform point in eff to sensor frame.
self.FS2FE = m3tc.force_moment_transform(
self.S2E) #Transform wrench in sensor frame to eff frame
self.FE2FS = m3tc.force_moment_transform(
self.E2S) #Transform wrench in eff frame to sensor frame
def set_tool_transform(self, T):
""" Set the homgenous transform from tool frame to end frame"""
self.T2E = nu.array(
T, nu.float32) #Transform point in hand frame to arm tool.
self.E2T = nu.transpose(
self.T2E) #Transform point in arm tool to hand frame.
self.FE2FT = m3tc.force_moment_transform(
self.E2T) #Transform wrench in end frame to tool frame
self.FT2FE = m3tc.force_moment_transform(
self.T2E) #Transform wrench in tool frame to end frame
def eff_wrench_2_end_wrench_transform(self):
T = self.T80.copy()
T[0:3, 3] = 0
return m3tc.force_moment_transform(T)
def set_fts_transform(self,T): """ Set the homogenous transform from the force-torque-sensor frame to eff frame""" self.S2E = nu.array(T,float) #Transform point in sensor frame to eff self.E2S = scipy.linalg.inv(self.S2E) #Transform point in eff to sensor frame. self.FS2FE = m3tc.force_moment_transform(self.S2E) #Transform wrench in sensor frame to eff frame self.FE2FS = m3tc.force_moment_transform(self.E2S) #Transform wrench in eff frame to sensor frame
def eff_wrench_2_end_wrench_transform(self):
T = self.T80.copy()
T[0:3, 3] = 0
return m3tc.force_moment_transform(T)
def set_tool_transform(self, T):
""" Set the homgenous transform from tool frame to end frame"""
self.T2E = nu.array(T, nu.float32) # Transform point in hand frame to arm tool.
self.E2T = nu.transpose(self.T2E) # Transform point in arm tool to hand frame.
self.FE2FT = m3tc.force_moment_transform(self.E2T) # Transform wrench in end frame to tool frame
self.FT2FE = m3tc.force_moment_transform(self.T2E) # Transform wrench in tool frame to end frame
