Exemple #1
0
def dalk_path_generation(path_filename, calib_filename):
    data = np.loadtxt(path_filename)
    oct2robot = np.loadtxt(calib_filename)[0, :].reshape((4, 4))
    robot2oct = np.linalg.inv(oct2robot)
    robot_target = []
    xyzrpy = []
    # dalk_frame = (so3.from_rpy([0, 0, -np.pi/2]), [52.5 * 0.0254, 6.5 * 0.0251, 0.75 * 0.0254])
    dalk_frame = (so3.from_rpy([0, np.pi, 0]), [0.0, 0.0, 0.0])
    for row in data[::1]:
        x_target = row[12 + 6:12 + 6 + 6]
        # xyzrpy.append(x_target)
        T = np.eye(4)
        T = rotation_matrix(x_target[3], [1, 0, 0]).dot(
            rotation_matrix(x_target[4], [0, 1, 0])).dot(
                rotation_matrix(x_target[5], [0, 0, 1]))
        T[:3, 3] = x_target[0:3]
        T = robot2oct.dot(T)
        T[:3, 3] = T[:3, 3] * 1
        T = oct2robot.dot(T)
        robot_target.append(T)
        T2rpyxyz = list(se3.from_homogeneous(T)[1]) + list(
            so3.rpy(se3.from_homogeneous(T)[0]))
        print(T2rpyxyz)
        xyzrpy.append(T2rpyxyz)

        # T_m = se3.mul(se3.inv(dalk_frame), se3.from_homogeneous(T))
        # T_h = se3.homogeneous(T_m)
        # T2rpyxyz = list(T_m[1]) + list(so3.rpy(T_m[0]))
        # xyzrpy.append(T2rpyxyz)
        # robot_target.append(T_h)
    return robot_target, xyzrpy
Exemple #2
0
    def createAnimationTrack(self, jointsOrder=None, name="BVHMotion"):
        """
        Create an animation track from the motion stored in this BHV file.
        """
        if jointsOrder == None:
            jointsData = [
                joint.matrixPoses for joint in self.getJoints()
                if not joint.isEndConnector()
            ]
            # We leave out end effectors as they should not have animation data
        else:
            nFrames = self.frameCount
            import re
            # Remove the tail from duplicate bone names
            for idx, jName in enumerate(jointsOrder):
                # Joint mappings can contain a rotation compensation
                if isinstance(jName, tuple):
                    jName, _ = jName
                if not jName:
                    continue
                r = re.search("(.*)_\d+$", jName)
                if r:
                    jointsOrder[idx] = r.group(1)

            jointsData = []
            for jointName in jointsOrder:
                if isinstance(jointName, tuple):
                    jointName, angle = jointName
                else:
                    angle = 0.0
                if jointName:
                    poseMats = self.getJointByCanonicalName(
                        jointName).matrixPoses.copy()
                    if isinstance(angle, float):
                        if angle != 0.0:
                            # Rotate around global Z axis
                            rot = tm.rotation_matrix(-angle * D, [0, 0, 1])
                            # Roll around global Y axis (this is a limitation)
                            roll = tm.rotation_matrix(angle * D, [0, 1, 0])
                            for i in xrange(nFrames):
                                # TODO make into numpy loop
                                poseMats[i] = np.dot(poseMats[i], rot)
                                poseMats[i] = np.dot(poseMats[i], roll)
                    else:  # Compensation (angle) is a transformation matrix
                        # Compensate animation frames
                        for i in xrange(nFrames):
                            # TODO make into numpy loop
                            poseMats[i] = np.mat(poseMats[i]) * np.mat(angle)
                            #poseMats[i] = np.mat(angle) # Test compensated rest pose
                    jointsData.append(poseMats)
                else:
                    jointsData.append(animation.emptyTrack(nFrames))

        nJoints = len(jointsData)
        nFrames = len(jointsData[0])

        # Interweave joints animation data, per frame with joints in breadth-first order
        animData = np.hstack(jointsData).reshape(nJoints * nFrames, 4, 4)
        framerate = 1.0 / self.frameTime
        return animation.AnimationTrack(name, animData, nFrames, framerate)
Exemple #3
0
	def rule_branch(self):
		'''
		@brief Create a branch
		'''	
		# Translate
		frame = self.current_node.frame	
		frame = np.dot(frame, tr.translation_matrix([0, 0, self.current_node.size]))
		
		# Add noise
		if self.noise != 0:
			self.current_phi_angle += random.uniform(-self.noise, self.noise)
			self.current_rho_angle += random.uniform(-self.noise, self.noise)
		
		# Phi Rotation
		phi_matrix = tr.rotation_matrix(self.current_phi_angle, [0, 0, 1])
		frame[:3,:3] = np.dot(frame[:3,:3], phi_matrix[:3,:3])
		
		# Rho rotation
		rho_matrix = tr.rotation_matrix(self.current_rho_angle, [1, 0, 0])	
		frame[:3,:3] = np.dot(frame[:3,:3], rho_matrix[:3,:3])
				
		# Create a new node
		node = Node(frame, self.current_node.size * self.scale_factor, self.current_node.diameter * self.current_subdivision_ratio * self.scale_factor)	
		
		self.current_node.add_node(node)
		self.current_node = node
		self.current_rho_angle = 0
		self.current_phi_angle = 0
		self.current_subdivision_ratio = 1
Exemple #4
0
def getMatrix(head, tail, roll):
    """
    Calculate an orientation (rest) matrix for a bone between specified head
    and tail positions with given bone roll angle.
    Returns length of the bone and rest orientation matrix in global coordinates.
    """
    vector = toZisUp3(tail - head)
    length = math.sqrt(np.dot(vector, vector))
    if length == 0:
        vector = [0,0,1]
    else:
        vector = vector/length
    yproj = np.dot(vector, YUnit)

    if yproj > 1-1e-6:
        axis = YUnit
        angle = 0
    elif yproj < -1+1e-6:
        axis = YUnit
        angle = pi
    else:
        axis = np.cross(YUnit, vector)
        axis = axis / math.sqrt(np.dot(axis,axis))
        angle = math.acos(yproj)
    mat = tm.rotation_matrix(angle, axis)
    if roll:
        mat = np.dot(mat, tm.rotation_matrix(roll, YUnit))
    mat = fromZisUp4(mat)
    mat[:3,3] = head
    return length, mat
Exemple #5
0
        def setup_particle_orientation(invert, theta, phi):
            # spin about z-axis vector to create phi spin
            phi_mat = transformations.rotation_matrix(phi + (pi if invert else 0), vz)
            # rotate z-x axis to create orientation vector
            axis_rotation = transformations.rotation_matrix((-1 if invert else 1) * -(theta - pi/2), vy)

            return transformations.concatenate_matrices(axis_rotation, phi_mat)
Exemple #6
0
    def set_block(self, s, d):
        aoy = m.atan2(s[2], s[0])
        aoz = m.atan2(s[1], m.sqrt(s[0]**2 + s[2]**2))

        rot = tr.rotation_matrix(aoy, (0, 1, 0))
        rot = np.dot(tr.rotation_matrix(-aoz, (0, 0, 1)), rot)
        rot = np.dot(tr.rotation_matrix(m.pi / 2.0, (0, 0, 1)), rot)

        v, n, t = self.gen_v(1, 1, s)
        for x in range(v.shape[0]):
            for y in range(v.shape[1]):
                for z in range(v.shape[2]):
                    v[x, y, z] = np.dot(rot, v[x, y, z])
                    n[x, y,
                      z] = np.resize(np.dot(rot, np.resize(n[x, y, z], 4)), 3)
        Mesh.__init__(self, buffers=(v, n, t))

        self.x = np.array(
            ((0, 0, 0, 1), (s[0], 0, 0, 1), (0, 0, s[2], 1),
             (s[0], 0, s[2], 1), (0, s[1], 0, 1), (s[0], s[1], 0, 1),
             (0, s[1], s[2], 1), (s[0], s[1], s[2], 1)), np.float64)
        for i in range(len(self.x)):
            self.x[i] = np.dot(rot, self.x[i])
        self.r = np.resize(
            np.dot(rot,
                   np.array((s[0], s[1], s[2], 0), np.float64) / 2.0), 3)
        self.m = np.array([d * s[0] * s[1] * s[2] / 8.0] * len(self.x),
                          np.float64)
        self.M = self.calc_m(self.x, self.m)
        self.Mi = np.linalg.inv(self.M)
