Exemplo n.º 1
0
def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
    marker = Marker()
    marker.header.frame_id = ar.link_tf_name(arm, 0)
    marker.header.stamp = time_stamp
    marker.ns = arm
    marker.type = Marker.ARROW
    marker.action = Marker.ADD
    marker.pose.position.x = cep[0, 0]
    marker.pose.position.y = cep[1, 0]
    marker.pose.position.z = cep[2, 0]
    marker.scale.x = 0.1
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    marker.lifetime = rospy.Duration()

    marker.id = marker_id * 100 + 0
    #rot1 = tr.Ry(math.radians(90.)) * rot.T
    rot1 = rot * tr.rotY(math.pi / 2)
    quat = tr.matrix_to_quaternion(rot1)
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]

    marker.color.r = c1[0]
    marker.color.g = c1[1]
    marker.color.b = c1[2]
    marker.color.a = 1.
    marker_pub.publish(marker)

    marker.id = marker_id * 100 + 1
    if arm == 'left_arm':
        #rot2 = tr.Rz(math.radians(90.)) * rot.T
        rot2 = rot * tr.rotZ(-math.pi / 2)
    else:
        #rot2 = tr.Rz(math.radians(-90.)) * rot.T
        rot2 = rot * tr.rotZ(math.pi / 2)

    quat = tr.matrix_to_quaternion(rot2)
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]

    marker.color.r = c2[0]
    marker.color.g = c2[1]
    marker.color.b = c2[2]
    marker.color.a = 1.
    marker_pub.publish(marker)
Exemplo n.º 2
0
def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
    marker = Marker()
    marker.header.frame_id = ar.link_tf_name(arm, 0)
    marker.header.stamp = time_stamp
    marker.ns = arm
    marker.type = Marker.ARROW
    marker.action = Marker.ADD
    marker.pose.position.x = cep[0,0]
    marker.pose.position.y = cep[1,0]
    marker.pose.position.z = cep[2,0]
    marker.scale.x = 0.1
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    marker.lifetime = rospy.Duration()

    marker.id = marker_id*100 + 0
    #rot1 = tr.Ry(math.radians(90.)) * rot.T
    rot1 = rot * tr.rotY(math.pi/2)
    quat = tr.matrix_to_quaternion(rot1)
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]

    marker.color.r = c1[0]
    marker.color.g = c1[1]
    marker.color.b = c1[2]
    marker.color.a = 1.
    marker_pub.publish(marker)

    marker.id = marker_id*100 + 1
    if arm == 'left_arm':
        #rot2 = tr.Rz(math.radians(90.)) * rot.T
        rot2 = rot * tr.rotZ(-math.pi/2)
    else:
        #rot2 = tr.Rz(math.radians(-90.)) * rot.T
        rot2 = rot * tr.rotZ(math.pi/2)

    quat = tr.matrix_to_quaternion(rot2)
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]

    marker.color.r = c2[0]
    marker.color.g = c2[1]
    marker.color.b = c2[2]
    marker.color.a = 1.
    marker_pub.publish(marker)
Exemplo n.º 3
0
    arms = ar.M3HrlRobot()
    arm_client = ac.MekaArmClient(arms)

    force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
    force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
    marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)

    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():
        rospy.sleep(0.1)
        f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
        f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp
        force_r_pub.publish(FloatArray(h, f_r))
        force_l_pub.publish(FloatArray(h, f_l))

        publish_sphere_marker((0.5, 0.5, 0.5), 0.08, torso_link_name,
                              time_stamp, 'both_arms', 0)

        for arm in [r_arm, l_arm]:
            q = arm_client.get_joint_angles(arm)
Exemplo n.º 4
0
    def step_ros(self):
        r_arm = 'right_arm'
        l_arm = 'left_arm'

        self.cb_lock.acquire()
        r_jep = copy.copy(self.r_jep)
        l_jep = copy.copy(self.l_jep)
        r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
        l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
        self.cb_lock.release()

        self.set_jep(r_arm, r_jep)
        self.set_jep(l_arm, l_jep)
        self.set_alpha(r_arm, r_alpha)
        self.set_alpha(l_arm, l_alpha)

        self.step()

        motor_pwr_state = self.is_motor_power_on()

        if not motor_pwr_state:
            self.maintain_configuration()

        q_r = self.get_joint_angles(r_arm)
        q_l = self.get_joint_angles(l_arm)

        if self.floating_arms:
            if self.qr_prev == None or self.floating_arms_counter < 100:
                self.qr_prev = q_r
                self.ql_prev = q_l
                self.floating_arms_counter += 1
            else:
                tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
                r_arr = np.array(q_r)
                l_arr = np.array(q_l)
                prev_r_arr = np.array(self.qr_prev)
                prev_l_arr = np.array(self.ql_prev)

                dqr = np.array(q_r) - np.array(prev_r_arr)
                dql = np.array(q_l) - np.array(prev_l_arr)
                dqr = dqr * (np.abs(dqr) > tol)
                dql = dql * (np.abs(dql) > tol)

                r_jep = (np.array(r_jep) + dqr).tolist()
                l_jep = (np.array(l_jep) + dql).tolist()

                self.cb_lock.acquire()
                self.r_jep = copy.copy(r_jep)
                self.l_jep = copy.copy(l_jep)
                self.cb_lock.release()

                change_idxs = np.where(dqr != 0)
                prev_r_arr[change_idxs] = r_arr[change_idxs]
                change_idxs = np.where(dql != 0)
                prev_l_arr[change_idxs] = l_arr[change_idxs]
                self.qr_prev = prev_r_arr.tolist()
                self.ql_prev = prev_l_arr.tolist()

        f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
        f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
        f_r = self.xhat_force[r_arm]
        f_l = self.xhat_force[l_arm]

        # publish stuff over ROS.
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp

        self.q_r_pub.publish(FloatArray(h, q_r))
        self.q_l_pub.publish(FloatArray(h, q_l))

        self.jep_r_pub.publish(FloatArray(h, r_jep))
        self.jep_l_pub.publish(FloatArray(h, l_jep))

        self.alph_r_pub.publish(FloatArray(h, r_alpha))
        self.alph_l_pub.publish(FloatArray(h, l_alpha))

        h.frame_id = '/torso_lift_link'
        nms = self.joint_names_list['right_arm'] + self.joint_names_list[
            'left_arm']
        pos = q_r + q_l
        self.joint_state_pub.publish(
            JointState(h, nms, pos, [0.] * len(pos), [0.] * len(pos)))

        h.frame_id = ar.link_tf_name(r_arm, 7)
        self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
        self.force_r_pub.publish(FloatArray(h, f_r))

        h.frame_id = ar.link_tf_name(l_arm, 7)
        self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
        self.force_l_pub.publish(FloatArray(h, f_l))

        self.pwr_state_pub.publish(Bool(motor_pwr_state))
