def publish(self): if self.last_pub and (rospy.Time.now() - self.last_pub) < self.interval: return (Tpose, Ttf, frames, stamp) = self.get_Tpose() frame_id = frames[0] if Tpose is None: return self.last_pub = rospy.Time.now() Tpose_p = Tpose[0:3, 3] Tpose_q = tft.quaternion_from_matrix(Tpose) if self.options.pose: pub_msg = PoseStamped() pub_msg.header.stamp = stamp pub_msg.header.frame_id = frame_id pub_msg.pose.position = Point(*(Tpose_p.tolist())) pub_msg.pose.orientation = Quaternion(*(Tpose_q.tolist())) self.pub.publish(pub_msg) if self.options.tf: from_frame = frames[1] to_frame = frames[2] tf_p = Ttf[0:3, 3] tf_q = tft.quaternion_from_matrix(Ttf) if self.options.tf_always_new: stamp = rospy.Time.now() self.br.sendTransform(tf_p, tf_q, stamp, to_frame, from_frame)
def interpolate_cartesian(start_pose, end_pose, num_steps): diff_pos = end_pose[:3,3] - start_pose[:3,3] start_quat = tf_trans.quaternion_from_matrix(start_pose) end_quat = tf_trans.quaternion_from_matrix(end_pose) mat_list = [] for fraction in np.linspace(0, 1, num_steps): cur_quat = tf_trans.quaternion_slerp(start_quat, end_quat, fraction) cur_mat = np.mat(tf_trans.quaternion_matrix(cur_quat)) cur_mat[:3,3] = start_pose[:3,3] + fraction * diff_pos mat_list.append(cur_mat) return mat_list
def quaternion_dist(B_a, B_b): quat_a = tf_trans.quaternion_from_matrix(B_a) quat_b = tf_trans.quaternion_from_matrix(B_b) quat_a_norm = quat_a / np.linalg.norm(quat_a) quat_b_norm = quat_b / np.linalg.norm(quat_b) dot = np.dot(quat_a_norm, quat_b_norm) if dot > 0.99999: dist = 0 else: dist = np.arccos(dot) return dist
def pack_marker(pos, _xaxis): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.ARROW marker.action = marker.ADD marker.scale.x = 3.0 marker.scale.y = 0.2 marker.scale.z = 0.2 # get some likely scale marker.color.a = 1.0 # calculate q from xaxis xaxis = np.array(_xaxis) / np.linalg.norm(_xaxis) yaxis = np.cross(xaxis, [0, 1, 0]) zaxis = np.cross(xaxis, yaxis) R = np.array([list(xaxis), yaxis, zaxis]).T T = np.hstack( (R, np.zeros((3,1))) ) T = np.vstack( (T, np.array([0, 0, 0, 1]).reshape((1,4))) ) q = transformations.quaternion_from_matrix(T) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = pos[2] return marker
def fill_from_Orientation_Data(o): '''Fill messages with information from 'Orientation Data' MTData2 block.''' self.pub_imu = True try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] except KeyError: pass try: # FIXME check that Euler angles are in radians x, y, z, w = quaternion_from_euler(radians(o['Roll']), radians(o['Pitch']), radians(o['Yaw'])) except KeyError: pass try: a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\ o['e'], o['f'], o['g'], o['h'], o['i'] m = identity_matrix() m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i)) x, y, z, w = quaternion_from_matrix(m) except KeyError: pass self.imu_msg.orientation.x = x self.imu_msg.orientation.y = y self.imu_msg.orientation.z = z self.imu_msg.orientation.w = w self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.))
def numpy_matrix_to_quaternion(np_matrix): '''Given a 3x3 rotation matrix, return its geometry_msgs Quaternion''' assert np_matrix.shape == (3, 3), "Must submit 3x3 rotation matrix" pose_mat = np.eye(4) pose_mat[:3, :3] = np_matrix np_quaternion = transformations.quaternion_from_matrix(pose_mat) return geometry_msgs.Quaternion(*np_quaternion)
def pack_pose(time, child, parent, matrix=None, trans=None, quat=None): if matrix is not None and (trans is None and quat is None): trans = tfs.translation_from_matrix(matrix) quat = tfs.quaternion_from_matrix(matrix) elif matrix is None and trans is not None and quat is not None: matrix = None # nothing else: print 'invalid use' t = TransformStamped() t.header.frame_id = parent t.header.stamp = time t.child_frame_id = child t.transform.translation.x = trans[0] t.transform.translation.y = trans[1] t.transform.translation.z = trans[2] quat = numpy.array(quat) quat = quat / numpy.linalg.norm(quat, ord=2) t.transform.rotation.x = quat[0] t.transform.rotation.y = quat[1] t.transform.rotation.z = quat[2] t.transform.rotation.w = quat[3] return t
def matrix_to_pose(matrix): # type: (np.ndarray) -> Pose quat_array = quaternion_from_matrix(matrix) orientation = array_to_quaternion(quat_array) position = array_to_point(matrix[0:3, 3]) return Pose(position=position, orientation=orientation)
def interpolate_ep(self, ep_a, ep_b, t_vals): pos_a, rot_a = ep_a pos_b, rot_b = ep_b num_samps = len(t_vals) pos_waypoints = np.array(pos_a) + np.array(np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals) pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T] rot_homo_a, rot_homo_b = np.eye(4), np.eye(4) rot_homo_a[:3,:3] = rot_a rot_homo_b[:3,:3] = rot_b quat_a = tf_trans.quaternion_from_matrix(rot_homo_a) quat_b = tf_trans.quaternion_from_matrix(rot_homo_b) rot_waypoints = [] for t in t_vals: cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t) rot_waypoints.append(np.mat(tf_trans.quaternion_matrix(cur_quat))[:3,:3]) return zip(pos_waypoints, rot_waypoints)
def calibrate3d(self): A = np.matrix(self.A) B = np.matrix(self.B) ret_R, ret_t = rigid_transform_3D(A, B) new_col = ret_t.reshape(3, 1) tmp = np.append(ret_R, new_col, axis=1) aug=np.array([[0.0,0.0,0.0,1.0]]) translation = np.squeeze(np.asarray(ret_t)) T = np.append(tmp,aug,axis=0) quaternion = quaternion_from_matrix(T) # Send the transform to ROS self.tf_broadcaster.sendTransform(ret_t ,quaternion , rospy.Time.now(), self.kinect.link_frame,self.base_frame) ## Compute inverse of transformation invR = ret_R.T invT = -invR * ret_t B_in_A = np.empty(B.shape) for i in xrange(len(B)): p = invR*B[i].T + invT B_in_A[i] = p.T ## Compute the standard deviation err = A-B_in_A std = np.std(err,axis=0) self.static_transform = '<node pkg="tf" type="static_transform_publisher" name="'+self.transform_name+'" args="'\ +' '.join(map(str, translation))+' '+' '.join(map(str, quaternion))+' '+self.base_frame+' '+self.kinect.link_frame+' 100" />' self.depth_pt_pub.publish(self.get_prepared_pointcloud(A,self.kinect.link_frame)) self.world_pt_pub.publish(self.get_prepared_pointcloud(B,self.base_frame))
def TransformToPose( G ): t = array( G )[0:3,3] q = tr.quaternion_from_matrix( G ) orientation = geometry_msgs.msg.Quaternion( q[0], q[1], q[2], q[3] ) position = geometry_msgs.msg.Point( t[0], t[1], t[2] ) pose = geometry_msgs.msg.Pose( position, orientation ) return pose
def publish_state(self, t): state_msg = TransformStamped() t_ned_imu = tft.translation_matrix(self.model.get_position()) R_ned_imu = tft.quaternion_matrix(self.model.get_orientation()) T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu) #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu)) if self.T_imu_vicon is None: # grab the static transform from imu to vicon frame from param server: frames = rospy.get_param("frames", None) ypr = frames['vicon_to_imu']['rotation'] q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw R_vicon_imu = tft.quaternion_matrix(q_vicon_imu) t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation']) # rospy.loginfo(str(R_vicon_imu)) # rospy.loginfo(str(t_vicon_imu)) self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu) self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu) self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx') rospy.loginfo(str(self.T_enu_ned)) rospy.loginfo(str(self.T_imu_vicon)) #rospy.loginfo(str(T_vicon_imu)) # we have /ned -> /imu, need to output /enu -> /vicon: T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon ) state_msg.header.stamp = t state_msg.header.frame_id = "/enu" state_msg.child_frame_id = "/simflyer1/flyer_vicon" stt = state_msg.transform.translation stt.x, stt.y, stt.z = T_enu_vicon[:3,3] stro = state_msg.transform.rotation stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon) self.pub_state.publish(state_msg)
def fromMatrix(m): """ :param m: 4x4 array :type m: :func:`numpy.array` :return: New PoseMath object Return a PoseMath object initialized from 4x4 matrix m .. doctest:: >>> import numpy >>> import tf_conversions.posemath as pm >>> print PoseMath.fromMatrix(numpy.identity(4)) position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 """ (x, y, z) = (m[0, 3], m[1, 3], m[2, 3]) q = transformations.quaternion_from_matrix(m) return PoseMath(Pose(Point(x, y, z), Quaternion(*q)))
def _makePreGraspPose(self, boxMat, axis): if axis==0: #x axis alpha = 0 gamma = 0 else: #y axis alpha = pi/2 gamma = -pi/2 ik_request = PositionIKRequest() ik_request.ik_link_name = self.toolLinkName with self._joint_state_lock: ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states) ik_request.pose_stamped = PoseStamped() ik_request.pose_stamped.header.stamp = rospy.Time.now() ik_request.pose_stamped.header.frame_id = self.frameID beta = pi/2 while beta < 3*pi/2: rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz') distance = self.preGraspDistance + self.gripperFingerLength preGraspMat = transformations.translation_matrix([0,0,-distance]) fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat) p = transformations.translation_from_matrix(fullMat) q = transformations.quaternion_from_matrix(fullMat) print "trying {} radians".format(beta) try: self._ik_server(ik_request, Constraints(), rospy.Duration(5.0)) except rospy.service.ServiceException: beta += 0.1 else: if ik_resp.error_code.val > 0: return beta else: #print ik_resp.error_code.val beta += 0.01 rospy.logerr('No way to pick this up. All IK solutions failed.') return 7 * pi / 6
def set_orientation(self, orientation): orientation = np.array(orientation) if orientation.shape == (4, 4): # We're getting a homogeneous rotation matrix - not a quaternion orientation = transformations.quaternion_from_matrix(orientation) return PoseEditor2(self.nav, [self.position, orientation])
def transformQuaternion(self, target_frame, ps): """ :param target_frame: the tf target frame, a string :param ps: the geometry_msgs.msg.QuaternionStamped message :return: new geometry_msgs.msg.QuaternionStamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns a new QuaternionStamped message. """ # mat44 is frame-to-frame transform as a 4x4 mat44 = self.asMatrix(target_frame, ps.header) # pose44 is the given quat as a 4x4 pose44 = xyzw_to_mat44(ps.quaternion) # txpose is the new pose in target_frame as a 4x4 txpose = numpy.dot(mat44, pose44) # quat is orientation of txpose quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value QuaternionStamped r = geometry_msgs.msg.QuaternionStamped() r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.quaternion = geometry_msgs.msg.Quaternion(*quat) return r
def transformPose(self, target_frame, ps): """ :param target_frame: the tf target frame, a string :param ps: the geometry_msgs.msg.PoseStamped message :return: new geometry_msgs.msg.PoseStamped message, in frame target_frame :raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message. """ # mat44 is frame-to-frame transform as a 4x4 mat44 = self.asMatrix(target_frame, ps.header) # pose44 is the given pose as a 4x4 pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation)) # txpose is the new pose in target_frame as a 4x4 txpose = numpy.dot(mat44, pose44) # xyz and quat are txpose's position and orientation xyz = tuple(transformations.translation_from_matrix(txpose))[:3] quat = tuple(transformations.quaternion_from_matrix(txpose)) # assemble return value PoseStamped r = geometry_msgs.msg.PoseStamped() r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat)) return r
def computeTransformation(world, slam, camera): # Here we do not care about the slamMworld transformation # timing as it is constant. tWorld = listener.getLatestCommonTime(world, camera) tMap = listener.getLatestCommonTime(slam, camera) t = min(tWorld, tMap) # Current pose given by the SLAM. (slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t) slamMcam = np.matrix( transformer.fromTranslationRotation(slamMcam_T, slamMcam_Q)) # Estimation of the camera position given by control. #FIXME: order is weird but it works. (worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t) worldMcam = np.matrix( transformer.fromTranslationRotation(worldMcam_T, worldMcam_Q)) (slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t) slamMworld = np.matrix( transformer.fromTranslationRotation(slamMworld_T, slamMworld_Q)) slamMcamEstimated = slamMworld * worldMcam # Inverse frames. camMslam = np.linalg.inv(slamMcam) camMslamEstimated = np.linalg.inv(slamMcamEstimated) # Split. camMslam_T = translation_from_matrix(camMslam) camMslam_Q = quaternion_from_matrix(camMslam) camMslam_E = euler_from_matrix(camMslam) camMslamEstimated_T = translation_from_matrix(camMslamEstimated) camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated) camMslamEstimated_E = euler_from_matrix(camMslamEstimated) # Compute correction. camMslamCorrected_T = [ camMslam_T[0], camMslamEstimated_T[1], camMslam_T[2] ] camMslamCorrected_E = [ camMslamEstimated_E[0], camMslam_E[1], camMslamEstimated_E[2] ] camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E) return (camMslamCorrected_T, camMslamCorrected_Q, t)
def find_controlled_tool_pose(self, target_pose): # find current tool location t_B_c = self.get_transform("torso_lift_link", self.tool_frame) # find error in position err_pos = target_pose[:3, 3] - t_B_c[:3, 3] # find error in angle err_ang = util.quaternion_dist(target_pose, t_B_c) # find control values u_x = self.x_pid.update_state(err_pos[0, 0]) u_y = self.y_pid.update_state(err_pos[1, 0]) u_z = self.z_pid.update_state(err_pos[2, 0]) + self.grav_comp u_pos = np.mat([u_x, u_y, u_z]).T u_a = self.a_pid.update_state(err_ang) # quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) - # np.array(tf_trans.quaternion_from_matrix(t_B_c))) # u_qx = self.qx_pid.update_state(quat_diff[0]) # u_qy = self.qy_pid.update_state(quat_diff[1]) # u_qz = self.qz_pid.update_state(quat_diff[2]) # u_qw = self.qw_pid.update_state(quat_diff[3]) # u_quat = np.array([u_qx, u_qy, u_qz, u_qw]) # u_quat = u_quat / np.linalg.norm(u_quat) # find commanded frame of tool # rotation u_a = np.clip(u_a, 0, 1) ei_q_slerp = tf_trans.quaternion_slerp( tf_trans.quaternion_from_matrix(t_B_c), tf_trans.quaternion_from_matrix(target_pose), u_a) new_quat = ei_q_slerp # new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c), # u_quat) t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat)) # position u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max) t_B_new[:3, 3] = t_B_c[:3, 3] + u_pos_clipped # publish informative topics self.goal_pub.publish( util.pose_mat_to_stamped_msg("torso_lift_link", target_pose)) self.cur_pub.publish( util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c)) self.cmd_pub.publish( util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new)) return t_B_new, err_pos, err_ang
def computeTransformation(world, slam, camera): # Here we do not care about the slamMworld transformation # timing as it is constant. tWorld = listener.getLatestCommonTime(world, camera) tMap = listener.getLatestCommonTime(slam, camera) t = min(tWorld, tMap) # Current pose given by the SLAM. (slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t) slamMcam = np.matrix(transformer.fromTranslationRotation(slamMcam_T, slamMcam_Q)) # Estimation of the camera position given by control. #FIXME: order is weird but it works. (worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t) worldMcam = np.matrix(transformer.fromTranslationRotation(worldMcam_T, worldMcam_Q)) (slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t) slamMworld = np.matrix(transformer.fromTranslationRotation(slamMworld_T, slamMworld_Q)) slamMcamEstimated = slamMworld * worldMcam # Inverse frames. camMslam = np.linalg.inv(slamMcam) camMslamEstimated = np.linalg.inv(slamMcamEstimated) # Split. camMslam_T = translation_from_matrix(camMslam) camMslam_Q = quaternion_from_matrix(camMslam) camMslam_E = euler_from_matrix(camMslam) camMslamEstimated_T = translation_from_matrix(camMslamEstimated) camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated) camMslamEstimated_E = euler_from_matrix(camMslamEstimated) # Compute correction. camMslamCorrected_T = [camMslam_T[0], camMslamEstimated_T[1], camMslam_T[2]] camMslamCorrected_E = [camMslamEstimated_E[0], camMslam_E[1], camMslamEstimated_E[2]] camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E) return (camMslamCorrected_T, camMslamCorrected_Q, t)
def _unpack_recv_data(self, recv_data): name = recv_data[0] q = quaternion_from_matrix(self._matrix_from_list(recv_data[1:])) return name, Pose(position=Vector3(x=float(recv_data[13])*self.mm2m, y=float(recv_data[14])*self.mm2m, z=float(recv_data[15])*self.mm2m), orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]))
def test_quaternion_from_matrix(self, q): # FIXME matrix = quaternion_matrix(q) q2 = quaternion_from_matrix(matrix) # q1 = speed_up_and_execute(spw.quaternion_from_matrix, [matrix]) q1_2 = np.array([x.evalf(real=True) for x in spw.quaternion_from_matrix(matrix)]).astype(float) # self.assertTrue(np.isclose(q1, q2).all() or np.isclose(q1, -q2).all(), msg='{} != {} | {}'.format(q, q1, q1_2)) self.assertTrue(np.isclose(q1_2, q2).all() or np.isclose(q1_2, -q2).all(), msg='{} != {}'.format(q, q1_2))
def quaternion(self): """The quaternion corresponding to this rotation""" if self._quaternion is None: self._quaternion = tft.quaternion_from_matrix(self.to_tf()) if self._quaternion[3] < 0: self._quaternion = -self._quaternion self._quaternion.flags.writeable = False return self._quaternion
def mat2pose(m): """ Converts numpy matrix (4x4) into pose np.array([[x],[y],[qw],[qx],[qy],[qz]]) """ q = np.expand_dims(quaternion_from_matrix(m), axis=1) p = np.expand_dims(m[:3, 3], axis=1) pose = np.vstack((p, q)) return pose
def transform_to_pose(matrix): pose = Pose() trans_vector = tft.translation_from_matrix(matrix) pose.position = Point(trans_vector[0], trans_vector[1], trans_vector[2]) quartern = tft.quaternion_from_matrix(matrix) pose.orientation = Quaternion(quartern[0], quartern[1], quartern[2], quartern[3]) return pose
def G2qt( G ): """ Get 3x3 matrix and 3x1 from 4x4 """ q = tr.quaternion_from_matrix( G ) t = G[0:3,3] return q,t
def matrixToQuaternion(matrix): """Concert 3x3 rotation matrix into a quaternion""" (R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix # Build 4x4 matrix M = [[R11, R21, R31, 0], [R12, R22, R32, 0], [R13, R23, R33, 0], [0, 0, 0, 1]] A = numpy.array(M) [w, x, y, z] = quaternion_from_matrix(A) return [w, x, y, z]
def setterPatternRotation(data, value, collection_key): assert len(value) == 3, "value must be a list with length 3." matrix = utilities.rodriguesToMatrix(value) hmatrix = np.identity(4).astype(np.float) hmatrix[0:3, 0:3] = matrix quat = transformations.quaternion_from_matrix(hmatrix) data['patterns']['collections'][collection_key]['quat'] = quat
def fwd_kin(self): self.robot = URDF.from_parameter_server() self.kin = KDLKinematics(self.robot, BASE, EE_FRAME) g = self.kin.forward(self.q) p = np.array(g[0:3,-1]).ravel() self.pose.pose.position = Point(*p.ravel()) self.pose.pose.orientation = Quaternion(*tr.quaternion_from_matrix(g)) return
def adjustRobotPose(self, pose): Tr2_w = self.poseToTransform(pose) Tadj = np.dot(np.dot(self.Tv_w, self.Tw_r1), Tr2_w) pose.position.x, pose.position.y, pose.position.z = Tadj[0:3, 3] q = tr.quaternion_from_matrix(Tadj) for i, attr in enumerate(['x', 'y', 'z', 'w']): pose.orientation.__setattr__(attr, q[i]) return pose
def kdl_to_quaternion(rotation_matrix): return Quaternion(*quaternion_from_matrix([[ rotation_matrix[0, 0], rotation_matrix[0, 1], rotation_matrix[0, 2], 0 ], [ rotation_matrix[1, 0], rotation_matrix[1, 1], rotation_matrix[1, 2], 0 ], [ rotation_matrix[2, 0], rotation_matrix[2, 1], rotation_matrix[2, 2], 0 ], [0, 0, 0, 1]]))
def adjustRobotPose(self, pose): Tr2_w = self.poseToTransform(pose) Tadj = np.dot(np.dot(self.Tv_w, self.Tw_r1),Tr2_w) pose.position.x, pose.position.y, pose.position.z = Tadj[0:3,3] q = tr.quaternion_from_matrix(Tadj) for i,attr in enumerate(['x', 'y', 'z', 'w']): pose.orientation.__setattr__(attr, q[i]) return pose
def flipOrbToNdt (orbPose): qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw] orbFlip = trafo.concatenate_matrices( trafo.quaternion_matrix(qOrb), trafo.rotation_matrix(np.pi/2, (1,0,0)), trafo.rotation_matrix(np.pi/2, (0,0,1)) ) return trafo.quaternion_from_matrix(orbFlip)
def flipOrbToNDT (orbPose): qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw] orbFlip = trafo.concatenate_matrices( trafo.quaternion_matrix(qOrb), trafo.rotation_matrix(np.pi/2, (1,0,0)), trafo.rotation_matrix(np.pi/2, (0,0,1)) ) return trafo.quaternion_from_matrix(orbFlip)
def transformTo(tf_xyzquat, pose): T_mat = tfm.concatenate_matrices(tfm.translation_matrix(tf_xyzquat[0:3]), tfm.quaternion_matrix(tf_xyzquat[3:7])) pose_mat = tfm.concatenate_matrices(tfm.translation_matrix(pose[0:3]), tfm.quaternion_matrix(pose[3:7])) new_pose_mat = np.dot(T_mat, pose_mat) return tfm.translation_from_matrix(new_pose_mat).tolist( ) + tfm.quaternion_from_matrix(new_pose_mat).tolist()
def plan_place(self, desired_pose): places = list() ps = PoseStamped() # ps.pose = self.current_grasping_target.object.primitive_poses[0] ps.header.frame_id = self.current_grasping_target.object.header.frame_id grasp_to_pick_matrix = self.computeGraspToPickMatrix() place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose) grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix position_vector = grasp2_to_place_matrix[0:-1, 3] quaternion = quaternion_from_matrix(grasp2_to_place_matrix) position_array = position_vector.getA1() l = PlaceLocation() direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector l.place_pose.header.frame_id = ps.header.frame_id l.place_pose.pose.position.x = position_array[0] l.place_pose.pose.position.y = position_array[1] l.place_pose.pose.position.z = position_array[2] l.place_pose.pose.orientation.x = quaternion[0] l.place_pose.pose.orientation.y = quaternion[1] l.place_pose.pose.orientation.z = quaternion[2] l.place_pose.pose.orientation.w = quaternion[3] # copy the posture, approach and retreat from the grasp used approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach) approach.desired_distance /= 2.0 l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in roll direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) print "DEBUG: Places: ", places[0] success, place_result = self.pickplace.place_with_retry( self.current_grasping_target.object.name, places, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return place_result.trajectory_stages else: rospy.loginfo("Plan rejected. Replanning...") else: rospy.logwarn("Planning failed. Replanning...") desired_pose[2] += 0.05 return self.plan_place(desired_pose)
def odometry_callback(self, msg): world_T_t265 = None odom_T_t265 = None #self.listener.waitForTransform(world_frame, t265_world_frame, rospy.Time(0), rospy.Duration(1)) #self.listener.waitForTransform(odom_frame, t265_odom_frame, rospy.Time(0), rospy.Duration(1)) # See the tf.transformations library documentation: http://docs.ros.org/en/jade/api/tf/html/python/transformations.html try: #(trans, rot) = self.listener.lookupTransform(self.world_frame, self.t265_world_frame, rospy.Time(0)) T = self.tfBuffer.lookup_transform(self.world_frame, self.t265_world_frame, rospy.Time()) trans = (T.transform.translation.x, T.transform.translation.y, T.transform.translation.z) rot = (T.transform.rotation.x, T.transform.rotation.y, T.transform.rotation.z, T.transform.rotation.w) world_T_t265 = tf.concatenate_matrices( tf.translation_matrix(trans), tf.quaternion_matrix(rot)) #t265_T_world = tf.inverse_matrix(world_T_t265) except (LookupException, ConnectivityException, ExtrapolationException): rospy.logwarn("OdomGlue: Could not lookup world to T265 transform") return try: #(trans, rot) = self.listener.lookupTransform(self.odom_frame, self.t265_odom_frame, rospy.Time(0)) T = self.tfBuffer.lookup_transform(self.odom_frame, self.t265_odom_frame, rospy.Time()) trans = (T.transform.translation.x, T.transform.translation.y, T.transform.translation.z) rot = (T.transform.rotation.x, T.transform.rotation.y, T.transform.rotation.z, T.transform.rotation.w) odom_T_t265 = tf.concatenate_matrices(tf.translation_matrix(trans), tf.quaternion_matrix(rot)) t265_T_odom = tf.inverse_matrix(odom_T_t265) except (LookupException, ConnectivityException, ExtrapolationException): rospy.logwarn("OdomGlue: Could not lookup odom to T265 transform") return world_T_odom = tf.concatenate_matrices(world_T_t265, t265_T_odom) translation = (0.0, 0.0, 0.0) rotation = (0.0, 0.0, 0.0, 1.0) try: self.broadcaster.sendTransform( tf.translation_from_matrix(world_T_odom), tf.quaternion_from_matrix(world_T_odom), rospy.Time.now(), self.odom_frame, self.world_frame) except: pass
def cb_initial(self,msg): trans = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z] rot = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] print ("Re-Initializing pose to: ", trans, rot) # Calculate BASE_LINK to MAP transform transform = t.concatenate_matrices(t.translation_matrix(trans), t.quaternion_matrix(rot)) inversed_transform = t.inverse_matrix(transform) inv_trans = t.translation_from_matrix(inversed_transform) inv_rot = t.quaternion_from_matrix(inversed_transform) base_to_map_pose = PoseStamped() base_to_map_pose.header.stamp = msg.header.stamp base_to_map_pose.header.frame_id = "base_link" base_to_map_pose.pose.position.x, base_to_map_pose.pose.position.y, base_to_map_pose.pose.position.z = inv_trans base_to_map_pose.pose.orientation.x, base_to_map_pose.pose.orientation.y, base_to_map_pose.pose.orientation.z, base_to_map_pose.pose.orientation.w = inv_rot self.listener.waitForTransform("odom", "base_link",msg.header.stamp, rospy.Duration(0.5)) map_odom = self.listener.transformPose('odom',base_to_map_pose) # Calculate MAP to ODOM transform trans_odom = [map_odom.pose.position.x, map_odom.pose.position.y, map_odom.pose.position.z] rot_odom = [map_odom.pose.orientation.x, map_odom.pose.orientation.y, map_odom.pose.orientation.z, map_odom.pose.orientation.w] transform_odom = t.concatenate_matrices(t.translation_matrix(trans_odom), t.quaternion_matrix(rot_odom)) inversed_transform_odom = t.inverse_matrix(transform_odom) inv_trans_odom = t.translation_from_matrix(inversed_transform_odom) inv_rot_odom = t.quaternion_from_matrix(inversed_transform_odom) map_to_odom = TransformStamped() map_to_odom.header.stamp = rospy.Time.now() map_to_odom.header.frame_id = 'map' map_to_odom.child_frame_id = 'odom' map_to_odom.transform.translation.x, map_to_odom.transform.translation.y, map_to_odom.transform.translation.z = inv_trans_odom map_to_odom.transform.rotation.x, map_to_odom.transform.rotation.y, map_to_odom.transform.rotation.z, map_to_odom.transform.rotation.w = inv_rot_odom br = tf2_ros.StaticTransformBroadcaster() br.sendTransform(map_to_odom)
def send_reset_to_rovio(self): rospy.wait_for_service('rovio/reset_to_pose') try: rovio_reset_srv = rospy.ServiceProxy('rovio/reset_to_pose', SrvResetToPose) # compute pose from local ENU (East-North-Up frame) to # IMU frame of the ViSensor (== C frame, according to MSF) T_Enu_C = tf.concatenate_matrices(self._T_Enu_I, self._T_I_C) q_Enu_C = tf.quaternion_from_matrix(T_Enu_C) # set new Sensor IMU position and orientation respect to World frame # (which is now aligned to local ENU) self._pose_world_imu_msg.position.x = T_Enu_C[0, 3] self._pose_world_imu_msg.position.y = T_Enu_C[1, 3] self._pose_world_imu_msg.position.z = T_Enu_C[2, 3] self._pose_world_imu_msg.orientation.w = q_Enu_C[3] self._pose_world_imu_msg.orientation.x = q_Enu_C[0] self._pose_world_imu_msg.orientation.y = q_Enu_C[1] self._pose_world_imu_msg.orientation.z = q_Enu_C[2] rovio_reset_srv(self._pose_world_imu_msg) if self._verbose: q_Enu_I = tf.quaternion_from_matrix(self._T_Enu_I) (yaw, pitch, roll) = tf.euler_from_quaternion(q_Enu_I, 'rzyx') rospy.loginfo(rospy.get_name() + ": body frame of MAV assumed with " + str(math.degrees(roll)) + " (deg) roll, " + str(math.degrees(pitch)) + " (deg) pitch, " + str(math.degrees(yaw)) + " (deg) yaw from local ENU (local axis, ZYX)") self.create_rovio_init_info() success = True message = "Service call to reset Rovio internal state sent" return (success, message) except rospy.ServiceException, e: success = False message = "Service call to reset Rovio internal state failed: %s" % e return (success, message)
def save_params(pos, rot, filename): rot_homo = np.eye(4) rot_homo[:3, :3] = rot rot_quat = tf_trans.quaternion_from_matrix(rot_homo) optitrak_params = {"position": pos.T.A[0].tolist(), "orientation": rot_quat.tolist()} print optitrak_params rosparam.upload_params("optitrak_calibration", optitrak_params) rospy.sleep(0.5) rosparam.dump_params(filename, "optitrak_calibration")
def get_ext(cameraid): ret = get_numbers_from_file(os.environ['CODE_BASE'] + '/catkin_ws/src/passive_vision/camerainfo/' + cameraid + '.pose.txt') print cameraid, ret trans = [ret[3], ret[7], ret[11]] ret[3] = 0 ret[7] = 0 ret[11] = 0 quat = tfm.quaternion_from_matrix(np.array(ret).reshape((4,4))) return trans + quat.tolist()
def interpolate_ep(self, ep_a, ep_b, t_vals): pos_a, rot_a = ep_a pos_b, rot_b = ep_b num_samps = len(t_vals) pos_waypoints = np.array(pos_a) + np.array( np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals) pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T] rot_homo_a, rot_homo_b = np.eye(4), np.eye(4) rot_homo_a[:3, :3] = rot_a rot_homo_b[:3, :3] = rot_b quat_a = tf_trans.quaternion_from_matrix(rot_homo_a) quat_b = tf_trans.quaternion_from_matrix(rot_homo_b) rot_waypoints = [] for t in t_vals: cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t) rot_waypoints.append( np.mat(tf_trans.quaternion_matrix(cur_quat))[:3, :3]) return zip(pos_waypoints, rot_waypoints)
def correct_orientation(device, orientation): if device.device_class in DEVICE_ORIENTATION_CORRECTION: start_orient_mtx = t.quaternion_matrix(orientation) correction = DEVICE_ORIENTATION_CORRECTION[device.device_class] result_mtx = t.concatenate_matrices(start_orient_mtx, correction) result_list = t.quaternion_from_matrix(result_mtx) return tuple(result_list) else: return orientation
def mat44_to_pose(mat): """ Convert 4*4 transformation matrix to pose msg (no header information added) :param mat: :return: """ trans = Point(*tuple(transformations.translation_from_matrix(mat))) rot = Quaternion(*tuple(transformations.quaternion_from_matrix(mat))) return Pose(trans, rot)
def fwd_kin_trajectory(self, joint_trajectory): endEffTrajectory = np.zeros((joint_trajectory.shape[0], 7)) for i in range(joint_trajectory.shape[0]): T, tmp = self.fwd_kin(joint_trajectory[i, :]) pos = T[0:3, 3] quat = tf_tran.quaternion_from_matrix(T) endEffTrajectory[i, :] = np.hstack((pos, quat)) return endEffTrajectory
def apply (self, ot): rotmat1 = self.toRotMat() rotmat2 = ot.toRotMat() q = trafo.quaternion_from_matrix(rotmat1.dot(rotmat2)) return Pose (self.timestamp, rotmat1[0][0:3].dot([ot.x, ot.y, ot.z]) + self.x, rotmat1[1][0:3].dot([ot.x, ot.y, ot.z]) + self.y, rotmat1[2][0:3].dot([ot.x, ot.y, ot.z]) + self.z, q[0], q[1], q[2], q[3])
def get_relative_pose(object_a, object_b): object_a_mat = get_object_pose_as_matrix(object_a) object_b_mat = get_object_pose_as_matrix(object_b) rel_mat = numpy.linalg.inv(object_a_mat).dot(object_b_mat) trans = transformations.translation_from_matrix(rel_mat) rot = transformations.quaternion_from_matrix(rel_mat) rot = [rot[3], rot[0], rot[1], rot[2]] print "pose: [%f, %f, %f, %f, %f, %f, %f]" % (trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3]) return (trans, rot)
def transform_to_pose(matrix): pose = Pose() print(matrix) quat4 = tft.quaternion_from_matrix(matrix) pose.orientation = Quaternion(quat4[0], quat4[1], quat4[2], quat4[3]) vector3 = np.dot(matrix, np.array([0, 0, 0, 1]))[:3] pose.position = Point(vector3[0], vector3[1], vector3[2]) print pose return pose
def transform_to_pose(matrix): pose = Pose() quat_from_mat = tft.quaternion_from_matrix(matrix) pose.orientation = Quaternion(quat_from_mat[0], quat_from_mat[1], quat_from_mat[2], quat_from_mat[3]) x = matrix[0][3] y = matrix[1][3] z = matrix[2][3] pose.position = Point(x, y, z) return pose
def align_assembly_frames(self, assemble_quat): rot = quaternion_matrix(assemble_quat) rot_x = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, -1.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) rot = np.dot(rot, rot_x) new_assemble_quat = quaternion_from_matrix(rot) return new_assemble_quat
def normal_to_quaternion(normal): # type: (np.ndarray) -> np.ndarray rotation_matrix = np.zeros((4, 4)) rotation_matrix[3, 3] = 1.0 rotation_matrix[:3, :3] = np.vstack(( normal, np.cross(np.array([0.0, 0.0, 1.0]), normal), np.array([0.0, 0.0, 1.0]), )).T return transformations.quaternion_from_matrix(rotation_matrix)
def _inverse_tuples(t): trans, rot = t euler = tr.euler_from_quaternion(rot) m = tr.compose_matrix(translate=trans, angles=euler) m_inv = tr.inverse_matrix(m) trans_inv = tr.translation_from_matrix(m_inv) rot_inv = tr.rotation_matrix(*tr.rotation_from_matrix(m_inv)) quat_inv = tr.quaternion_from_matrix(rot_inv) return (trans_inv, quat_inv)
def __init__(self, cell_id, markers_topic, world_frame, cell_frame, main_cell_frame, tf_listener): self.cell_id = cell_id self.markers_topic = markers_topic self.calibrated = False self.positions = [ np.array([0, 0, 0], dtype='f8'), np.array([0, 0, 0], dtype='f8'), np.array([0, 0, 0], dtype='f8'), np.array([0, 0, 0], dtype='f8') ] self.avg = 100 self.cnt = [0, 0, 0, 0] self.cell_frame = cell_frame self.world_frame = world_frame self.listener = tf_listener # type: tf.TransformListener() self.main_cell_frame = main_cell_frame self.transformation = Transform() m = rospy.get_param("~" + self.cell_id + "/calibration_matrix", None) self.markers_sub = rospy.Subscriber(self.markers_topic, AlvarMarkers, self.markers_cb, queue_size=1) self.marker_detection_enable_publisher = rospy.Publisher( "/art/" + self.cell_id + "/ar_track_alvar_bundle_calibration/enable_detection", Bool, queue_size=1, latch=True) self.x_offset = 0 self.y_offset = 0 self.z_offset = 0 self.x_rotate_offset = 0 self.y_rotate_offset = 0 self.z_rotate_offset = 0 if m is not None: m = np.matrix(ast.literal_eval(m)) self.transformation.rotation = ArtCalibrationHelper.normalize_vector( transformations.quaternion_from_matrix(m)) self.transformation.translation = transformations.translation_from_matrix( m) self.calibrated = True rospy.loginfo("Cell: " + str(self.cell_id) + " calibration loaded from param") self.stop_marker_detection() else: self.start_marker_detection() rospy.loginfo("Cell: " + str(self.cell_id) + " ready")
def get_asm_pose_in_gazebo(self, obj_name, asm_pose, z_offset): obj_pose = self.client_getModel(obj_name, "table").pose obj_tf_mat = self.get_tf_mat_from_pose(obj_pose) asm_tf_mat = self.get_tf_mat_from_pose(asm_pose) target_tf_mat = tf.concatenate_matrices(obj_tf_mat, asm_tf_mat) target_tr = list(tf.translation_from_matrix(target_tf_mat)) target_tr[2] += z_offset target_quat = tf.quaternion_from_matrix(target_tf_mat) return self.get_pose_by_tr_quat(target_tr, target_quat)
def pose_callback(self,msg): #print 'got msg' frame_id = msg.header pose = msg.pose p = tft.translation_matrix([pose.position.x,pose.position.y,pose.position.z]) rot = tft.quaternion_matrix([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) Tcam_to_cb = dot(p,rot) #print 'Tcam_to_cb',Tcam_to_cb #Tworld_to_cb = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),tft.euler_matrix(0,0,pi/2)) Tworld_to_cb = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),array(tb_to_mat(self.yaw, self.pitch, self.roll))) #print 'Tworld_to_cb',Tworld_to_cb if self.invert_tf: Tworld_to_cb = tft.inverse_matrix(Tworld_to_cb) Tworld_to_cam = dot(Tworld_to_cb,tft.inverse_matrix(Tcam_to_cb)) #print 'Tworld_to_cam',Tworld_to_cam Tworld_to_cam_p = Tworld_to_cam[0:3,3] Tworld_to_cam_q = tft.quaternion_from_matrix(Tworld_to_cam) pub_msg = PoseStamped() pub_msg.header.stamp = msg.header.stamp pub_msg.header.frame_id = '/world' pub_msg.pose.position = Point(*(Tworld_to_cam_p.tolist())) pub_msg.pose.orientation = Quaternion(*(Tworld_to_cam_q.tolist())) #print pub_msg self.pub.publish(pub_msg) pub_cb_msg = PoseStamped() pub_cb_msg.header.stamp = msg.header.stamp pub_cb_msg.header.frame_id = '/world' pub_cb_msg.pose.position = Point(*(Tworld_to_cb[0:3,3].tolist())) pub_cb_msg.pose.orientation = Quaternion(*(tft.quaternion_from_matrix(Tworld_to_cb).tolist())) self.pub_cb.publish(pub_cb_msg) self.transform = Tworld_to_cam self.transform_frame = msg.header.frame_id self.br.sendTransform(Tworld_to_cam_p,Tworld_to_cam_q,msg.header.stamp,self.transform_frame,'/world')
def find_controlled_tool_pose(self, target_pose): # find current tool location t_B_c = self.get_transform("torso_lift_link", self.tool_frame) # find error in position err_pos = target_pose[:3,3] - t_B_c[:3,3] # find error in angle err_ang = util.quaternion_dist(target_pose, t_B_c) # find control values u_x = self.x_pid.update_state(err_pos[0,0]) u_y = self.y_pid.update_state(err_pos[1,0]) u_z = self.z_pid.update_state(err_pos[2,0]) + self.grav_comp u_pos = np.mat([u_x, u_y, u_z]).T u_a = self.a_pid.update_state(err_ang) # quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) - # np.array(tf_trans.quaternion_from_matrix(t_B_c))) # u_qx = self.qx_pid.update_state(quat_diff[0]) # u_qy = self.qy_pid.update_state(quat_diff[1]) # u_qz = self.qz_pid.update_state(quat_diff[2]) # u_qw = self.qw_pid.update_state(quat_diff[3]) # u_quat = np.array([u_qx, u_qy, u_qz, u_qw]) # u_quat = u_quat / np.linalg.norm(u_quat) # find commanded frame of tool # rotation u_a = np.clip(u_a, 0, 1) ei_q_slerp = tf_trans.quaternion_slerp(tf_trans.quaternion_from_matrix(t_B_c), tf_trans.quaternion_from_matrix(target_pose), u_a) new_quat = ei_q_slerp # new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c), # u_quat) t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat)) # position u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max) t_B_new[:3,3] = t_B_c[:3,3] + u_pos_clipped # publish informative topics self.goal_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", target_pose)) self.cur_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c)) self.cmd_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new)) return t_B_new, err_pos, err_ang
def timercb(self, data): if not self.running_flag: return if self.listener.frameExists(SIMFRAME) and self.listener.frameExists(CONTFRAME): t = self.listener.getLatestCommonTime(SIMFRAME, CONTFRAME) try: position, quaternion = self.listener.lookupTransform(SIMFRAME, CONTFRAME, t) except (tf.Exception): rospy.logerr("Could not transform from "\ "{0:s} to {1:s}".format(SIMFRAME,CONTFRAME)) return else: rospy.logerr("Could not find required frames "\ "for transformation from {0:s} to {1:s}".format(SIMFRAME,CONTFRAME)) return # now we can use this position to integrate the trep simulation: ucont = np.zeros(NU) ucont[self.system.kin_configs.index(self.system.get_config('xs'))] = position[0] ucont[self.system.kin_configs.index(self.system.get_config('ys'))] = position[1] ucont[self.system.kin_configs.index(self.system.get_config('zs'))] = position[2] # step integrator: try: self.mvi.step(self.mvi.t2 + DT, k2=ucont) except trep.ConvergenceError as e: rospy.loginfo("Could not take step: %s"%e.message) return # if we successfully integrated, let's publish the point and the tf p = PointStamped() p.header.stamp = rospy.Time.now() p.header.frame_id = SIMFRAME # get transform from trep world to mass frame: gwm = self.system.get_frame(MASSFRAME).g() ptrans = gwm[0:3, -1] # print ptrans p.point.x = ptrans[0] p.point.y = ptrans[1] p.point.z = ptrans[2] self.mass_pub.publish(p) # now we can send the transform: qtrans = TR.quaternion_from_matrix(gwm) self.br.sendTransform(ptrans, qtrans, p.header.stamp, MASSFRAME, SIMFRAME) # now we can publish the markers: for m in self.markers.markers: m.header.stamp = p.header.stamp self.mass_marker.pose = GM.Pose(position=GM.Point(*ptrans)) p1 = GM.Point(*ptrans) p2 = GM.Point(*ucont) self.link_marker.points = [p1, p2] self.marker_pub.publish(self.markers) # now we can render the forces: self.render_forces() return