Exemple #7
0
def _getMatrix(head, tail, roll):
    """
    Calculate an orientation (rest) matrix for a bone between specified head
    and tail positions with given bone roll angle.
    Returns length of the bone and rest orientation matrix in global coordinates.
    """
    # TODO validate, or replace

    vector = toZisUp3(tail - head)
    length = math.sqrt(np.dot(vector, vector))
    if length == 0:
        vector = [0,0,1]
    else:
        vector = vector/length
    yproj = np.dot(vector, YUnit)

    if yproj > 1-1e-6:
        axis = YUnit
        angle = 0
    elif yproj < -1+1e-6:
        axis = YUnit
        angle = pi
    else:
        axis = np.cross(YUnit, vector)
        axis = axis / math.sqrt(np.dot(axis,axis))
        angle = math.acos(yproj)
    mat = tm.rotation_matrix(angle, axis)
    if roll:
        mat = np.dot(mat, tm.rotation_matrix(roll, YUnit))
    mat = fromZisUp4(mat)
    mat[:3,3] = head
    return length, mat
def make_spin_pattern_sagimeridian(sagimeridian_pole, angular_velocity,
                                   frame_rate):
    #spin the world around a pole on the mid-sagital meridian.
    from scipy.misc import imresize
    unit_vector = np.array([1., 0., 0.])
    zp_offset = (2 * pi) / 96 * 4
    orientation_vector = dot(
        tran.rotation_matrix(sagimeridian_pole, array([0, 1, 0]))[:3, :3],
        unit_vector)
    orientation_vector = dot(
        tran.rotation_matrix(zp_offset, array([0, 0, 1]))[:3, :3],
        orientation_vector) * angular_velocity
    xyzp = integrate_frame(xyz,
                           rotation_vect=orientation_vector,
                           translation_vect=np.array([0, 0, 0]),
                           frame_period=1 / frame_rate)
    proj = arena_project(xyzp)
    imgs = digitize_points(*proj, display_shape=display_shape)
    imgs = imgs[:, :, newaxis]
    frames_per_cycle = int(((2 * pi) / angular_velocity) * frame_rate)
    for i in range(frames_per_cycle):
        xyzp = integrate_frame(xyzp,
                               rotation_vect=orientation_vector,
                               translation_vect=np.array([0, 0, 0]),
                               frame_period=1 / frame_rate)
        proj = arena_project(xyzp)
        img = digitize_points(*proj, display_shape=display_shape)
        imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
    from scipy import io
    io.savemat('sagimeridian_%03d' % (around(rad2deg(sagimeridian_pole))),
               {'imgs': imgs})
def make_spin_pattern_coromeridian(coromeridian_pole,
                                   angular_velocity,
                                   frame_rate,
                                   max_sensory_radius=2,
                                   star_density=50):
    #Spin the world around a pole around a coronal maridian pole
    from scipy.misc import imresize
    unit_vector = np.array([0., 1., 0.])
    zp_offset = (2 * pi) / 96 * 4
    orientation_vector = dot(
        tran.rotation_matrix(coromeridian_pole, array([1, 0, 0]))[:3, :3],
        unit_vector)
    orientation_vector = dot(
        tran.rotation_matrix(zp_offset, array([0, 0, 1]))[:3, :3],
        orientation_vector) * angular_velocity
    xyz = make_universe(star_density=star_density,
                        max_sensory_radius=max_sensory_radius)
    xyzp = integrate_frame(xyz,
                           rotation_vect=orientation_vector,
                           translation_vect=np.array([0, 0, 0]),
                           frame_period=1 / frame_rate)
    proj = arena_project(xyzp)
    imgs = digitize_points(*proj, display_shape=display_shape)
    imgs = imgs[:, :, newaxis]
    frames_per_cycle = int(((2 * pi) / angular_velocity) * frame_rate)
    for i in range(frames_per_cycle):
        xyzp = integrate_frame(xyzp,
                               rotation_vect=orientation_vector,
                               translation_vect=np.array([0, 0, 0]),
                               frame_period=1 / frame_rate)
        proj = arena_project(xyzp)
        img = digitize_points(*proj, display_shape=display_shape)
        imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
    from scipy import io
    return imgs
Exemple #10
0
	def inverse_kinematics( self , pos ,  normal ) :
		l1 , l2 , l3 = .91 , .81 , .33
		dy , dz  = .27 , .26 

		normal = normal / la.norm( normal )

		pos1 = pos + normal * l3

		e = m.sqrt(pos1[2]*pos1[2]+pos1[0]*pos1[0]-dz*dz)

		a1 = m.atan2(pos1[2], -pos1[0]) + m.atan2(dz, e)

		pos2 = np.array( [ e , pos1[1]-dy , .0 ] );
		a3 = -m.acos(min(1.0,(pos2[0]*pos2[0]+pos2[1]*pos2[1]-l1*l1-l2*l2)/(2.0*l1*l2)))
		k = l1 + l2 * m.cos(a3)
		l = l2 * m.sin(a3)
		a2 = -m.atan2(pos2[1],m.sqrt(pos2[0]*pos2[0]+pos2[2]*pos2[2])) - m.atan2(l,k);

		rotmat = tr.rotation_matrix( -a1 , (0,1,0) ) 
		normal1 = np.resize( np.dot( rotmat , np.resize( normal , 4 ) ) , 3 )

		rotmat = tr.rotation_matrix( -a2-a3 , (0,0,1) ) 
		normal1 = np.resize( np.dot( rotmat , np.resize( normal1, 4 ) ) , 3 )

		a5 = m.acos( normal1[0] )
		a4 = m.atan2(normal1[2], normal1[1])

		return a1 , a2 , a3 , a4 , a5
Exemple #11
0
def check_points_in_range(current_rob_pos, other_rob_pos):
    # calculate bottom right corner of the bounding rectangle
    rx, ry, rth = current_rob_pos
    rth = (rth / 100) + (pi / 2)
    zx = round(rx - (m * sin(rth)), 3)
    zy = round(ry + (m * cos(rth)), 3)

    # calculating the transformation matrix of the new frame at that vertex
    alpha, beta, gamma = 0, 0, (-(pi / 2 - rth))
    origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]
    Rx = transformations.rotation_matrix(alpha, xaxis)
    Ry = transformations.rotation_matrix(beta, yaxis)
    Rz = transformations.rotation_matrix(gamma, zaxis)
    T = transformations.translation_matrix([zx, zy, 0])
    R = transformations.concatenate_matrices(Rx, Ry, Rz)
    Z = transformations.shear_matrix(beta, xaxis, origin, zaxis)
    S = transformations.scale_matrix(1, origin)
    M = transformations.concatenate_matrices(T, R, Z, S)
    M = np.linalg.inv(M)

    # calculating the new point in that frame
    other_x = other_rob_pos[0]
    other_y = other_rob_pos[1]
    other_z = 0
    other_h = 1
    other_point_homogenous = np.array([other_x, other_y, other_z, other_h])
    new_point = np.dot(M, other_point_homogenous)
    new_point = np.round(new_point, 3)

    # checking that the point lies within the boundaries
    px, py, pz, ph = new_point
    if px <= (2 * m) and px >= 0 and py <= m and py >= 0:
        return 1
    else:
        return 0
