예제 #1
0
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()  # 0.0010000000474974513
max_depth = 6
max_depth_scaled = max_depth / depth_scale

# getting transf matrix from camera to robot origin
rot_camera_to_90 = mr.RollPitchYawToRot(0, 0, np.pi / 4)
transf_camera_to_90 = mr.RpToTrans(rot_camera_to_90, [0, 0, 0])
transf_90_to_base = np.array([[0, 0,1, 0.07274],\
                              [0,-1,0, 0.06474],\
                              [1, 0,0, -0.0175],\
                              [0, 0,0,       1]])
transf_camera_to_base = transf_90_to_base @ transf_camera_to_90

# initializing globals
trajectories = []
old_trajectories = []
depth_background = np.array([])
betas_depth_to_dia = np.load(
    "/home/justin/catkin_ws/src/v4_6dof/scripts/constants/betas_baseball.npy")

mc = MotorController()
rospy.init_node('talker', anonymous=True)
예제 #2
0
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()  # 0.0010000000474974513
max_depth = 6
max_depth_scaled = max_depth / depth_scale

# getting transf matrix from camera to robot origin
rot_camera_to_90 = mr.RollPitchYawToRot(0, 0, np.pi / 4)
transf_camera_to_90 = mr.RpToTrans(rot_camera_to_90, [0, 0, 0])
transf_90_to_base = np.array([[0, 0,1, 0.07274],\
                              [0,-1,0, 0.06474],\
                              [1, 0,0, -0.0175],\
                              [0, 0,0,       1]])
transf_camera_to_base = transf_90_to_base @ transf_camera_to_90

# initializing globals
trajectories = []
old_trajectories = []
depth_background = np.array([])
betas_depth_to_dia = np.load(
    "/home/justin/catkin_ws/src/v4_6dof/scripts/constants/betas.npy")

mc = MotorController()
rospy.init_node('talker', anonymous=True)
예제 #3
0
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale() # 0.0010000000474974513
max_depth = 6
max_depth_scaled = max_depth/depth_scale

# getting transf matrix from camera to robot origin
rot_camera_to_90 = mr.RollPitchYawToRot(0,0,np.pi/4)
transf_camera_to_90 = mr.RpToTrans(rot_camera_to_90, [0,0,0])
transf_90_to_base = np.array([[0, 0,1, 0.07274],\
                              [0,-1,0, 0.06474],\
                              [1, 0,0, -0.0175],\
                              [0, 0,0,       1]])
transf_camera_to_base = transf_90_to_base @ transf_camera_to_90

# initializing globals
trajectories = []
old_trajectories = []
depth_background = np.array([])
betas_depth_to_dia = np.load("/home/justin/catkin_ws/src/v4_6dof/scripts/constants/betas.npy")

mc = MotorController()
rospy.init_node('talker', anonymous=True)
from modules.MotorController import MotorController
from modules import modern_robotics as mr

# import v4_6dof.msg as msg

mc = MotorController()
np.set_printoptions(precision=7, suppress=True)
# these_angles = [1*np.pi/2, -1*np.pi/2, 1*np.pi/2, 0*np.pi/2, -1*np.pi/2, 0*np.pi/2]
# bodyM = mr.FKinBody(mc.M_rest, mc.body_list, these_angles)
# spaceM = mr.FKinSpace(mc.M_rest, mc.space_list, these_angles)
# print(spaceM)
# print(bodyM)

rospy.init_node('talker', anonymous=True)

mc.anglePublish([0, -1*np.pi/2, 1*np.pi/2, 0*np.pi/2, -1*np.pi/2, 0*np.pi/2], 0.3, True)
print(mr.FKinSpace(mc.M_rest, mc.space_list, [0, -1*np.pi/2, 1*np.pi/2, 0*np.pi/2, -1*np.pi/2, 0*np.pi/2]))

xyz = [0.129, 0.418, 0.322]
# R = mr.RollPitchYawToRot(np.pi/8, -1*np.pi/7, np.pi/9)
R = mr.RollPitchYawToRot(0,0,np.pi/4)
transf = mr.RpToTrans(R, xyz)
time = 1

print(f"this is goal: {transf}")
mc.transfMatrixAnalyticalPublish(transf, time)
mc.anglePublish([0, -1*np.pi/2, 1*np.pi/2, 0*np.pi/2, -1*np.pi/2, 0*np.pi/2], 0.3, True)
mc.transfMatrixCartesianPublish(transf, time)