Exemplo n.º 5
0
    arms = ar.M3HrlRobot()
    arm_client = ac.MekaArmClient(arms)

    force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
    force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
    marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)

    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():
        rospy.sleep(0.1)
        f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
        f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp
        force_r_pub.publish(FloatArray(h, f_r))
        force_l_pub.publish(FloatArray(h, f_l))

        publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
                              time_stamp, 'both_arms', 0)

        for arm in [r_arm, l_arm]:
            q = arm_client.get_joint_angles(arm)
Exemplo n.º 6
0
    def step_ros(self):
        r_arm = 'right_arm'
        l_arm = 'left_arm'

        self.cb_lock.acquire()
        r_jep = copy.copy(self.r_jep)
        l_jep = copy.copy(self.l_jep)
        r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
        l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
        self.cb_lock.release()

        self.set_jep(r_arm, r_jep)
        self.set_jep(l_arm, l_jep)
        self.set_alpha(r_arm, r_alpha)
        self.set_alpha(l_arm, l_alpha)

        self.step()

        motor_pwr_state = self.is_motor_power_on()

        if not motor_pwr_state:
            self.maintain_configuration()

        q_r = self.get_joint_angles(r_arm)
        q_l = self.get_joint_angles(l_arm)

        if self.floating_arms:
            if self.qr_prev == None or self.floating_arms_counter < 100:
                self.qr_prev = q_r
                self.ql_prev = q_l
                self.floating_arms_counter += 1
            else:
                tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
                r_arr = np.array(q_r)
                l_arr = np.array(q_l)
                prev_r_arr = np.array(self.qr_prev)
                prev_l_arr = np.array(self.ql_prev)

                dqr = np.array(q_r) - np.array(prev_r_arr)
                dql = np.array(q_l) - np.array(prev_l_arr)
                dqr = dqr * (np.abs(dqr) > tol)
                dql = dql * (np.abs(dql) > tol)
                
                r_jep = (np.array(r_jep) + dqr).tolist()
                l_jep = (np.array(l_jep) + dql).tolist()

                self.cb_lock.acquire()
                self.r_jep = copy.copy(r_jep)
                self.l_jep = copy.copy(l_jep)
                self.cb_lock.release()

                change_idxs = np.where(dqr != 0)
                prev_r_arr[change_idxs] = r_arr[change_idxs]
                change_idxs = np.where(dql != 0)
                prev_l_arr[change_idxs] = l_arr[change_idxs]
                self.qr_prev = prev_r_arr.tolist()
                self.ql_prev = prev_l_arr.tolist()

        f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
        f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
        f_r = self.xhat_force[r_arm]
        f_l = self.xhat_force[l_arm]

        # publish stuff over ROS.
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp

        self.q_r_pub.publish(FloatArray(h, q_r))
        self.q_l_pub.publish(FloatArray(h, q_l))

        self.jep_r_pub.publish(FloatArray(h, r_jep))
        self.jep_l_pub.publish(FloatArray(h, l_jep))

        self.alph_r_pub.publish(FloatArray(h, r_alpha))
        self.alph_l_pub.publish(FloatArray(h, l_alpha))

        h.frame_id = '/torso_lift_link'
        nms = self.joint_names_list['right_arm'] + self.joint_names_list['left_arm']
        pos = q_r + q_l
        self.joint_state_pub.publish(JointState(h, nms, pos,
                                    [0.]*len(pos), [0.]*len(pos)))

        h.frame_id = ar.link_tf_name(r_arm, 7)
        self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
        self.force_r_pub.publish(FloatArray(h, f_r))
        
        h.frame_id = ar.link_tf_name(l_arm, 7)
        self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
        self.force_l_pub.publish(FloatArray(h, f_l))

        self.pwr_state_pub.publish(Bool(motor_pwr_state))