Exemple #12
0
def gen_meas_3d(func, N, scale=1):
    """ Generate a set of measurements according to a height function.
    """
    # Use 3-D dirichlet distribution to sample 3-simplex then project to 2-d
    # Gives a nicely uniform spread of points
    m_a, m_b = np.random.dirichlet((1, 1, 1), N)[:, :2].T
    m_g = func(m_a, m_b)
    m_z = np.array([m_a, m_b, m_g]).T.reshape(N, 3, 1)

    std_a = 0.01 * scale
    std_b = 0.01 * scale
    std_g = 0.02 * scale
    std_z = np.diag([std_a, std_b, std_g])

    alpha_rot = np.pi * np.random.random_sample(N) - np.pi / 2
    beta_rot = np.pi * np.random.random_sample(N) - np.pi / 2
    gamma_rot = np.pi * np.random.random_sample(N) - np.pi / 2
    xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
    C_z = np.empty((N, 3, 3), dtype=float)

    for i in trange(N):
        # Rotate Covariance Matrix
        Rx = rotation_matrix(alpha_rot[i], xaxis)
        Ry = rotation_matrix(beta_rot[i], yaxis)
        Rz = rotation_matrix(gamma_rot[i], zaxis)
        R = concatenate_matrices(Rx, Ry, Rz)[:-1, :-1]

        C = R.dot(std_z * std_z).dot(R.T)
        e = np.random.multivariate_normal(np.zeros(3), C, 1).T

        m_z[i, :, :] += e
        C_z[i] = C

    return m_z, C_z
    def __predict_nominal_state(self, imu_measurement: np.array):
        p = self.nominal_state[:3].reshape(-1, 1)
        q = self.nominal_state[3:7]
        v = self.nominal_state[7:10].reshape(-1, 1)
        a_b = self.nominal_state[10:13].reshape(-1, 1)
        w_b = self.nominal_state[13:16]
        g = self.nominal_state[16:19].reshape(-1, 1)

        w_m = imu_measurement[1:4].copy()
        a_m = imu_measurement[4:7].reshape(-1, 1).copy()
        dt = imu_measurement[0] - self.last_predict_time
        """
        dp/dt = v
        dv/dt = R(a_m - a_b) + g
        dq/dt = 0.5 * q x(quaternion product) (w_m - w_b)
        
        a_m and w_m are the measurements of IMU.
        a_b and w_b are biases of acc and gyro, respectively.
        R = R{q}, which bring the point from local coordinate to global coordinate.
        """
        w_m -= w_b
        a_m -= a_b

        # use the zero-order integration to integrate the quaternion.
        # q_{n+1} = q_n x q{(w_m - w_b) * dt}
        angle = la.norm(w_m)
        axis = w_m / angle
        R_w = tr.rotation_matrix(0.5 * dt * angle, axis)
        q_w = tr.quaternion_from_matrix(R_w, True)
        q_half_next = tr.quaternion_multiply(q, q_w)

        R_w = tr.rotation_matrix(dt * angle, axis)
        q_w = tr.quaternion_from_matrix(R_w, True)
        q_next = tr.quaternion_multiply(q, q_w)
        if q_next[0] < 0:  # force the quaternion has a positive real part.
            q_next *= -1

        # use RK4 method to integration velocity and position.
        # integrate velocity first.
        R = tr.quaternion_matrix(q)[:3, :3]
        R_half_next = tr.quaternion_matrix(q_half_next)[:3, :3]
        R_next = tr.quaternion_matrix(q_next)[:3, :3]
        v_k1 = R @ a_m + g
        v_k2 = R_half_next @ a_m + g
        # v_k3 = R_half_next @ a_m + g  # yes. v_k2 = v_k3.
        v_k3 = v_k2
        v_k4 = R_next @ a_m + g
        v_next = v + dt * (v_k1 + 2 * v_k2 + 2 * v_k3 + v_k4) / 6

        # integrate position
        p_k1 = v
        p_k2 = v + 0.5 * dt * v_k1  # k2 = v_next_half = v + 0.5 * dt * v' = v + 0.5 * dt * v_k1(evaluate at t0)
        p_k3 = v + 0.5 * dt * v_k2  # v_k2 is evaluated at t0 + 0.5*delta
        p_k4 = v + dt * v_k3
        p_next = p + dt * (p_k1 + 2 * p_k2 + 2 * p_k3 + p_k4) / 6

        self.nominal_state[:3] = p_next.reshape(3, )
        self.nominal_state[3:7] = q_next
        self.nominal_state[7:10] = v_next.reshape(3, )
def compute_transform(rx, rz, tx, ty, tz):
	origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]

	Rx = tf.rotation_matrix(rx, xaxis)
	Rz = tf.rotation_matrix(rz, zaxis)
	T = tf.translation_matrix([tx, ty, tz])
	
	return T.dot(Rz).dot(Rx)	
Exemple #15
0
    def createAnimationTrack(self, jointsOrder = None, name="BVHMotion"):
        """
        Create an animation track from the motion stored in this BHV file.
        """
        if jointsOrder == None:
            jointsData = [joint.matrixPoses for joint in self.getJoints() if not joint.isEndConnector()]
            # We leave out end effectors as they should not have animation data
        else:
            nFrames = self.frameCount
            import re
            # Remove the tail from duplicate bone names
            for idx,jName in enumerate(jointsOrder):
                # Joint mappings can contain a rotation compensation
                if isinstance(jName, tuple):
                    jName, _ = jName
                if not jName:
                    continue
                r = re.search("(.*)_\d+$",jName)
                if r:
                    jointsOrder[idx] = r.group(1)

            jointsData = []
            for jointName in jointsOrder:
                if isinstance(jointName, tuple):
                    jointName, angle = jointName
                else:
                    angle = 0.0
                if jointName:
                    poseMats = self.getJointByCanonicalName(jointName).matrixPoses.copy()
                    if isinstance(angle, float):
                        if angle != 0.0:
                            # Rotate around global Z axis
                            rot = tm.rotation_matrix(-angle*D, [0,0,1])
                            # Roll around global Y axis (this is a limitation)
                            roll = tm.rotation_matrix(angle*D, [0,1,0])
                            for i in range(nFrames):
                                # TODO make into numpy loop
                                poseMats[i] = np.dot(poseMats[i], rot)
                                poseMats[i] = np.dot(poseMats[i], roll)
                    else:   # Compensation (angle) is a transformation matrix
                        # Compensate animation frames
                        for i in range(nFrames):
                            # TODO make into numpy loop
                            poseMats[i] = np.mat(poseMats[i]) * np.mat(angle)
                            #poseMats[i] = np.mat(angle) # Test compensated rest pose
                    jointsData.append(poseMats)
                else:
                    jointsData.append(animation.emptyTrack(nFrames))

        nJoints = len(jointsData)
        nFrames = len(jointsData[0])

        # Interweave joints animation data, per frame with joints in breadth-first order
        animData = np.hstack(jointsData).reshape(nJoints*nFrames,4,4)
        framerate = 1.0/self.frameTime
        return animation.AnimationTrack(name, animData, nFrames, framerate)
Exemple #16
0
    def transform(self, pitch, yaw):
        """Gives the translation matrix associated with this arm with the given pitch
        and yaw.
        
        """
        tLength = tr.translation_matrix(np.array([self.length, 0, 0]))
        rPitch = tr.rotation_matrix(pitch, yaxis)
        rYaw = tr.rotation_matrix(yaw, zaxis)

        #return armLength * rPitch * rYaw
        return tr.concatenate_matrices(rYaw, rPitch, tLength)
Exemple #17
0
    def error(self, target_pos, baseTransform, parameters):
        armLength = tr.translation_matrix([self.length, 0, 0])
        rPitch = tr.rotation_matrix(parameters[0], yaxis)
        rYaw = tr.rotation_matrix(parameters[1], zaxis)
        transform = tr.concatenate_matrices(baseTransform, rYaw, rPitch,
                                            armLength)

        if self.after is None:
            end = tr.translation_from_matrix(transform)
            return distance(target_pos, end)
        else:
            return self.after.error(target_pos, transform, parameters[2:])
def rotation_matrix_angles(alpha, beta, gamma):
    """ Returns the rotation matrix that applies a rotation by the given Euler angles.

        It applies Rz(gamma)*Rx(beta)*Rz(alpha).
    """
    R1 = trans.rotation_matrix(gamma, z)
    R2 = trans.rotation_matrix(beta, x)
    R3 = trans.rotation_matrix(alpha, z)

    M = trans.concatenate_matrices(R1, R2, R3)

    return M
    def update_sagital_rot(self, robot):

        # ok we assume a symetric configuration with both foot on the ground

        self.LAnkleRotY = tf.rotation_matrix(
            radians(robot.l_ankle_y.present_position), self.yaxis)
        self.RAnkleRotY = tf.rotation_matrix(
            radians(robot.r_ankle_y.present_position), self.yaxis)

        self.LKneeRotY = tf.rotation_matrix(
            radians(- robot.l_knee_y.present_position), self.yaxis)
        self.RKneeRotY = tf.rotation_matrix(
            radians(- robot.r_knee_y.present_position), self.yaxis)

        self.LHipRotY = tf.rotation_matrix(
            radians(- robot.l_hip_y.present_position), self.yaxis)
        self.RHipRotY = tf.rotation_matrix(
            radians(- robot.r_hip_y.present_position), self.yaxis)

        self.AbsRotY = tf.rotation_matrix(
            radians(robot.abs_y.present_position), self.yaxis)
        self.BustRotY = tf.rotation_matrix(
            radians(robot.bust_y.present_position), self.yaxis)

        self.roty = array([tf.rotation_matrix(0, self.yaxis), (self.LAnkleRotY + self.RAnkleRotY) / 2.0,
                           (self.LKneeRotY + self.RKneeRotY) / 2.0, (self.LHipRotY + self.RHipRotY) / 2.0, self.AbsRotY, self.BustRotY])

        # self.transformy =array([tf.concatenate_matrices(self.roty[i], self.SegCom[i]) for i in range(len(self.roty))])
        # self.transformy =array([self.roty[i].dot( self.SegCom[i]) for i in range(len(self.roty))])

        self.transformy = array([identity(4),
                                 self.FootT,
                                 tf.concatenate_matrices(
            (self.LAnkleRotY + self.RAnkleRotY) / 2.0, self.ThighT),
            tf.concatenate_matrices(
            (self.LKneeRotY + self.RKneeRotY) / 2.0, self.LegT),
            tf.concatenate_matrices(
            (self.LHipRotY + self.RHipRotY) / 2.0, self.HipT),
            tf.concatenate_matrices(
            self.AbsRotY, self.AbsT),
            tf.concatenate_matrices(
            self.BustRotY, self.BustT)
        ])

        a = dot(self.transformy[1], self.transformy[2])
        b = dot(a, self.transformy[3])
        c = dot(b, self.transformy[4])
        d = dot(c, self.transformy[5])
        e = dot(d, self.transformy[6])

        self.comtrans = array([self.transformy[1],
                               a,
                               b,
                               c,
                               d,
                               e
                               ])