#09-08-2021 max speed is 1.1rad/sec for all motors
예제 #5
0
def unpack_XML(xml):
    obj = untangle.parse(xml)
    # [child["name"] for child in o.root.child]

    # initialize T_list, body_list 
    T_list = []
    body_list = np.array([0,0,0,0,0,0])

    np.set_printoptions(precision=7, suppress=True)

    # grabbing the last joint (ee_joint) and its xyz
    rpy_ee = [float(n) for n in obj.robot.joint[len(obj.robot.joint)-1].origin["rpy"].split()]
    R_ee = mr.RollPitchYawToRot(rpy_ee[0],rpy_ee[1],rpy_ee[2])
    p_ee = [float(n) for n in obj.robot.joint[len(obj.robot.joint)-1].origin["xyz"].split()]
    T_ee = mr.RpToTrans(R_ee, p_ee)

    # skips all joints that are type fixed (like the base link and ee_link)
    joint_list = [joint for joint in obj.robot.joint if joint["type"]!="fixed"]

    for joint in reversed(joint_list):
        # find the roll-pitch-yaw, about the z-y-x axes of the previous joint ; convert to a rotation matrix
        rpy = [float(n) for n in joint.origin["rpy"].split()]
        R = mr.RollPitchYawToRot(rpy[0], rpy[1], rpy[2])

        # find the distance from previous joint to current joint
        p = np.array([float(n) for n in joint.origin["xyz"].split()])

        # this T takes previous joint to current joint, or is current joint relative to prev joint
        # T_56, T_lower_higher
        T = mr.RpToTrans(R,p)
        T_list.insert(0,T)

        # T_ee is end_effector joint relative to current joint, need inverse of that to get v
        (R_ee, p_ee) = mr.TransToRp(mr.TransInv(T_ee))

        # find which axis the motor at this joint turns about
        current_omega = [float(n) for n in joint.axis["xyz"].split()]
        # convert the axis into ee_frame
        ee_omega = np.dot(R_ee, current_omega)
        # skew symmetric it
        ee_omega_skewed = mr.VecToso3(ee_omega)

        # negative one here just works somehow
        current_v = -1*np.dot(ee_omega_skewed, p_ee)

        # combine w,v into body_axis, then insert into body_list
        body_axis = np.r_[current_omega, current_v]
        body_list = np.c_[body_axis, body_list]
        print(f"bodyaxis: {body_axis}")

        # update T_ee to be relative to current link T_56 * T_6ee = T_5ee
        T_ee = np.dot(T, T_ee)

    # remove the filler column needed to sart appending
    body_list = np.delete(body_list, len(body_list[0])-1,1)

    ##### inverse dynamics #####
    # need G_list, or spatial inertai matrix list
    #   6x6 matrix, top left corner is 3x3 rotational inertia matrix, bottom right is mass of link * identity matrix
    G_list = []

    # urdf file has world link and ee_link, so that all joints have a parent and child
    # also we dont need the base link and link from joint 6 to ee_link called link6 (remember 6 joints should only have 5 links)
    # so for this for loop, skip the first two and last two
    for link in obj.robot.link[2:-2]: 
        mass = float(link.inertial.mass["value"])

    # got these values from solidworks, similar enough to eigenvectors of rotational inertia matrix
        # ix = [float(n) for n in link.inertial.origin["ix"].split()]
        # iy = [float(n) for n in link.inertial.origin["iy"].split()]
        # iz = [float(n) for n in link.inertial.origin["iz"].split()]
        # principle_axes = np.c_[ix,iy,iz]

        # translate from parent joint to CoM 
        # negative one b/c this is from parent link origin to CoM, but I need CoM to parent link origin
        xyz_CoM= -1*np.array([float(n) for n in link.inertial.origin["xyz"].split()])

        # grab Ixx, Ixy, Ixz, Iyy, Iyz, Izz about the CoM, with the parent link coordinate systems
        inertia_values_CoM = [float(n) for n in (vars(link.inertial.inertia_CoM)["_attributes"].values())]
        
        # putting those values into a rotational inertia matrix, centered at CoM, using parent link coords
        I_CoM = np.array([[inertia_values_CoM[0], inertia_values_CoM[1], inertia_values_CoM[2]],
                        [inertia_values_CoM[1], inertia_values_CoM[3], inertia_values_CoM[4]],
                        [inertia_values_CoM[2], inertia_values_CoM[4], inertia_values_CoM[5]]])

        # grabbing the eigenvectors of the rotational inertia matrix, to find the principle axes of inertia
        w,v = np.linalg.eig(I_CoM) 
        # rotational inertia matrix, centered at CoM, aligned w/ principle axes of inertia
        rotated_I_CoM = np.transpose(v) @ I_CoM @ v
        # print(f"eigenvectors:\n {v}")  
        # print(f"inertia: \n{I_CoM}")
        # print(f"inertia about rotated coords: \n{rotated_I_CoM}")

        # rotational inertia matrix, centered at parent link origin, aligned w/ parent link origin coords
        translated_T_CoM = I_CoM + mass*(np.inner(xyz_CoM, xyz_CoM)*np.identity(3) - np.outer(xyz_CoM, xyz_CoM))
        # print(f"inertial rotational matrix at parent link: \n{translated_T_CoM}")

        # translated_T_CoM is pretty close to the value obtained from SOLIDWORKS
        # inertia_values_joint = [float(n) for n in (vars(link.inertial.inertia_joint)["_attributes"].values())]
        # I_joint = np.array([[inertia_values_joint[0], inertia_values_joint[1], inertia_values_joint[2]],
        #                     [inertia_values_joint[1], inertia_values_joint[3], inertia_values_joint[4]],
        #                     [inertia_values_joint[2], inertia_values_joint[4], inertia_values_joint[5]]])
    
        mI = mass*np.identity(3)
        zeros = np.zeros((3,3))
        Gi = np.c_[np.r_[rotated_I_CoM, zeros], np.r_[zeros,mI]]
        G_list.append(Gi)

    return T_ee, T_list, body_list, G_list