Exemple #20
0
def generate_leg(leg: str, params: list):
    front_signs = {'front': '', 'rear': '-'}
    side_signs = {'left': '', 'right': '-'}
    lower_limits = {'left': '0', 'right': str(-pi)}
    upper_limits = {'left': str(pi), 'right': '0'}
    with open('../urdf/leg.urdf.template', 'r') as file:
        leg_template = Template(file.read())

    mappings = {'l_width': 0.02, 'leg': leg}
    front, side = leg.split('_')
    mappings['front_sign'] = front_signs[front]
    mappings['side_sign'] = side_signs[side]
    mappings['lower_limit'] = lower_limits[side]
    mappings['upper_limit'] = upper_limits[side]

    for param in params:
        try:
            d = float(param['d'])
        except ValueError:
            d = 0.0
        try:
            th = float(param['th'])
        except ValueError:
            th = 0.0
        try:
            a = float(param['a'])
        except ValueError:
            a = 0.0
        try:
            al = float(param['al'])
        except ValueError:
            al = 0.0

        tz = translation_matrix((0, 0, d))
        rz = rotation_matrix(th, zaxis)
        tx = translation_matrix((a, 0, 0))
        rx = rotation_matrix(al, xaxis)

        matrix = concatenate_matrices(tz, rz, tx, rx)

        rpy = euler_from_matrix(matrix)
        xyz = translation_from_matrix(matrix)

        name = param['joint']

        mappings[f'{name}_j_xyz'] = '{} {} {}'.format(*xyz)
        mappings[f'{name}_j_rpy'] = '{} {} {}'.format(*rpy)
        mappings[f'{name}_l_xyz'] = '{} 0 0'.format(xyz[0] / 2.)
        mappings[f'{name}_l_rpy'] = '0 0 0'
        mappings[f'{name}_l_len'] = str(a)

    return leg_template.substitute(mappings)
def rotated_vector_mat(axisFrom, axisTo):
    v0 = Vector((axisFrom[0], axisFrom[1], axisFrom[2]))
    v1 = Vector((axisTo[0], axisTo[1], axisTo[2]))
    # rotation
    rot = v1.rotation_difference(v0).to_euler()
    trace([degrees(a) for a in rot])
    matrix1 = rotation_matrix(rot[0], [1, 0, 0])
    matrix11 = rotation_matrix(rot[1], [0, 1, 0])
    matrix111 = rotation_matrix(rot[2], [0, 0, 1])
    # combine transformations
    mat_out = matrix1 * matrix11 * matrix111
    print(mat_out)
    return mat_out
def make_translation_pattern_coromeridian(coromeridian_pole,
                                          translation_velocity,
                                          frame_rate,
                                          epoch_duration=2.25,
                                          max_sensory_radius=2,
                                          star_density=50):
    #translate in a direction on the equator.
    from scipy.misc import imresize
    frame_period = 1 / frame_rate
    frame_distance = frame_period * translation_velocity
    frames_per_cycle = around(epoch_duration * frame_rate).astype(int)
    ####################
    xyz = make_universe(star_density=star_density,
                        max_sensory_radius=max_sensory_radius + frame_distance)
    ###align the orientation vector with arena coordinates

    unit_vector = np.array([0., 1., 0.])
    zp_offset = (2 * pi) / 96 * 4
    orientation_vector = dot(
        tran.rotation_matrix(coromeridian_pole, array([1, 0, 0]))[:3, :3],
        unit_vector)
    orientation_vector = dot(
        tran.rotation_matrix(zp_offset, array([0, 0, 1]))[:3, :3],
        orientation_vector) * translation_velocity

    xyzp = integrate_frame(xyz,
                           rotation_vect=np.array([0.1, 0.1, 0.1]),
                           translation_vect=orientation_vector,
                           frame_period=1 / frame_rate)
    ###crop to max_sensory_radius
    xyzp = crop_universe(xyzp, max_sensory_radius)
    proj = arena_project(xyzp)
    imgs = digitize_points(*proj, display_shape=display_shape)
    imgs = imgs[:, :, newaxis]
    xyzp = refill_universe(xyzp, star_density, max_sensory_radius,
                           frame_distance)
    #####################
    for i in range(frames_per_cycle):
        xyzp = integrate_frame(xyzp,
                               rotation_vect=np.array([1e-10, 1e-10, 1e-10]),
                               translation_vect=orientation_vector,
                               frame_period=1 / frame_rate)
        xyzp = crop_universe(xyzp, max_sensory_radius)
        proj = arena_project(xyzp)
        img = digitize_points(*proj, display_shape=display_shape)
        imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
        xyzp = refill_universe(xyzp, star_density, max_sensory_radius,
                               frame_distance)
    from scipy import io
    return imgs
  def dp_2_pc(self, depth_img, xmap, ymap, trans, rot):

    offset = 50

    def convert_to_zdepth(img):
      return ((img[:, :, 0] * (750 / 255)) + offset) * 1.03

    alpha_thresh = 100

    flattened_alpha = np.ravel(depth_img[:,:,3].T)
    id = np.arange(flattened_alpha.shape[0])
    cond_idx = id[flattened_alpha > 100]

    xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1]
    cloud = np.zeros((424, 512, 3))
    dp_im = convert_to_zdepth(depth_img)

    cloud[:, :, 2] = dp_im[:, :]
    cloud[:, :, 0] = xmap * cloud[:, :, 2]
    cloud[:, :, 1] = ymap * cloud[:, :, 2]

    cloud = np.reshape(cloud, (-1, 3))
    # cloud = cloud[(cloud[:, 2] > offset + 0.2)]

    if (rot[0] > 0):
      rotyx = 180.0 - rot[0]
    else:
      rotyx = (-rot[0])

    rotyy = rot[1]

    if (trans[0] * trans[2]) > 0:
      if rotyy < 0:
        rotyy = -rotyy
      else:
        rotyy = -180 + rotyy
    else:
      if rotyy < 0:
        rotyy = -180 + rotyy
      else:
        rotyy = -rotyy

    rot_mat = np.dot(rotation_matrix(0, zaxis), np.dot(rotation_matrix(math.radians(rotyy), yaxis), rotation_matrix(math.radians(rotyx), xaxis)))

    cloud = np.dot(cloud, rot_mat[:3, :3].T)
    cloud += trans

    cloud = cloud[cond_idx, :]

    return cloud
Exemple #24
0
def rotmat3(alpha,beta,gamma,xyzreftuple = ([1, 0, 0], [0, 1, 0], [0, 0, 1]),angtype='deg'):

    xaxis, yaxis, zaxis = xyzreftuple

    if angtype == 'deg':
        alpha = np.deg2rad(alpha)
        beta  = np.deg2rad(beta)
        gamma = np.deg2rad(gamma)

    Rx = trans.rotation_matrix(alpha, xaxis)
    Ry = trans.rotation_matrix(beta, yaxis)
    Rz = trans.rotation_matrix(gamma, zaxis)
    R = trans.concatenate_matrices(Rz, Ry, Rx)[:3,:3]

    return R
Exemple #25
0
 def _orientation_distance(self, joint_orientation):
     joint_euler_angles = quaternion_to_euler(joint_orientation)
     rotmat_constraint = np.eye(4)
     rotmat_target = np.eye(4)
     for i in range(3):
         if self.orientation[i] is not None:
             tmp_constraint = rotation_matrix(
                 np.deg2rad(self.orientation[i]), self.ROTATION_AXIS[i])
             rotmat_constraint = np.dot(tmp_constraint, rotmat_constraint)
             tmp_target = rotation_matrix(np.deg2rad(joint_euler_angles[i]),
                                          self.ROTATION_AXIS[i])
             rotmat_target = np.dot(tmp_target, rotmat_target)
     rotation_distance = GlobalTransformConstraint._vector_distance(
         np.ravel(rotmat_constraint), np.ravel(rotmat_target), 16)
     return rotation_distance
Exemple #26
0
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
    rospy.logdebug("joint: %s"%jointname)
    U = get_pr2_urdf()
        
    joint = U.joints[jointname]
    joint.origin = joint.origin or urdf.Pose()
    if not joint.origin.position: joint.origin.position = [0,0,0]
    if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
   
    parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))

    childFromRotated = np.eye(4)
    if valuedict and jointname in valuedict:        
        
        if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
            childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
        elif joint.joint_type == 'prismatic':
            childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname]) 
            
    parentFromRotated = np.dot(parentFromChild, childFromRotated)            
    originFromParent = conversions.pose_to_hmat(ps.pose)
    originFromRotated = np.dot(originFromParent, parentFromRotated)
    
    newps = gm.PoseStamped()
    newps.header = ps.header
    newps.pose = conversions.hmat_to_pose(originFromRotated)
    
    return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
Exemple #27
0
	def __init__( self , fovy , ratio , near , far , robot_files ) :
		self.fovy = fovy
		self.near = near 
		self.far = far
		self.ratio = ratio

		self.camera = None
		self.plane  = Plane( (2,2) )

		self.wall = Plane( (20,10) )
		self.mw = tr.rotation_matrix( -m.pi / 2.0 , (1,0,0) )
		self.mw = np.dot( self.mw , tr.translation_matrix( (0,3,0) ) )

		self.robot = Robot( robot_files )

		self.x = 0.0

		self.last_time = timer()

		self.plane_alpha = 65.0 / 180.0 * m.pi

		self.lpos = [ 1 ,-1 , 0 ]

		self._make_plane_matrix()

		self.draw_robot = True
		self.draw_sparks = True 
		self.draw_front = False
		self.draw_back = False
Exemple #28
0
 def Rotation(self, angle, dim, axis):
     self.size = dim
     mat = tm.rotation_matrix(angle, Matrix.Axis[axis])
     if dim == 3:
         self.matrix = mat[:3,:3]
     else:
         self.matrix = mat
Exemple #29
0
	def make_pts( self , corners ) :
		dx = (corners[3] - corners[0])
		dx = dx / la.norm(dx)
		dy = (corners[1] - corners[0]) / (self.size[1]+3-1)
		dz = np.cross( dx , dy )
		dz = dz / la.norm(dz)
		ax = np.cross( dx , dz )
		ax = ax / la.norm(ax)

		an = 2.0 * m.pi / float(self.size[0]) 
		rot = tr.rotation_matrix( an , ax )

		ox = (corners[3] + corners[0]) / 2.0
		rx = np.resize( ox - corners[0] , 4 )

		self.axis   = ax
		self.center = ox

		del self.pts[:]

		for y in range(self.size[1]+3) :
			drx = copy(rx)
			for x in range(self.size[0]) :
				pt = ox + np.resize(drx,3) 
				self.pts.append(     pt )
				drx = np.dot( drx , rot )
			self.pts.append( self.pts[-self.size[0]] )
			self.pts.append( self.pts[-self.size[0]] )
			self.pts.append( self.pts[-self.size[0]] )
			ox += dy 
Exemple #30
0
    def update_local_transformation(self):
        """
        update local transformation w.r.t element's parent

        .. seealso:: VRML transform calculation http://www.web3d.org/x3d/specifications/vrml/ISO-IEC-14772-VRML97/part1/nodesRef.html#Transform
        """
        if self.jointType in ["free", "freeflyer"]:
            self.localTransformation = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])

            scale = [1,1,1]
            if self.parent:
                scale = self.cumul_scale()
            scale_translation = [self.translation[i]*scale[i] for i in range(3)]
            self.localTransformation[0:3,3]=numpy.array(scale_translation)+\
                numpy.dot(numpy.eye(3)-self.localTransformation[0:3,:3],numpy.array(self.center))

        elif ( type(self) == Joint and self.jointType in [ "rotate", "rotation", "revolute"]
               and self.jointId >= 0):
            if self.jointAxis in ["x","X"]:
                axis = [1, 0, 0]
            elif self.jointAxis in ["y","Y"]:
                axis = [0, 1, 0]
            elif self.jointAxis in ["z","Z"]:
                axis = [0, 0, 1]
            else:
                raise RuntimeError("Invalid joint axis {0}".format(self.jointAxis))
            self.localR2= tf.rotation_matrix(self.angle, axis)[:3,:3]
            self.localTransformation[0:3,0:3] = numpy.dot(self.localR1, self.localR2)
Exemple #31
0
 def Rotation(self, angle, dim, axis):
     self.size = dim
     mat = tm.rotation_matrix(angle, Matrix.Axis[axis])
     if dim == 3:
         self.matrix = mat[:3, :3]
     else:
         self.matrix = mat
    def plot_fingertip_contacts(self):
        self._plot_handler = []
        colors = [
            np.array((1, 0, 0)),
            np.array((0, 1, 0)),
            np.array((0, 0, 1))
        ]
        tip_link_ids = self.get_fingertip_links()
        point_size = 0.005

        for i in range(len(tip_link_ids)):
            link = self._or_hand.GetLink(tip_link_ids[i])
            T = link.GetGlobalMassFrame()
            local_frame_rot = transformations.rotation_matrix(
                math.pi / 6., [0, 0, 1])[:3, :3]
            T[:3, :3] = T[:3, :3].dot(local_frame_rot)

            offset = T[0:3, 0:3].dot(self.get_tip_offsets())
            T[0:3, 3] = T[0:3, 3] + offset

            position = T[:3, -1]
            self._plot_handler.append(
                self._or_env.plot3(points=position,
                                   pointsize=point_size,
                                   colors=colors[i],
                                   drawstyle=1))
            for j in range(3):
                normal = T[:3, j]
                self._plot_handler.append(
                    self._or_env.drawarrow(p1=position,
                                           p2=position + 0.05 * normal,
                                           linewidth=0.001,
                                           color=colors[j]))
Exemple #33
0
    def stretchTo(self, goal, doStretch):
        length, self.matrixGlobal = getMatrix(self.getHead(), goal, 0)
        if doStretch:
            factor = length / self.length
            self.matrixGlobal[:3, 1] *= factor
        pose = self.getPoseFromGlobal()

        if 0 and self.name in ["DfmKneeBack_L", "DfmLoLeg_L"]:
            log.debug("Stretch %s", self.name)
            log.debug("G %s", goal)
            log.debug("M1 %s", self.matrixGlobal)
            log.debug("P1 %s", pose)

        az, ay, ax = tm.euler_from_matrix(pose, axes='szyx')
        rot = tm.rotation_matrix(-ay + self.roll, CBone.Axes[1])
        self.matrixGlobal[:3, :3] = np.dot(self.matrixGlobal[:3, :3],
                                           rot[:3, :3])
        pose2 = self.getPoseFromGlobal()

        if 0 and self.name in ["DfmKneeBack_L", "DfmLoLeg_L"]:
            log.debug("A %s %s %s", ax, ay, az)
            log.debug("R %s", rot)
            log.debug("M2 %s", self.matrixGlobal)
            log.debug("P2 %s", pose)
            log.debug("")
Exemple #34
0
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
    rospy.logdebug("joint: %s" % jointname)
    U = get_pr2_urdf()

    joint = U.joints[jointname]
    joint.origin = joint.origin or urdf.Pose()
    if not joint.origin.position: joint.origin.position = [0, 0, 0]
    if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]

    parentFromChild = conversions.trans_rot_to_hmat(
        joint.origin.position,
        transformations.quaternion_from_euler(*joint.origin.rotation))

    childFromRotated = np.eye(4)
    if valuedict and jointname in valuedict:

        if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
            childFromRotated = transformations.rotation_matrix(
                valuedict[jointname], joint.axis)
        elif joint.joint_type == 'prismatic':
            childFromRotated = transformations.translation_matrix(
                np.array(joint.axis) * valuedict[jointname])

    parentFromRotated = np.dot(parentFromChild, childFromRotated)
    originFromParent = conversions.pose_to_hmat(ps.pose)
    originFromRotated = np.dot(originFromParent, parentFromRotated)

    newps = gm.PoseStamped()
    newps.header = ps.header
    newps.pose = conversions.hmat_to_pose(originFromRotated)

    return make_kin_tree_from_link(newps,
                                   joint.child,
                                   ns=ns,
                                   valuedict=valuedict)
Exemple #35
0
    def make_pts(self, corners):
        dx = (corners[3] - corners[0])
        dx = dx / la.norm(dx)
        dy = (corners[1] - corners[0]) / (self.size[1] + 3 - 1)
        dz = np.cross(dx, dy)
        dz = dz / la.norm(dz)
        ax = np.cross(dx, dz)
        ax = ax / la.norm(ax)

        an = 2.0 * m.pi / float(self.size[0])
        rot = tr.rotation_matrix(an, ax)

        ox = (corners[3] + corners[0]) / 2.0
        rx = np.resize(ox - corners[0], 4)

        self.axis = ax
        self.center = ox

        del self.pts[:]

        for y in range(self.size[1] + 3):
            drx = copy(rx)
            for x in range(self.size[0]):
                pt = ox + np.resize(drx, 3)
                self.pts.append(pt)
                drx = np.dot(drx, rot)
            self.pts.append(self.pts[-self.size[0]])
            self.pts.append(self.pts[-self.size[0]])
            self.pts.append(self.pts[-self.size[0]])
            ox += dy
Exemple #36
0
 def poleTargetCorrect(self, head, goal, pole, angle):
     yvec = goal-head
     xvec = pole-head
     xy = dot(xvec, yvec)/dot(yvec,yvec)
     xvec = xvec - xy * yvec
     xlen = math.sqrt(dot(xvec,xvec))
     if xlen > 1e-6:
         xvec = xvec / xlen
         zvec = self.matrixGlobal[:3,2]
         zlen = math.sqrt(dot(zvec,zvec))
         zvec = zvec / zlen
         angle0 = math.asin( dot(xvec,zvec) )
         rot = tm.rotation_matrix(angle - angle0, CBone.Axes[1])
         #m0 = self.matrixGlobal.copy()
         self.matrixGlobal[:3,:3] = dot(self.matrixGlobal[:3,:3], rot[:3,:3])
         
         if 0 and self.name == "DfmUpArm2_L":
             log.debug("")
             log.debug("IK %s", self.name)
             log.debug("X %s", xvec)
             log.debug("Y %s", yvec)
             log.debug("Z %s", zvec)
             log.debug("A0 %s", angle0)
             log.debug("A %s", angle)
             log.debug("R %s", rot)
             log.debug("M0 %s", m0)
             log.debug("M %s", self.matrixGlobal)
def align(atoms, init_vec, align_vec):
    import transformations
    import numpy as np

    if len(init_vec) == 2:
        orig_atom_ind = init_vec[0]
        final_atom_ind = init_vec[1]
        init_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position

    if len(align_vec) == 2:
        orig_atom_ind = align_vec[0]
        final_atom_ind = align_vec[1]
        align_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position

    rot_axis = np.cross(init_vec, align_vec)
    nrot_axis = rot_axis/np.linalg.norm(rot_axis)

    angle = transformations.angle_between_vectors(init_vec, align_vec)

    rot_M = transformations.rotation_matrix(angle, nrot_axis)[:3,:3]

    for a in atoms:
        a.position = rot_M.dot(a.position)

    return atoms
Exemple #38
0
        def get_new_point(
            reference_point, start_point, angle, axis
        ):  # where angle is radians of the stem length (edge to opposite vertex)

            M = rotation_matrix(angle, axis)
            new_point = np.dot(start_point, M[:3, :3].T)
            return new_point + reference_point
Exemple #39
0
def ur2np(ur_pose):
    # ROS pose
    ros_pose = []

    # ROS position
    ros_pose.append(ur_pose[0])
    ros_pose.append(ur_pose[1])
    ros_pose.append(ur_pose[2])

    # Ros orientation
    angle = sqrt(ur_pose[3]**2 + ur_pose[4]**2 + ur_pose[5]**2)
    direction = [i / angle for i in ur_pose[3:6]]
    np_T = tf.rotation_matrix(angle, direction)
    np_q = tf.quaternion_from_matrix(np_T)
    ros_pose.append(np_q[0])
    ros_pose.append(np_q[1])
    ros_pose.append(np_q[2])
    ros_pose.append(np_q[3])

    # orientation
    np_pose = tf.quaternion_matrix(
        [ros_pose[3], ros_pose[4], ros_pose[5], ros_pose[6]])

    # position
    np_pose[0][3] = ros_pose[0]
    np_pose[1][3] = ros_pose[1]
    np_pose[2][3] = ros_pose[2]

    return np_pose
Exemple #40
0
    def poleTargetCorrect(self, head, goal, pole, angle):
        yvec = goal-head
        xvec = pole-head
        xy = np.dot(xvec, yvec)/np.dot(yvec,yvec)
        xvec = xvec - xy * yvec
        xlen = math.sqrt(np.dot(xvec,xvec))
        if xlen > 1e-6:
            xvec = xvec / xlen
            zvec = self.matrixGlobal[:3,2]
            zlen = math.sqrt(np.dot(zvec,zvec))
            zvec = zvec / zlen
            angle0 = math.asin( np.dot(xvec,zvec) )
            rot = tm.rotation_matrix(angle - angle0, CBone.Axes[1])
            #m0 = self.matrixGlobal.copy()
            self.matrixGlobal[:3,:3] = np.dot(self.matrixGlobal[:3,:3], rot[:3,:3])

            if 0 and self.name == "DfmUpArm2_L":
                log.debug("")
                log.debug("IK %s", self.name)
                log.debug("X %s", xvec)
                log.debug("Y %s", yvec)
                log.debug("Z %s", zvec)
                log.debug("A0 %s", angle0)
                log.debug("A %s", angle)
                log.debug("R %s", rot)
                log.debug("M0 %s", m0)
                log.debug("M %s", self.matrixGlobal)
Exemple #41
0
    def send_msg(self):
        if self.msg_sent:
            return

        print('Sending msg')
        self.msg_sent = True
        front_point = self.transform('odom', 'front_point')
        inc = tf.concatenate_matrices(
            tf.rotation_matrix(-5 * pi / 180, (0, 0, 1)),
            tf.translation_matrix((0.03, 0.01, 0)),
        )
        path = [tf.concatenate_matrices(front_point, inc)]
        for i in range(179):
            path.append(tf.concatenate_matrices(path[-1], inc))

        msg = Path()
        msg.header.frame_id = 'odom'
        msg.header.stamp.sec = int(self.get_clock().now().nanoseconds / 10**9)
        msg.header.stamp.nanosec = self.get_clock().now().nanoseconds % 10**9

        poses = [matrix_to_pose(point) for point in path]
        for pose in poses:
            pose_stamped = PoseStamped()
            pose_stamped.header = msg.header
            pose_stamped.pose = pose
            msg.poses.append(pose_stamped)

        self.pub.publish(msg)
        print('Sent')
def writeBindPose(fp, meshes, skel, config):
    id,key = getId("Pose::" + skel.name)
    nBones = skel.getBoneCount()
    nMeshes = len(meshes)

    fp.write(
        '    Pose: %d, "%s", "BindPose" {\n' % (id, key)+
        '        Type: "BindPose"\n' +
        '        Version: 100\n' +
        '        NbPoseNodes: %d\n' % (1+nMeshes+nBones))

    startLinking()

    # Skeleton bind matrix
    skelbindmat = tm.rotation_matrix(math.pi/2, (1,0,0))
    poseNode(fp, "Model::%s" % skel.name, skelbindmat)

    for mesh in meshes:
        poseNode(fp, "Model::%sMesh" % mesh.name, skelbindmat)

    for bone in skel.getBones():
        bindmat,_ = bone.getBindMatrix(config.offset)
        poseNode(fp, "Model::%s" % bone.name, bindmat)

    stopLinking()
    fp.write('    }\n')
Exemple #43
0
    def init_local_transformation(self):
        """
        compute local transformation w.r.t for the first time (compute everything)
        """
        # rotation part
        self.localTransformation = numpy.eye(4)
        # print type(self), id(self), self.rotation
        try:
            angle = self.rotation[3]
        except IndexError:
            logger.exception("Failed on {0}, rotation={1}".format(type(self),self.rotation))
            raise
        direction = self.rotation[:3]
        self.localTransformation[0:3,0:3] = tf.rotation_matrix(angle, direction)[:3,:3]
        self.rpy = tf.euler_from_matrix(self.localTransformation)

        # last column
        scale = [1,1,1]
        if self.parent:
            scale = self.cumul_scale()
        scale_translation = [self.translation[i]*scale[i] for i in range(3)]

        self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
            numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
                      numpy.array(self.center))

        # last line
        self.localTransformation[3,0:4]=[0,0,0,1]
Exemple #44
0
    def init_local_transformation(self):
        """
        compute local transformation w.r.t for the first time (compute everything)
        """
        # rotation part
        self.localTransformation = numpy.eye(4)
        # print type(self), id(self), self.rotation
        try:
            angle = self.rotation[3]
        except IndexError:
            logger.exception("Failed on {0}, rotation={1}".format(
                type(self), self.rotation))
            raise
        direction = self.rotation[:3]
        self.localTransformation[0:3,
                                 0:3] = tf.rotation_matrix(angle,
                                                           direction)[:3, :3]
        self.rpy = tf.euler_from_matrix(self.localTransformation)

        # last column
        scale = [1, 1, 1]
        if self.parent:
            scale = self.cumul_scale()
        scale_translation = [self.translation[i] * scale[i] for i in range(3)]

        self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
            numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
                      numpy.array(self.center))

        # last line
        self.localTransformation[3, 0:4] = [0, 0, 0, 1]
Exemple #45
0
	def step( self , dt ) :
		rot = tr.rotation_matrix( dt * self.w , (0,0,1) )

		self.v1 = np.resize( np.dot( rot , np.resize( self.v1 , 4 ) ) , 2 )
		R = self.v1 * self.l1
		self.v2[1] = -R[1]
		L = self.l2 + rand.normalvariate(self.mu,self.sg) 
		self.v2[0] = m.sqrt(L**2-self.v2[1]**2)
Exemple #46
0
 def fixCSysBone(self, bname, infix, mat, index, axis, angle):
     csys = csysBoneName(bname, infix)
     bone = self.armature.bones[csys]
     rot = tm.rotation_matrix(angle, axis)
     cmat = np.dot(mat, rot)
     bone.tail = bone.head + self.armature.bones[bname].length * cmat[:3,1]
     normal = getUnitVector(mat[:3,index])
     bone.roll = computeRoll(bone.head, bone.tail, normal)
Exemple #47
0
	def _make_plane_matrix( self ) :
		r = tr.rotation_matrix( self.plane_alpha , (0,0,1) )
		s = tr.scale_matrix( 1 )
		t = tr.translation_matrix( (-1.25,.7,.05) )

		self.m = np.dot( np.dot( t , s ) , r )
		self.im = la.inv( self.m )
		self.im[3] = [ 0 , 0 , 0 , 1 ]
Exemple #48
0
 def rotate(self, angle, axis, rotWorld):
     mat = tm.rotation_matrix(angle*D, CBone.Axes[axis])
     if rotWorld:
         mat = dot(mat, self.matrixGlobal)        
         self.matrixGlobal[:3,:3] = mat[:3,:3]
         self.matrixPose = self.getPoseFromGlobal()
     else:
         mat = dot(mat, self.matrixPose)
         self.matrixPose[:3,:3] = mat[:3,:3]
Exemple #49
0
    def __init__(self,dis=[0,0,0],angle=0,axis=[0,0,1],unit='deg'):
        """initialization of object, dis = displacement, angle is the angle of
        rotation around the vector axis, unit is the unit of the angle."""
        self.__dis = np.array(dis).reshape((3,1))
        if unit == 'deg': angle = angle*np.pi/180.
        # otherwise, angle is assumed to be in radians
        self.__rot = tf.rotation_matrix(angle,axis)[0:3,0:3]

        self.__hom     = np.vstack( (np.hstack((self.__rot,self.__dis)), np.array([0,0,0,1]))) 
        self.__hom_inv = np.vstack( (np.hstack((self.__rot.T,-np.dot(self.__rot.T,self.__dis))), np.array([0,0,0,1]))) 
def calc_error(args, debug=False):

    # angle_x, angle_y, o_x, o_y, o_z = args
    angle_x, angle_y = args

    x = np.array([1, 0, 0, 1])
    R_x = transformations.rotation_matrix(angle_x, [1, 0, 0], eye_centre)
    R_y = transformations.rotation_matrix(angle_y, [0, 1, 0], eye_centre)
    S = transformations.scale_matrix(6)
    T = transformations.translation_matrix(eye_centre)
    # T = transformations.translation_matrix(eye_centre + np.array([o_x*100, o_y*100, o_z*100]))
    T2 = transformations.translation_matrix([0,0,-12])

    if debug:
        trans = transformations.concatenate_matrices(*reversed([T, T2, R_x, R_y]))
        pt = dehomo(trans.dot([0,0,-50,1]))
        cv2.line(img,
                 tuple(dehomo(cam_mat.dot(coord_swap.dot(true_iris_centre_3d))).astype(int)),
                 tuple(dehomo(cam_mat.dot(coord_swap.dot(pt))).astype(int)),
                 (255, 255, 0))

    est_pts = []
    for t in np.linspace(0, np.pi*2, accuracy):

        R = transformations.rotation_matrix(t, [0, 0, 1])
        trans = transformations.concatenate_matrices(*reversed([R, S, T, T2, R_x, R_y]))

        threeD_pt = coord_swap.dot(dehomo(trans.dot(x)))

        pt = cam_mat.dot(threeD_pt)

        if point_in_poly(dehomo(pt).astype(int), data["ldmks_lids_2d"]):
            est_pts.append(dehomo(pt).astype(int))
            if debug: cv2.circle(img, tuple(dehomo(pt).astype(int)), 1, (255, 0, 0), -1)

    try:
        D = cdist(est_pts, visible_pts, 'euclidean')
        H1 = np.max(np.min(D, axis=1))
        H2 = np.max(np.min(D, axis=0))
        return (H1 + H2) / 2.0
    except ValueError:
        return 20
def get_standard_matrixes():
    #90 degree clockwise around center of normalized square
    R90CW = tr.rotation_matrix(-math.pi/2,[0,0,1],[0.5,0.5,0.5])
    #R90CCW = tr.rotation_matrix(math.pi/2,[0,0,1],[0.5,0.5,0.5])

    #horizontal axis flip
    FH = tr.reflection_matrix([0.5,0.5,0],[0,1,0])

    #vertical axis flip
    FV = tr.reflection_matrix([0.5,0.5,0],[1,0,0])

    R90CW_FH = tr.concatenate_matrices(FH,R90CW)

    R90CW_FV = tr.concatenate_matrices(FV,R90CW)

    R180CW = tr.rotation_matrix(-math.pi,[0,0,1],[0.5,0.5,0])
    #R180CCW = tr.rotation_matrix(math.pi,[0,0,1],[0.5,0.5,0])
    #R270CCW = tr.rotation_matrix(2*math.pi,[0,0,1],[0.5,0.5,0])

    return R90CW,FH,FV,R90CW_FH,R90CW_FV,R180CW
Exemple #52
0
def test_obj_pose():
    from transformations import rotation_matrix, concatenate_matrices, euler_from_matrix
    from utils import _axis_rot_matrices, _ypr_from_rot_matrix
    alpha, beta, gamma = 0.123, -1.234, 2.345
    origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
    # I = identity_matrix()
    Rx_tf = rotation_matrix(gamma, xaxis)
    Ry_tf = rotation_matrix(beta, yaxis)
    Rz_tf = rotation_matrix(alpha, zaxis)

    obj = Obj("obj")
    Rz, Ry, Rx = _axis_rot_matrices([alpha, beta, gamma, 0., 0., 0.])
    assert np.allclose(Rx_tf[:3,:3], Rx)
    assert np.allclose(Ry_tf[:3,:3], Ry)
    assert np.allclose(Rz_tf[:3,:3], Rz)
    R = concatenate_matrices(Rz_tf, Ry_tf, Rx_tf)
    rot_mat = np.dot(Rz, np.dot(Ry, Rx))
    euler = euler_from_matrix(R, 'sxyz')
    assert np.allclose(R[:3,:3], rot_mat)
    assert np.allclose([gamma, beta, alpha], euler)
    assert np.allclose([alpha, beta, gamma], _ypr_from_rot_matrix(R[:3,:3]))
def writeBindPose(fp, meshes, skel, config):
    id, key = getId("Pose::" + skel.name)
    nBones = skel.getBoneCount()
    nMeshes = len(meshes)

    # Skeleton bind matrix
    skelbindmat = tm.rotation_matrix(math.pi / 2, (1, 0, 0))

    count = 1 + nMeshes + nBones

    if config.binary:
        from . import fbx_binary

        elem = fbx_binary.get_child_element(fp, "Objects")
        pelem = fbx_binary.fbx_data_bindpose_element(elem, key, id, count)
    else:
        fp.write(
            '    Pose: %d, "%s", "BindPose" {\n' % (id, key)
            + '        Type: "BindPose"\n'
            + "        Version: 100\n"
            + "        NbPoseNodes: %d\n" % count
        )

    startLinking()

    key = "Model::%s" % skel.name
    if config.binary:
        id, _ = getId(key)
        fbx_binary.fbx_data_pose_node_element(pelem, key, id, skelbindmat)
    else:
        poseNode(fp, key, skelbindmat)

    for mesh in meshes:
        key = "Model::%sMesh" % mesh.name
        if config.binary:
            id, _ = getId(key)
            fbx_binary.fbx_data_pose_node_element(pelem, key, id, skelbindmat)
        else:
            poseNode(fp, key, skelbindmat)

    for bone in skel.getBones():
        key = "Model::%s" % bone.name
        bindmat, _ = bone.getBindMatrix(config.offset)
        if config.binary:
            id, _ = getId(key)
            fbx_binary.fbx_data_pose_node_element(pelem, key, id, bindmat)
        else:
            poseNode(fp, key, bindmat)

    stopLinking()

    if not config.binary:
        fp.write("    }\n")
def cone_transf_m(apex,L,r_y):
    import numpy as np
    import transformations
    """a matrix that rotates and translates coordinates. A cone defined by the coordinates of it's apex,
    the vector L between the centre of its base and the apex, and a vector r_y between the centre of its base and a point of maximum radius (the cone can be elliptical)
    is translated such that the apex is at the origin, the cone lies along the z axis and the radius of maximum width lies along the y-axis"""
    trans = -np.array(apex).reshape(3,1)

    angle1 = transformations.angle_between_vectors(L, [0,0,1])
    if angle1:
        rot_axis1 = np.cross(L,[0,0,1])
        nrot_axis1 = rot_axis1/np.linalg.norm(rot_axis1)
        rot1 = transformations.rotation_matrix(angle1, nrot_axis1)[:3,:3]
    else:
        rot1 = np.identity(3)

    r_y = rot1.dot(r_y)
    angle2 = transformations.angle_between_vectors(r_y, [0,1,0])
    if angle2:
        rot_axis2 = np.cross([0,1,0],r_y)
        nrot_axis2 = rot_axis2/np.linalg.norm(rot_axis2)
        rot2 = transformations.rotation_matrix(angle2, nrot_axis2)[:3,:3]
    else:
        rot2 = np.identity(3)

    #combine rot matrices
    rot = np.dot(rot1,rot2)

    #make rotation matrix into an affine matrix
    final_rot = np.identity(4)
    final_rot[:3,:3] = rot

    #make translation into an affine matrix
    final_trans = np.identity(4)
    final_trans[:3,3:] = trans

    #combine translation and rotation
    final = final_rot.dot(final_trans)

    return final
Exemple #55
0
def canonical_rot_mat(start_on_unit_sphere, end_on_unit_sphere):
    theta = np.arccos(np.dot(start_on_unit_sphere, end_on_unit_sphere))
    if epsilon_eq(theta, 0, theta_epsilon):
        mat = identity_rotation.copy()
    elif epsilon_eq(theta, np.pi, theta_epsilon):
        mat = identity_rotation.copy()
        mat[2,2] = -1.0
    else:
        v = np.cross(start_on_unit_sphere, end_on_unit_sphere)
        mat = np.matrix(transformations.rotation_matrix(theta, v)[:3,:3])
    end_test = times_column3(mat, start_on_unit_sphere)
    assert sum(abs(end_on_unit_sphere - end_test)) < 1e-2, '%s -> %s' % (end_on_unit_sphere, end_test)
    return mat
Exemple #56
0
	def set_block( self , s , d ) :
		aoy = m.atan2( s[2] , s[0] )
		aoz = m.atan2( s[1] , m.sqrt(s[0]**2+s[2]**2) )

		rot = tr.rotation_matrix( aoy , (0,1,0) )
		rot = np.dot( tr.rotation_matrix( -aoz , (0,0,1) ) , rot )
		rot = np.dot( tr.rotation_matrix( m.pi/2.0 , (0,0,1) ) , rot )

		v , n , t = self.gen_v( 1 , 1 , s )
		for x in range(v.shape[0]) :
			for y in range(v.shape[1]) :
				for z in range(v.shape[2]) :
					v[x,y,z] = np.dot(rot,v[x,y,z])
					n[x,y,z] = np.resize(np.dot(rot,np.resize(n[x,y,z],4)),3)
		Mesh.__init__( self , buffers = (v,n,t) )

		self.x = np.array( ((0,0,0,1),(s[0],0,0,1),(0,0,s[2],1),(s[0],0,s[2],1),(0,s[1],0,1),(s[0],s[1],0,1),(0,s[1],s[2],1),(s[0],s[1],s[2],1)) , np.float64 )
		for i in range(len(self.x)) : self.x[i] = np.dot(rot,self.x[i])
		self.r = np.resize( np.dot( rot , np.array((s[0],s[1],s[2],0) , np.float64 )/2.0 ) , 3 )
		self.m = np.array( [ d*s[0]*s[1]*s[2] / 8.0 ] * len(self.x) , np.float64 )
		self.M = self.calc_m( self.x , self.m )
		self.Mi = np.linalg.inv( self.M )
Exemple #57
0
def getMatrix(head, tail, roll):
    vector = toBlender3(tail - head)
    length = math.sqrt(dot(vector, vector))
    vector = vector/length
    yproj = dot(vector, YUnit)
    
    if yproj > 1-1e-6:
        axis = YUnit
        angle = 0
    elif yproj < -1+1e-6:
        axis = YUnit
        angle = pi
    else:
        axis = numpy.cross(YUnit, vector)
        axis = axis / math.sqrt(dot(axis,axis))
        angle = math.acos(yproj)
    mat = tm.rotation_matrix(angle, axis)
    if roll:
        mat = dot(mat, tm.rotation_matrix(roll, YUnit))         
    mat = fromBlender4(mat)
    mat[:3,3] = head
    return length, mat
Exemple #58
0
    def stretchTo(self, goal, doStretch):
        """
        Orient bone to point to goal position. Set doStretch to true to
        position the tail joint at goal, false to maintain length of this bone.
        """
        length, self.matPoseGlobal = _getMatrix(self.getHead(), goal, 0)
        if doStretch:
            factor = length/self.length
            self.matPoseGlobal[:3,1] *= factor
        pose = self.getPoseFromGlobal()

        az,ay,ax = tm.euler_from_matrix(pose, axes='szyx')
        rot = tm.rotation_matrix(-ay + self.roll_angle, Bone.Axes[1])
        self.matPoseGlobal[:3,:3] = np.dot(self.matPoseGlobal[:3,:3], rot[:3,:3])
Exemple #59
0
def sketch(robot):
    res = ""
    res += "def robot {\n"
    oldT = numpy.eye(4)
    for j in robot.moving_joint_list[:20]:
        T = j.globalTransformation
        T1 = numpy.eye(4)
        if j.jointAxis in ['y','Y']:
            T1 = tf.rotation_matrix(math.pi/2, [0,0,1])
        if j.jointAxis in ['z','Z']:
            T1 = tf.rotation_matrix(math.pi/2, [0,1,0])
        T = numpy.dot(T,T1)
        relT = numpy.dot(numpy.linalg.inv(oldT),T)
        angle, direc, point = tf.rotation_from_matrix(relT)
        print T
        p = relT[:3,3]
        p = [w*10 for w in p]
        res += """put {rotate (%f,(%f, %f, %f),[%f, %f, %f]) then translate ([%f, %f, %f])} {joint}
"""%(angle*180/math.pi,0,0,0, direc[0], direc[1], direc[2], p[0],p[1],p[2])
        oldT = T

    res += "}\n"
    return res